基于粒子滤波与两阶段AP剔除的三维定位算法

1. 算法总览

本算法面向三维空间中的目标定位问题，核心设计可以概括为：

2000 个粒子均匀铺满场景，每 T 个时刻执行一轮三阶段更新（预测 → 自然更新 → 系统重采样），并在每轮更新中嵌套两阶段 AP 剔除迭代：首段用全部 AP 跑 PF 做残差剔除，剔除残差大于 8 m 的 AP 且保证剩余 AP 不少于 3 个；二段用剩余 AP 再跑一次 PF 给出最终定位结果。

整体流程分为三层：

• 粒子层：2000 个粒子在场景内均匀初始化，承载位置假设与权重；
• 时间层：每 T 个时刻触发一轮三阶段更新，阶段之间顺序执行；
• AP 层：每轮更新内部执行两阶段迭代，第一阶段负责“筛 AP”，第二阶段负责“精定位”。

---

2. 初始化

在场景三维边界 $[x_{\min},x_{\max}]\times[y_{\min},y_{\max}]\times[z_{\min},z_{\max}]$ 内均匀撒 2000 个粒子：

$$\mathbf{x}_0^{(i)} \sim \mathcal{U}(\Omega),\quad i=1,\dots,N,\quad N=2000$$

初始权重全部相等：

$$\omega_0^{(i)}=\frac{1}{N}$$

设计意图：初始时刻没有任何先验信息，均匀分布是“最大熵”的选择，保证粒子集覆盖整个可能区域，避免一开始就把真实位置排除在粒子支撑集之外。

---

3. 三阶段更新（每 T 个时刻执行一轮）

3.1 第一阶段：预测（四种运动模型 + 边界处理）

对每个粒子，从四种运动模型中选择一种进行状态传播。四种模型的典型形式为：

模型	状态转移	适用场景
静止模型	$\mathbf{x}_k^{(i)}=\mathbf{x}_{k-1}^{(i)}+\mathbf{w}_k$	目标基本不动，仅保留微小过程噪声
匀速模型	$\mathbf{x}_k^{(i)}=\mathbf{x}_{k-1}^{(i)}+\mathbf{v}_{k-1}^{(i)}T+\mathbf{w}_k$	目标匀速运动
匀加速模型	$\mathbf{x}_k^{(i)}=\mathbf{x}_{k-1}^{(i)}+\mathbf{v}_{k-1}^{(i)}T+\frac{1}{2}\mathbf{a}_{k-1}^{(i)}T^2+\mathbf{w}_k$	目标存在持续加速度
随机游走模型	$\mathbf{x}_k^{(i)}=\mathbf{x}_{k-1}^{(i)}+\boldsymbol{\eta}_k$	目标运动无明确规律，用较大扩散探索

其中 $\mathbf{w}_k$、$\boldsymbol{\eta}_k$ 为过程噪声。四种模型可以按固定比例分配粒子（例如各 500 个），也可以根据上一轮各模型的似然表现动态调整各模型粒子数。

预测后粒子可能越出场景边界，采用以下三种边界处理策略（可按粒子或按模型配置）：

• 自由裁剪（Clamp）：越界分量直接截断到边界值，$\mathbf{x}\leftarrow\min(\max(\mathbf{x},\mathbf{x}_{\min}),\mathbf{x}_{\max})$；
• 反弹（Reflect）：越界分量关于边界镜像反射，保持粒子在边界附近的分布连续性；
• 中间扩散（Center Diffusion）：越界粒子按一定概率向场景中心方向重新扩散，避免粒子长期贴在边界上。

设计意图：多运动模型并行覆盖“静止—匀速—加速—无规律”四类目标行为，边界处理防止粒子逃逸导致有效覆盖区域收缩。

---

3.2 第二阶段：自然更新（高斯似然计算权重）

对每个粒子，用当前全部参与 AP 的测距残差计算高斯似然权重。

设参与更新的 AP 集合为 $\mathcal{S}$，第 $j$ 个 AP 坐标为 $\mathbf{a}_j$，测距为 $d_j$，第 $i$ 个粒子的预测位置为 $\mathbf{x}_k^{(i)}$，则残差为：

