加权粒子滤波（WPF）算法
加权粒子滤波（WPF）算法
1. 动机：标准 PF 的“AP 同权”假设在 NLOS 下不成立
标准粒子滤波在自然更新阶段，对每个 AP 的似然贡献是平等相乘的：
[
L^{(i)}
=
\prod_{j\in\mathcal{S}}
\mathcal{N}!\left(r_j^{(i)};\,0,\,\sigma_j^2\right)
]
这意味着：无论 AP 离目标远近、测距质量好坏，每个 AP 对粒子权重的“话语权”是一样的。
在理想视距（LOS）且各 AP 测距噪声相近时，这个假设没有问题。但在实际三维定位场景中，存在两个现实问题：
- 远端 AP 更容易受 NLOS 污染。AP 离目标越远，信号传播路径越可能被遮挡或反射，测距值出现系统性正偏差的概率越高；
- 远端 AP 的测距误差对定位的杠杆效应更大。同样的绝对测距误差，在几何构型上对远端 AP 约束方向的影响往往更分散，容易把粒子权重“拉偏”到错误区域。
标准 PF 对所有这些 AP 一视同仁，等价于让最不可靠的观测拥有和可靠观测相同的投票权。WPF 的出发点就是修正这一点：在自然更新阶段，按距离反比对每个 AP 的残差贡献加权，让远端 AP 自动降权。
2. 核心思想：距离反比加权（WAP）
WPF 继承标准 PF 的全部框架——2000 粒子均匀初始化、三阶段更新、系统重采样加微扰、两阶段 AP 剔除迭代——唯一的区别发生在第二阶段“自然更新”。
对每个 AP (j)，定义一个距离反比权重：
[
\mathrm{WAP}_j
=
\frac{1}{D_j+\epsilon}
]
其中：
- (D_j) 是第 (j) 个 AP 到目标的距离度量；
- (\epsilon>0) 是正则项（rectification term），防止 (D_j=0) 时除零，同时控制权重的动态范围。
工程上 (D_j) 有三种取法，按实现复杂度和精度排序：
| 取法 | 表达式 | 特点 | ||
|---|---|---|---|---|
| 测距值 | (D_j=d_j) | 最简单，所有粒子共享同一套 (\mathrm{WAP}_j)；NLOS 下 (d_j) 偏大，正好对应“远端 AP 降权” | ||
| 上轮估计距离 | (D_j=\ | \hat{\mathbf{x}}_{k-1}-\mathbf{a}_j\ | ) | 比测距值更稳，不受当前帧异常测距直接影响 |
| 粒子到 AP 距离 | (D_j^{(i)}=\ | \mathbf{x}_k^{(i)}-\mathbf{a}_j\ | ) | 最精细，但每个粒子对每个 AP 都要单独算权重，计算量最大 |
主推方案是测距值 (D_j=d_j)：它实现最简单，且在 NLOS 场景下具有明确的物理含义——测距值异常大的 AP，往往就是被 NLOS 污染的 AP，理应降权。
3. 加权似然：WPF 与 PF 的唯一公式差异
标准 PF 的似然是各 AP 高斯似然的乘积。WPF 把这个乘积改成带指数权重的乘积：
[
L^{(i)}
=
\prod_{j\in\mathcal{S}}
\left[
\mathcal{N}!\left(r_j^{(i)};\,0,\,\sigma_j^2\right)
\right]^{\widetilde{\mathrm{WAP}}_j}
]
等价地，在对数域写成更数值稳定的形式：
[
\log L^{(i)}
=
\sum_{j\in\mathcal{S}}
\widetilde{\mathrm{WAP}}_j
\cdot
\log
\mathcal{N}!\left(r_j^{(i)};\,0,\,\sigma_j^2\right)
]
其中 (\widetilde{\mathrm{WAP}}_j) 是归一化后的距离反比权重：
[
\widetilde{\mathrm{WAP}}j
=
\frac{\mathrm{WAP}_j}{\max{l\in\mathcal{S}}\mathrm{WAP}_l}
]
为什么要归一化？ 如果不归一化，当所有 AP 都很远时，(\mathrm{WAP}_j) 整体很小，对数似然被整体压缩，粒子权重更新过慢，滤波“反应迟钝”。归一化保证相对权重不变（近端 AP 权重大、远端 AP 权重小），但整体尺度合理，权重更新速度与标准 PF 可比。
3.1 加权似然的直观效果
- 对近端 AP：(\widetilde{\mathrm{WAP}}_j\approx 1)，其似然贡献与标准 PF 几乎相同；
- 对远端 AP：(\widetilde{\mathrm{WAP}}_j\ll 1)，其似然贡献被“压扁”——无论该 AP 的残差是大是小，它对粒子权重的拉动能力都被削弱。
这正是 WPF 想要的效果：远端 AP 仍然参与计算（不像硬剔除那样直接丢掉），但它的话语权被自动调低。
3.2 与“修改方差”的等价理解
从另一个角度看，指数加权近似等价于放大远端 AP 的方差：
[
\left[\mathcal{N}(r;0,\sigma^2)\right]^{w}
\propto
\mathcal{N}!\left(r;\,0,\,\frac{\sigma^2}{w}\right)
]
当 (w<1\) 时，等效方差 \(\sigma^2/w>\sigma^2)，即该 AP 的残差被允许更大而不受惩罚。这与“远端 AP 测距更不可信”的直觉完全一致。
4. WPF 与标准 PF 的完整对比
| 环节 | 标准 PF | WPF |
|---|---|---|
| 初始化 | 2000 粒子均匀分布 | 相同 |
| 第一阶段：预测 | 四种运动模型 + 边界处理 | 相同 |
| 第二阶段：自然更新 | 所有 AP 似然等权相乘 | 每个 AP 似然乘以距离反比指数权重 |
| 第三阶段：重采样 | ESS 触发系统重采样 + 微扰 | 相同 |
| 两阶段 AP 剔除 | 首段全 AP、二段剩余 AP | 相同（WPF 的加权作用在每一段内部） |
| 计算开销 | 基准 | 略高：每个 AP 多一次 (\mathrm{WAP}_j) 计算与一次幂运算；若 (D_j) 取粒子到 AP 距离，开销显著增加 |
一句话总结差异：PF 是“所有 AP 同权投票”，WPF 是“近端 AP 一票顶多票，远端 AP 一票算半票”。
5. WPF 与两阶段 AP 剔除的关系：软降权 vs 硬剔除
你已有的两阶段 AP 剔除是硬处理：
- 首段 PF 估计位置后，残差大于 8 m 的 AP 直接剔除；
- 二段只用剩余 AP 重新跑 PF。
WPF 是软处理：
- 所有 AP 始终参与，但远端 AP 的似然贡献被距离反比权重压低；
- 不直接丢弃任何 AP，避免“误杀”那些物理距离远但测距其实正常的 AP。
两者可以叠加使用，形成“软硬结合”的两级 NLOS 抑制：
- 第一级（软） ：WPF 的自然更新中，远端 AP 自动降权，粒子权重不会被远端异常测距剧烈拉偏；
- 第二级（硬） ：首段 WPF 给出更干净的估计后，再做 8 m 残差剔除，剔除决策比标准 PF 更可靠；
- 第三级（软） ：二段 WPF 在剩余 AP 上继续用距离反比加权，进一步抑制残余的远端偏差。
这个组合的逻辑是：先用软降权把估计拉稳，再用硬剔除清理明显坏点，最后再用软降权精化。软处理负责“防微杜渐”，硬处理负责“切除病灶”。
6. 参数选择与边界情况
6.1 正则项 (\epsilon) 的选择
[
\mathrm{WAP}_j=\frac{1}{d_j+\epsilon}
]
- (\epsilon) 太小：近端 AP 的 (\mathrm{WAP}_j) 可能非常大，归一化后远端 AP 权重趋近于 0，退化为“只信最近 AP”，几何构型被破坏；
- (\epsilon) 太大：所有 (\mathrm{WAP}_j) 趋于相等，WPF 退化为标准 PF，加权失效。
建议：(\epsilon) 取场景尺度的 5%–10%，或取典型测距噪声 (\sigma) 的若干倍。例如场景 50 m 见方、测距噪声 0.3 m，(\epsilon) 可取 2–5 m。
6.2 权重动态范围的限制
即使归一化后，仍建议对 (\widetilde{\mathrm{WAP}}_j) 设下限：
[
\widetilde{\mathrm{WAP}}j
\leftarrow
\max\left(\widetilde{\mathrm{WAP}}_j,\; w{\min}\right)
]
典型 (w_{\min}=0.05\sim 0.1)。原因是：如果某个 AP 的权重被压到接近 0，它实际上等于被静默剔除，而此时若近端 AP 数量不足，粒子集可能沿远端 AP 本可以约束的方向发散。
6.3 几何构型退化的风险
WPF 的加权逻辑是“距离越远越不信”。但如果所有 AP 都离目标很远（例如目标在场景边缘、AP 集中在另一侧），所有 (\mathrm{WAP}_j) 都很小，归一化后相对差异可能被放大，导致粒子集过度依赖个别近端 AP，几何构型变差。
缓解措施：
- 在 (\mathrm{WAP}_j) 中混入一个与距离无关的基准权重，例如 (\mathrm{WAP}_j=(1-\alpha)\cdot 1+\alpha\cdot\frac{1}{d_j+\epsilon})，(\alpha\in[0,1]) 控制距离加权的强度；
- 或对 (\widetilde{\mathrm{WAP}}_j) 设下限（见 6.2）。
6.4 与两阶段剔除的阈值配合
WPF 降权后，首段估计更稳，8 m 残差阈值的误判率会下降。但需要注意：WPF 不改变残差的计算方式，首段剔除仍然用 (\hat{\mathbf{x}}^{(1)}) 反算各 AP 残差。如果 WPF 把估计拉向近端 AP，远端 AP 的残差可能被“放大”，从而更容易触发剔除——这通常是期望的行为，但阈值仍需按实际场景标定。
6.5 计算开销
若 (D_j) 取测距值 (d_j)，每个 AP 每轮只多算一次倒数和一次幂运算，开销可忽略。若 (D_j) 取粒子到 AP 的距离，则每个粒子对每个 AP 都要算一次距离和权重，计算量约增加一个 (N\times|\mathcal{S}|) 的矩阵运算，在 2000 粒子、10 AP 的配置下仍可接受，但需注意实时性预算。
7. 完整伪代码（WPF 单段更新）
输入：粒子集 {x^(i), w^(i)}，AP 集合 S，测距 {d_j}，噪声 {σ_j}
正则项 ε，权重下限 w_min
输出：更新后的粒子集与权重
# 阶段 1：预测（与标准 PF 相同）
for i = 1..N:
按四种运动模型之一传播 x^(i)
边界处理：裁剪 / 反弹 / 中间扩散
# 阶段 2：自然更新（WPF 的核心差异）
for j in S:
WAP_j = 1 / (d_j + ε) # 距离反比权重
WAP_max = max_j WAP_j
for j in S:
WAP_j_norm = WAP_j / WAP_max # 归一化
WAP_j_norm = max(WAP_j_norm, w_min) # 下限保护
for i = 1..N:
logL = 0
for j in S:
r = d_j - ||x^(i) - a_j||
logL += WAP_j_norm * log( N(r; 0, σ_j^2) )
w^(i) = w^(i) * exp(logL)
归一化 w^(i)
# 阶段 3：系统重采样（与标准 PF 相同）
N_eff = 1 / Σ (w^(i))^2
if N_eff < N_th:
系统重采样，权重重置为 1/N
对每个粒子加微扰 ε_pert ~ N(0, σ_pert^2 I)
两阶段 AP 剔除迭代的外层流程与 PF 文档完全一致，只需把每一段内部的“自然更新”替换为上述 WPF 加权版本。
8. 总结
WPF 是对标准 PF 的一个最小侵入式改进：不改预测、不改重采样、不改两阶段剔除框架，只在自然更新阶段把“所有 AP 等权”改成“距离反比加权”。
它的设计逻辑可以概括为三句话：
- 远端 AP 更容易被 NLOS 污染，也更容易把粒子权重拉偏，所以应该降权；
- 降权用距离反比 (\mathrm{WAP}_j=1/(d_j+\epsilon)) 实现，简单、无需额外统计信息、物理含义明确；
- 降权是“软处理” ，与两阶段 AP 剔除的“硬处理”互补——软处理防止估计被逐渐拉偏，硬处理切除已经明显异常的观测。
代价是：WPF 引入了一个正则项 (\epsilon) 和一个权重下限 (w_{\min}) 需要标定；在 AP 几何构型本身不佳时，距离反比加权可能加剧对近端 AP 的过度依赖。工程上建议先跑标准 PF + 两阶段剔除作为基线，再加入 WPF 软降权，对比 RMSE 与 ESS 曲线，确认精度增益值得额外的参数调校成本。