$$r_j^{(i)}=d_j-\|\mathbf{x}_k^{(i)}-\mathbf{a}_j\|$$

高斯似然：

$$p(\mathbf{y}_k\mid\mathbf{x}_k^{(i)}) = \prod_{j\in\mathcal{S}} \frac{1}{\sqrt{2\pi}\sigma_j} \exp\left( -\frac{(r_j^{(i)})^2}{2\sigma_j^2} \right)$$

权重更新：

$$\omega_k^{(i)} = \omega_{k-1}^{(i)} \, p(\mathbf{y}_k\mid\mathbf{x}_k^{(i)})$$

归一化：

$$\tilde{\omega}_k^{(i)} = \frac{\omega_k^{(i)}}{\sum_{l=1}^{N}\omega_k^{(l)}}$$

设计意图：残差越小，粒子越能解释当前观测，权重越大；$\sigma_j$ 体现不同 AP 的测距可信度，为后续加权和 AP 剔除提供依据。

---

3.3 第三阶段：系统重采样（ESS 触发 + 微扰防退化）

计算有效粒子数：

$$N_{\text{eff}} = \frac{1}{\sum_{i=1}^{N}(\tilde{\omega}_k^{(i)})^2}$$

当 $N_{\text{eff}}<N_{\text{th}}$ 时触发系统重采样：

1. 生成一个均匀随机数 $u\sim\mathcal{U}(0,1/N)$；
2. 以等间隔 $1/N$ 生成 $N$ 个采样点 $u_m=u+(m-1)/N$；
3. 按累积权重定位每个采样点对应的粒子，复制该粒子；
4. 重采样后权重重置为 $1/N$。

重采样后对每个粒子施加微扰：

$$\mathbf{x}_k^{(i)} \leftarrow \mathbf{x}_k^{(i)}+\boldsymbol{\epsilon}^{(i)}, \quad \boldsymbol{\epsilon}^{(i)}\sim\mathcal{N}(\mathbf{0},\sigma_{\text{pert}}^2\mathbf{I})$$

设计意图：系统重采样方差低、实现简单；微扰用于打散被重复复制的粒子，缓解粒子贫化，防止粒子集在几步内塌缩到少数点上。

---

4. 两阶段 AP 剔除迭代（嵌套在每轮更新中）

每轮三阶段更新内部，执行两阶段迭代：

4.1 首段：全 AP 参与，残差剔除

Step 1：用全部 AP 集合 $\mathcal{S}_{\text{all}}$ 执行一轮完整的三阶段粒子滤波（预测 → 自然更新 → 系统重采样），得到首段位置估计：

$$\hat{\mathbf{x}}^{(1)} = \sum_{i=1}^{N} \tilde{\omega}_k^{(i)} \mathbf{x}_k^{(i)}$$

Step 2：用首段估计 $\hat{\mathbf{x}}^{(1)}$ 计算每个 AP 的残差：

$$e_j = \left| d_j-\|\hat{\mathbf{x}}^{(1)}-\mathbf{a}_j\| \right|, \quad j\in\mathcal{S}_{\text{all}}$$

Step 3：剔除残差大于 8 m 的 AP：

$$\mathcal{S}_{\text{keep}} = \{j\in\mathcal{S}_{\text{all}} : e_j\le 8\ \text{m}\}$$

Step 4：数量保护——若 $|\mathcal{S}_{\text{keep}}|<3$，则按残差从小到大排序，保留残差最小的 3 个 AP：

$$\mathcal{S}_{\text{keep}} \leftarrow \text{残差最小的 }3\text{ 个 AP}$$

设计意图：首段 PF 给出一个鲁棒的位置估计，用它反算各 AP 残差；残差大于 8 m 的 AP 大概率受到 NLOS、多径或粗差污染，予以剔除；强制保留至少 3 个 AP 保证三维定位的可解性（三维至少需要 3 个距离约束加上合理的几何构型，实际中通常希望不少于 4 个，这里 3 个是最低保护线）。

4.2 二段：剩余 AP 再跑一次 PF

Step 1：以首段结束时的粒子集作为二段的初始粒子集（复用粒子位置与权重，避免重新均匀初始化带来的信息损失）。

Step 2：用剩余 AP 集合 $\mathcal{S}_{\text{keep}}$ 重新执行一轮三阶段更新：

• 预测：沿用四种运动模型；
• 自然更新：似然只对 $\mathcal{S}_{\text{keep}}$ 中的 AP 计算，权重重新归一化；
• 系统重采样：同样按 ESS 阈值触发，重采样后加微扰。

Step 3：输出最终定位结果：

$$\hat{\mathbf{x}}^{(2)} = \sum_{i=1}^{N} \tilde{\omega}_k^{(i)} \mathbf{x}_k^{(i)}$$

设计意图：剔除坏 AP 后，似然函数不再被异常测距拉偏，二段 PF 在“干净”的观测集上做精定位。复用首段粒子集而不是重新初始化，是因为首段粒子集已经向真实位置附近收敛，二段在此基础上继续细化，收敛更快、更稳。

---

5. 完整伪代码

输入：场景边界 Ω，AP 集合 S_all，测距 {d_j}，粒子数 N=2000，更新周期 T
输出：每 T 个时刻的位置估计 x_hat

初始化：
  在 Ω 内均匀采样 N 个粒子 {x^(i)}，权重 w^(i) = 1/N

for 每个时间步 k = 1,2,...:
  if k mod T != 0:
    继续收集观测，不更新
    continue

  # ===== 首段：全 AP 粒子滤波 =====
  # 阶段 1：预测
  for i = 1..N:
    按四种运动模型之一传播 x^(i)
    对越界分量执行 裁剪 / 反弹 / 中间扩散

  # 阶段 2：自然更新（全部 AP）
  for i = 1..N:
    计算残差 r_j^(i) = d_j - ||x^(i) - a_j||,  j ∈ S_all
    似然 L^(i) = Π_j N(r_j^(i); 0, σ_j^2)
    w^(i) = w^(i) * L^(i)
  归一化 w^(i)

  # 阶段 3：系统重采样
  N_eff = 1 / Σ (w^(i))^2
  if N_eff < N_th:
    系统重采样，权重重置为 1/N
    对每个粒子加微扰 ε ~ N(0, σ_pert^2 I)

  # 首段估计
  x_hat_1 = Σ w^(i) * x^(i)

  # ===== AP 残差剔除 =====
  for j in S_all:
    e_j = |d_j - ||x_hat_1 - a_j|||
  S_keep = { j ∈ S_all : e_j <= 8m }
  if |S_keep| < 3:
    S_keep = 残差最小的 3 个 AP

  # ===== 二段：剩余 AP 粒子滤波 =====
  # 复用首段粒子集 {x^(i), w^(i)} 作为初始
  # 阶段 1：预测（同上）
  for i = 1..N:
    按四种运动模型之一传播 x^(i)
    边界处理

  # 阶段 2：自然更新（仅 S_keep）
  for i = 1..N:
    计算残差 r_j^(i) = d_j - ||x^(i) - a_j||,  j ∈ S_keep
    似然 L^(i) = Π_{j∈S_keep} N(r_j^(i); 0, σ_j^2)
    w^(i) = w^(i) * L^(i)
  归一化 w^(i)

  # 阶段 3：系统重采样
  N_eff = 1 / Σ (w^(i))^2
  if N_eff < N_th:
    系统重采样，权重重置为 1/N
    对每个粒子加微扰 ε ~ N(0, σ_pert^2 I)

  # 最终输出
  x_hat = Σ w^(i) * x^(i)
  输出 x_hat

---

6. 参数配置表

参数	符号	建议取值	说明
粒子数	$N$	2000	场景越大、精度要求越高，可适当增加
更新周期	$T$	按系统帧率与运动速度确定	每 T 个时刻执行一轮完整更新
运动模型数量	—	4	静止、匀速、匀加速、随机游走
重采样阈值	$N_{\text{th}}$	常用 $2N/3$	ESS 低于该值触发重采样
微扰标准差	$\sigma_{\text{pert}}$	场景尺度的 0.5%–2%	过大会破坏收敛，过小防不住贫化
AP 残差阈值	—	8 m	根据测距噪声水平与场景尺度调整
最少保留 AP 数	—	3	三维定位的最低可解数量保护
各 AP 测距噪声	$\sigma_j$	由标定或在线估计	体现不同 AP 的可信度

---

7. 设计要点与边界情况

7.1 两段之间粒子集复用的合理性

二段复用首段粒子集，而不是重新均匀初始化，原因有三：

1. 首段已经用全部 AP 把粒子集向真实位置附近拉近，二段在此基础上细化，收敛更快；
2. 若重新均匀初始化，二段相当于从零开始，首段的计算被浪费；
3. 复用粒子集可以让两段共享同一套运动模型传播，保持时间连续性。

需要注意：二段更新时权重需要基于 $\mathcal{S}_{\text{keep}}$ 重新计算，不能直接沿用首段权重，因为似然所依赖的 AP 集合已经改变。

7.2 剔除 AP 后的可解性保护

三维定位至少需要 3 个距离约束才能在几何上约束住位置（实际中通常希望 4 个以上以保证几何构型良好）。算法强制 $|\mathcal{S}_{\text{keep}}|\ge 3$，是为了防止剔除过狠导致二段无法定位。

如果剩余 AP 只有 3 个且几何构型退化（例如三个 AP 近似共线），二段 PF 的粒子集可能沿某个方向发散，此时应：

• 放宽残差阈值，让更多 AP 参与；
• 或直接输出首段估计作为降级结果；
• 或在权重中引入几何构型惩罚项。

7.3 残差阈值的敏感性

8 m 的阈值是一个工程取值，它的合理性取决于：

• 测距噪声水平 $\sigma_j$：阈值应显著大于正常噪声（例如 $3\sigma$ 到 $5\sigma$）；
• 场景尺度：大场景可以适当放宽；
• NLOS 偏差的典型幅度：阈值应低于典型 NLOS 偏差，才能起到剔除作用。

实际部署时建议根据在线残差分布自适应调整阈值，而不是固定 8 m。

7.4 微扰与重采样的配合

微扰在重采样之后施加，目的是恢复多样性。需要注意：

• 微扰方差应与过程噪声同量级或略小，过大会把已经收敛的粒子重新打散；
• 如果连续多轮触发重采样且微扰不足，粒子集仍会贫化，表现为 ESS 持续偏低、估计方差偏大；
• 可以在微扰后重新计算 ESS，若仍低于阈值，说明粒子集多样性恢复不足，需要考虑增大微扰或引入 MCMC 移动步骤。

7.5 两段迭代的计算开销

每轮更新实际执行了两次完整的三阶段 PF，计算量约为单次 PF 的两倍。在 2000 粒子、三维状态的配置下，这是可接受的；但如果场景中 AP 数量很多、更新频率很高，需要考虑：

• 首段使用较少粒子做粗筛，二段恢复 2000 粒子做精定位；
• 或首段只做一轮预测-更新不重采样，用加权残差做 AP 筛选，二段再完整跑 PF。

---

8. 总结

本算法的设计逻辑可以概括为：

1. 用 2000 粒子均匀覆盖场景，保证初始无信息时的不确定性被完整表达；
2. 用四种运动模型并行预测，覆盖静止到机动的多种目标行为；
3. 用高斯似然自然更新权重，让能解释观测的粒子获得更高可信度；
4. 用 ESS 触发的系统重采样 + 微扰，在抑制权重退化的同时防止粒子贫化；
5. 用两阶段 AP 剔除迭代，首段全 AP 粗定位并筛掉残差大于 8 m 的坏 AP，二段用剩余 AP 精定位，并用“至少保留 3 个 AP”保护三维可解性。

整体上，这是一个“粒子层负责表达不确定性、AP 层负责清理观测异常、两段迭代负责粗筛到精化”的级联式三维定位框架。