粒子滤波与加权粒子滤波：权重层次、似然计算、HDOP与数据驱动噪声建模

1. PF 的核心计算：残差、似然与粒子权重

1.1 PF 对每个粒子计算什么

对第 $i$ 个粒子 $\mathbf{x}^{(i)}$，PF 在自然更新阶段依次计算三样东西。

（1）对每个 AP 的残差（每个粒子 × 每个 AP 一个标量）：

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

它衡量“如果目标真在这个粒子位置上，第 $j$ 个 AP 的测距应该和实测差多少”。

（2）该粒子的似然（每个粒子一个标量）：

$$L^{(i)} = \prod_{j\in\mathcal{S}} \mathcal{N}\left(r_j^{(i)};0,\sigma_j^2\right)$$

它衡量“这个粒子位置对所有 AP 的测距的综合解释能力”。残差越小、越接近 0，似然越大。

（3）该粒子的权重（每个粒子一个标量）：

$$\omega^{(i)}\leftarrow \omega^{(i)}\cdot L^{(i)},\qquad \tilde{\omega}^{(i)} = \frac{\omega^{(i)}}{\sum_l \omega^{(l)}}$$

所以，PF 对每个粒子最终计算并维护的，是一个标量权重 $\tilde{\omega}^{(i)}$，它表示“这个粒子假设的位置有多可信”。重采样按这个权重复制/淘汰粒子，最终输出位置是这个权重的加权平均：

$$\hat{\mathbf{x}} = \sum_i \tilde{\omega}^{(i)}\mathbf{x}^{(i)}$$

1.2 似然到底是什么

似然不是“概率”，而是“反过来的概率”——给定已经发生的数据，去评价某个假设有多合理。

1.2.1 概率和似然的区别

这两个东西数学公式长得一模一样，但看的方向相反。

概率：已知模型/参数，问“什么样的数据会出现，各有多大可能”。

比如：已知一枚硬币正面朝上的概率是 0.5，问“抛 10 次出现 7 次正面”的概率是多少。

似然：已知数据，问“哪个参数/假设最能解释这组数据”。

比如：抛了 10 次，出现 7 次正面，问“这枚硬币正面朝上的概率是 0.5 更合理，还是 0.7 更合理”。

公式还是那个公式，但谁固定、谁变化反过来了：

• 概率：参数固定，数据变化；
• 似然：数据固定，参数变化。

1.2.2 一个生活化的例子

你听到窗外有“喵”的一声（数据）。现在有两个假设：

• 假设 A：外面是一只猫；
• 假设 B：外面是一只狗。

你问自己：“如果外面真是猫，发出‘喵’的可能性有多大？” 很大。“如果外面真是狗，发出‘喵’的可能性有多大？” 很小。

于是你说：“猫这个假设的似然更高。”

注意：你并没有真的看到猫。你只是用听到的声音（数据），去反推哪个假设更能解释这个数据。似然就是“假设对数据的解释能力”。

1.2.3 回到 PF 里

PF 里：

• 数据是实测距离 $d_j$（设备测出来的，固定不变）；
• 假设是粒子位置 $\mathbf{x}^{(i)}$（每个粒子就是一个候选假设）。

对第 $i$ 个粒子，似然问的是：

“如果目标真的在这个粒子位置上，那么测出 $d_j$ 这个距离值的可能性有多大？”

• 如果粒子离 AP 的预测距离和实测距离很接近 → 这个假设能很好地解释数据 → 似然大；
• 如果预测距离和实测距离差很远 → 这个假设解释不了数据 → 似然小。

所以 PF 的自然更新，本质上就是：拿每个粒子假设去“解释”当前这组测距数据，谁解释得好，谁的似然大，谁的权重就高。

1.2.4 为什么叫“似然”而不叫“概率”

因为严格来说，粒子位置 $\mathbf{x}^{(i)}$ 不是一个随机变量，它是一个假设。我们不能说“目标是这个位置的概率是 0.3”——目标只有一个真实位置，不是随机分布的。但我们可以说“这个位置假设对数据的似然是 0.3”。

“似然”这个词就是用来回避“概率”这个词的：它衡量的是假设的合理性，不是假设发生的概率。

似然 = 给定已经观测到的数据，某个假设能多好地解释这组数据。 公式和概率一样，但方向反过来：概率是“参数定、数据变”，似然是“数据定、假设变”。PF 里数据是测距值，假设是粒子位置，似然大的粒子就是“能解释当前测距”的粒子。

1.3 单个 AP 的似然：公式拆解

假设目标真实位置在 $\mathbf{x}$，第 $j$ 个 AP 到目标的真实距离是：

$$R_j=\|\mathbf{x}-\mathbf{a}_j\|$$

但测距设备有噪声，实测值 $d_j$ 不会刚好等于 $R_j$，而是：

$$d_j = R_j + \text{噪声}$$

假设噪声服从零均值高斯分布，标准差为 $\sigma_j$。那么给定目标在 $\mathbf{x}$，测距值 $d_j$ 出现的概率密度是：

$$p(d_j\mid \mathbf{x}) = \frac{1}{\sqrt{2\pi}\sigma_j} \exp\left( -\frac{(d_j-\|\mathbf{x}-\mathbf{a}_j\|)^2}{2\sigma_j^2} \right)$$

这个式子就是单个 AP 的似然。把它拆成三块看：

$$p(d_j\mid \mathbf{x}) = \underbrace{\frac{1}{\sqrt{2\pi}\sigma_j}}_{\text{归一化常数}} \cdot \exp\left( -\frac{ \overbrace{(d_j-\|\mathbf{x}-\mathbf{a}_j\|)^2}^{\text{残差平方}} }{ \underbrace{2\sigma_j^2}_{\text{噪声方差的两倍}} } \right)$$

第一块：残差

$$r_j = d_j-\|\mathbf{x}-\mathbf{a}_j\|$$

• $d_j$ 是实测距离；
• $\|\mathbf{x}-\mathbf{a}_j\|$ 是“如果目标在粒子 $\mathbf{x}$ 处，到 AP 的理论距离”；
• 两者相减，就是这个粒子对这个 AP 的测距解释误差。

第二块：残差平方除以 $2\sigma^2$

• 残差越小，这一项越接近 0；
• 残差越大，这一项越大，指数里的负号让它把整个值压小；
• $\sigma_j$ 越大（这个 AP 越不准），同样的残差被惩罚得越轻。

第三块：指数函数

• 残差 $=0$ 时，$\exp(0)=1$，似然达到最大值；
• 残差变大，似然迅速下降，下降速度由 $\sigma_j$ 决定。

一句话：单个 AP 的似然就是一个以“理论距离”为中心、以 $\sigma_j$ 为宽度的钟形曲线，实测距离落在中心附近，似然大；偏离越远，似然越小。

1.4 “以理论距离为中心”是什么意思

不是“距离 AP 远的点”。这里的关键是：钟形曲线的横轴是“距离数值”，不是“空间位置”。

单个 AP 的似然函数是：

$$p(d_j\mid \mathbf{x}^{(i)}) = \frac{1}{\sqrt{2\pi}\sigma_j} \exp\left( -\frac{(d_j-\|\mathbf{x}^{(i)}-\mathbf{a}_j\|)^2}{2\sigma_j^2} \right)$$

把它看成关于实测距离 $d_j$ 的函数，横轴是“距离值”（单位：米），纵轴是“似然值”。

这条曲线的中心（峰值位置）在：

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

右边这个量就是“理论距离”——它是一个标量数值，不是空间中的一个点。它表示：如果目标真的在粒子 $\mathbf{x}^{(i)}$ 这个位置，它到第 $j$ 个 AP 的距离应该是多少米。

所以“以理论距离为中心”的意思是：

这条钟形曲线的峰值，落在“理论距离”这个数值上。

举例：粒子在 $(5,0,0)$，AP 在 $(0,0,0)$，理论距离 $=5$ m。那么这条钟形曲线的中心就在横轴的 $5$ 这个刻度上。实测距离 $d_j=5$ 时，似然最大；实测 $d_j=7$ 时，偏离中心 $2$ m，似然下降；实测 $d_j=3$ 时，也偏离 $2$ m，似然同样下降。

它说的是“实测距离这个数值离理论距离这个数值有多远”，不是“空间中的点离 AP 有多远”。

换一个粒子，中心就移动：

• 粒子 A 在 $(1,0,0)$，到 AP1 的理论距离 $=1$，钟形曲线中心在 $1$；
• 粒子 B 在 $(5,0,0)$，到 AP1 的理论距离 $=5$，钟形曲线中心在 $5$；
• 粒子 C 在 $(9,0,0)$，到 AP1 的理论距离 $=9$，钟形曲线中心在 $9$。

如果实测距离是 $d_1=5$：

• 粒子 A 的曲线中心在 1，实测 5 偏离中心 4，似然极小；
• 粒子 B 的曲线中心在 5，实测 5 正好落在中心，似然最大；
• 粒子 C 的曲线中心在 9，实测 5 偏离中心 4，似然极小。

“以理论距离为中心”= 每个粒子根据自己的位置，画一条中心在自己“预测距离”上的钟形曲线；实测距离落在谁的曲线中心附近，谁的似然就大。

“以理论距离为中心”不是“空间上离 AP 远的点”，而是“钟形曲线的横轴中心落在‘粒子到 AP 的预测距离’这个数值上”。 横轴是距离数值，不是空间位置；中心随粒子位置移动，实测距离越接近这个中心，该粒子的似然越大。

1.5 多个 AP 为什么连乘

PF 的自然更新里写的是：

$$L^{(i)} = \prod_{j\in\mathcal{S}} p(d_j\mid \mathbf{x}^{(i)})$$

这是假设各 AP 的测距噪声相互独立。独立事件的联合概率等于各自概率的乘积，所以把每个 AP 的似然乘起来，得到：

如果目标在粒子 $\mathbf{x}^{(i)}$ 处，那么“同时观测到这一组测距值 $d_1,d_2,\dots,d_{|\mathcal{S}|}$”的联合概率密度。

这个乘积 $L^{(i)}$ 就是粒子的似然。它综合了所有 AP 的信息：一个粒子要对所有 AP 的测距都解释得好，乘积才大；只要有一个 AP 的残差特别大，乘积就会被拉得很小。

1.6 一个具体数字，完整算一遍

设场景里有两个 AP：

• AP1 在 $(0,0,0)$，$\sigma_1=0.5$ m，实测 $d_1=5.0$ m；
• AP2 在 $(10,0,0)$，$\sigma_2=0.5$ m，实测 $d_2=6.0$ m。

有一个粒子 $\mathbf{x}^{(i)}=(1,0,0)$。算它对两个 AP 的似然。

AP1：

理论距离：

$$\|\mathbf{x}^{(i)}-\mathbf{a}_1\| = \|(1,0,0)-(0,0,0)\| = 1.0\text{ m}$$

残差：

$$r_1=5.0-1.0=4.0\text{ m}$$

似然：

$$p(d_1\mid\mathbf{x}^{(i)}) = \frac{1}{\sqrt{2\pi}\cdot 0.5} \exp\left(-\frac{4.0^2}{2\cdot 0.5^2}\right) = 0.798\cdot \exp(-32) \approx 1.0\times 10^{-14}$$

AP2：

理论距离：

$$\|\mathbf{x}^{(i)}-\mathbf{a}_2\| = \|(1,0,0)-(10,0,0)\| = 9.0\text{ m}$$

残差：

$$r_2=6.0-9.0=-3.0\text{ m}$$

似然：

$$p(d_2\mid\mathbf{x}^{(i)}) = 0.798\cdot \exp\left(-\frac{(-3.0)^2}{2\cdot 0.5^2}\right) = 0.798\cdot \exp(-18) \approx 1.2\times 10^{-8}$$

联合似然：

$$L^{(i)} = p(d_1\mid\mathbf{x}^{(i)})\cdot p(d_2\mid\mathbf{x}^{(i)}) \approx 1.0\times 10^{-14}\times 1.2\times 10^{-8} \approx 1.2\times 10^{-22}$$

这个数字非常小，说明粒子 $(1,0,0)$ 对两个 AP 的测距都解释得很差。

换一个粒子 $\mathbf{x}^{(k)}=(5,0,0)$：

• 到 AP1 理论距离 5.0，残差 $5.0-5.0=0$，似然 $=0.798$；
• 到 AP2 理论距离 5.0，残差 $6.0-5.0=1.0$，似然 $=0.798\cdot\exp(-2)\approx 0.108$；
• 联合似然 $=0.798\times 0.108\approx 0.086$。

对比：粒子 $(5,0,0)$ 的似然 $0.086$ 比粒子 $(1,0,0)$ 的 $1.2\times 10^{-22}$ 大了 20 多个数量级。所以 $(5,0,0)$ 的权重会远大于 $(1,0,0)$，重采样时 $(5,0,0)$ 附近的粒子会被大量复制。

1.7 为什么实际代码里用对数

联合似然是很多个小于 1 的数相乘，AP 一多，结果会小到浮点数下溢变成 0。所以工程上取对数，把连乘变成连加：

$$\log L^{(i)} = \sum_{j\in\mathcal{S}} \log p(d_j\mid\mathbf{x}^{(i)})$$

单个 AP 的对数似然：

$$\log p(d_j\mid\mathbf{x}^{(i)}) = -\log(\sqrt{2\pi}\sigma_j) - \frac{(d_j-\|\mathbf{x}^{(i)}-\mathbf{a}_j\|)^2}{2\sigma_j^2}$$

粒子权重更新在对数域做：

$$\log\omega^{(i)} \leftarrow \log\omega^{(i)} + \log L^{(i)}$$

最后归一化时，先减去所有粒子对数权重的最大值（防止指数溢出），再取指数：

$$\tilde{\omega}^{(i)} = \frac{ \exp(\log\omega^{(i)}-\max_l\log\omega^{(l)}) }{ \sum_m \exp(\log\omega^{(m)}-\max_l\log\omega^{(l)}) }$$

这就是代码里常见的“log-sum-exp”技巧。数学上和直接连乘完全等价，只是数值上不会下溢。

1.8 残差里没有真值：为什么还要撒粒子

1.8.1 残差公式里没有真值

回看残差定义：

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

这里三个量分别是：

符号	是什么	我们知道吗
$d_j$	第 $j$ 个 AP 的实测距离	知道，设备测出来的
$\mathbf{a}_j$	第 $j$ 个 AP 的已知坐标	知道，部署时标定的
$\mathbf{x}^{(i)}$	第 $i$ 个粒子假设的位置	知道，是我们自己撒的点
$\|\mathbf{x}^{(i)}-\mathbf{a}_j\|$	如果目标在这个粒子位置，到 AP 的距离应该是多少	可以算，因为粒子位置是我们设的

真值 $\mathbf{x}_{\text{true}}$ 从头到尾没有出现在这个公式里。 我们不知道真值，所以没法算“真实距离”。我们算的是“粒子假设位置下的理论距离”，然后拿它和实测距离比较。

1.8.2 用猜谜游戏类比

假设目标藏在房间里，你手里有一把测距仪，能测出目标到几个固定点的距离，但你不知道目标在哪。

你的做法是：在房间里撒 2000 个猜测点（粒子），每个点都代表一个“如果目标在这里”的假设。对每个猜测点，你问：

“如果目标真的在这个点上，那它到 AP1 的距离应该是多少？到 AP2 呢？……这些‘应该的距离’和我手里测距仪的读数差多少？”

• 某个猜测点的“应该距离”和实测距离很接近 → 这个猜测很合理，给它高权重；
• 某个猜测点的“应该距离”和实测距离差很远 → 这个猜测很离谱，给它低权重。

你不需要知道真值，你只需要能对每个猜测点算“预测距离”，再和实测比较。这正是 PF 在做的事。

1.8.3 一个具体例子把“真值”和“粒子”分开

设真实目标在 $\mathbf{x}_{\text{true}}=(5,0,0)$，但我们不知道。两个 AP：

• AP1 在 $(0,0,0)$，实测 $d_1=5.0$；
• AP2 在 $(10,0,0)$，实测 $d_2=5.0$。

撒三个粒子：

• 粒子 A：$\mathbf{x}^{(A)}=(1,0,0)$
• 粒子 B：$\mathbf{x}^{(B)}=(5,0,0)$
• 粒子 C：$\mathbf{x}^{(C)}=(9,0,0)$

对每个粒子算残差和似然（$\sigma=0.5$）：

粒子 A：

• 到 AP1 的“预测距离” $=\|(1,0,0)-(0,0,0)\|=1.0$，残差 $r_1=5.0-1.0=4.0$；
• 到 AP2 的“预测距离” $=\|(1,0,0)-(10,0,0)\|=9.0$，残差 $r_2=5.0-9.0=-4.0$；
• 两个残差都很大 → 似然极小。

粒子 B：

• 到 AP1 的“预测距离” $=\|(5,0,0)-(0,0,0)\|=5.0$，残差 $r_1=5.0-5.0=0$；
• 到 AP2 的“预测距离” $=\|(5,0,0)-(10,0,0)\|=5.0$，残差 $r_2=5.0-5.0=0$；
• 两个残差都为零 → 似然最大。

粒子 C：

• 到 AP1 的“预测距离” $=9.0$，残差 $r_1=-4.0$；
• 到 AP2 的“预测距离” $=1.0$，残差 $r_2=4.0$；
• 两个残差都很大 → 似然极小。

结果：粒子 B 的权重远大于 A 和 C。重采样后，B 附近会被大量复制，A 和 C 被淘汰。

注意：粒子 B 恰好和真值重合，但我们并不是因为“知道真值是 $(5,0,0)$”才给它高权重。我们是因为它的预测距离和实测距离完全吻合才给它高权重。如果真值在 $(6,0,0)$，那么粒子 B 的残差就不会是 0，而 $(6,0,0)$ 附近的粒子会胜出——是观测 $d_1,d_2$ 在筛选粒子，不是真值在筛选粒子。

1.8.4 为什么不能直接“解方程”求出真值

在无噪声、AP 数量刚好、几何构型良好时，确实可以直接解。但真实场景中：

1. 测距有噪声，两个球面方程的交点不是一个点，而是一片模糊区域；
2. AP 数量多于未知数，方程是超定的，没有精确解，只能找“最合理”的解；
3. NLOS 会让某些测距严重偏离，直接解方程会被坏数据拉偏；
4. 目标在运动，每一帧的观测都要和运动模型结合，不是孤立地解一帧方程。

PF 的做法是：不直接解方程，而是撒大量假设点，用观测去评估每个假设的合理性，让合理的假设存活、不合理的被淘汰。这是一种“用采样代替解析求解”的策略。它不要求方程有闭式解，也不要求噪声是高斯的，还能自然处理多峰、NLOS、运动模型等复杂情况。

1.8.5 总结

残差 = 实测距离 − 粒子假设位置下的理论距离。

• “实测距离”来自设备；
• “理论距离”来自粒子自己的位置，不是真值；
• 真值我们不知道，所以才要撒粒子去逼近它；
• 粒子滤波的本质是：用大量假设覆盖未知空间，用观测给每个假设打分，让高分假设存活、低分假设淘汰，最终高分假设的加权平均就是估计值。

---

2. WPF 的两个权重层次

2.1 NLOS 锚点降权，降的是“谁的”权重

关键结论：

WPF 降的不是粒子的权重 $\tilde{\omega}^{(i)}$，而是该 AP 在“决定粒子权重”这件事上的话语权。

标准 PF 中，似然是各 AP 高斯似然的等权乘积：

$$L^{(i)} = \prod_j \mathcal{N}\left(r_j^{(i)};0,\sigma_j^2\right)$$

每个 AP 对 $L^{(i)}$ 的贡献是“一票”。WPF 把乘积改成带指数的乘积：

$$L_{\text{WPF}}^{(i)} = \prod_j \left[ \mathcal{N}\left(r_j^{(i)};0,\sigma_j^2\right) \right]^{\widetilde{\mathrm{WAP}}_j}$$

其中 $\widetilde{\mathrm{WAP}}_j$ 是每个 AP 一个标量，由该 AP 到目标的距离决定：

$$\widetilde{\mathrm{WAP}}_j = \frac{1/(d_j+\epsilon)}{\max_l 1/(d_l+\epsilon)}$$

于是：

• 近端 AP：$\widetilde{\mathrm{WAP}}_j\approx 1$，它的一票基本等于原来的一票；
• 远端/NLOS AP：$\widetilde{\mathrm{WAP}}_j\ll 1$，它的一票被压缩成“半票”甚至“十分之一票”。

对数域看最清楚：

$$\log L_{\text{WPF}}^{(i)} = \sum_j \widetilde{\mathrm{WAP}}_j \cdot \log \mathcal{N}\left(r_j^{(i)};0,\sigma_j^2\right)$$

$\widetilde{\mathrm{WAP}}_j$ 是乘在“第 $j$ 个 AP 的对数似然项”上的系数，它不直接出现在粒子权重上，而是通过改变似然来间接改变粒子权重。

2.2 WPF 的对数似然

标准 PF 的联合似然是：

$$L^{(i)} = \prod_j p(d_j\mid\mathbf{x}^{(i)})$$

WPF 改成：

$$L_{\text{WPF}}^{(i)} = \prod_j \left[ p(d_j\mid\mathbf{x}^{(i)}) \right]^{\widetilde{\mathrm{WAP}}_j}$$

对数域：

$$\log L_{\text{WPF}}^{(i)} = \sum_j \widetilde{\mathrm{WAP}}_j \cdot \log p(d_j\mid\mathbf{x}^{(i)})$$

$\widetilde{\mathrm{WAP}}_j$ 就是乘在“第 $j$ 个 AP 的对数似然项”前面的一个系数。近端 AP 系数接近 1，它的对数似然项原样计入；远端/NLOS AP 系数很小，它的对数似然项被压缩，即使残差很大，对总似然的拉低作用也被削弱。

2.3 数值例子：两层权重分开

设两个 AP：

• AP1：近端 LOS，$d_1=5$ m，$\sigma_1=0.5$ m；
• AP2：远端 NLOS，$d_2=40$ m，$\sigma_2=0.5$ m，实测被 NLOS 拉大了 3 m。

两个候选粒子：

• 粒子 A：位置靠近真实值，对 AP1 残差 $r_1=0.1$，对 AP2 残差 $r_2=3.0$（因为 NLOS 偏差）；
• 粒子 B：位置被 AP2 的异常测距“骗”过去，对 AP1 残差 $r_1=4.0$，对 AP2 残差 $r_2=0.1$。

标准 PF（等权乘积）：

	AP1 似然	AP2 似然	总似然 $L$
粒子 A	$\mathcal{N}(0.1;0,0.5^2)\approx 0.78$	$\mathcal{N}(3.0;0,0.5^2)\approx 2.4\times10^{-8}$	$\approx 1.9\times10^{-8}$
粒子 B	$\mathcal{N}(4.0;0,0.5^2)\approx 1.3\times10^{-14}$	$\mathcal{N}(0.1;0,0.5^2)\approx 0.78$	$\approx 1.0\times10^{-14}$

标准 PF 下，粒子 A 的总似然仍然远大于 B，但这个差距是被 NLOS 的 AP2 大幅拉开的——AP2 用一票否决权把粒子 A 的似然压了 8 个数量级。

WPF（距离反比加权）：

$$\widetilde{\mathrm{WAP}}_1 = \frac{1/(5+\epsilon)}{1/(5+\epsilon)} = 1,\qquad \widetilde{\mathrm{WAP}}_2 = \frac{1/(40+\epsilon)}{1/(5+\epsilon)} = \frac{5+\epsilon}{40+\epsilon} \approx 0.12$$

	AP1 项 $\mathcal{N}^{1}$	AP2 项 $\mathcal{N}^{0.12}$	总似然 $L_{\text{WPF}}$
粒子 A	$0.78$	$(2.4\times10^{-8})^{0.12}\approx 0.11$	$\approx 0.086$
粒子 B	$1.3\times10^{-14}$	$(0.78)^{0.12}\approx 0.97$	$\approx 1.3\times10^{-14}$

关键变化：粒子 A 的总似然从 $10^{-8}$ 量级被“救回”到 $10^{-1}$ 量级，因为 AP2 的异常残差被指数 $0.12$ 压扁了；而粒子 B 依然很差，因为 AP1 的残差 4.0 没有被降权，仍然是一票否决。最终粒子 A 的权重远大于 B，滤波结果不会被 NLOS 的 AP2 拉偏。

这个例子里：

• 粒子的权重 $\tilde{\omega}^{(i)}$ 仍然是每个粒子一个标量（A 大 B 小）；
• 被降权的是 AP2 的 $\widetilde{\mathrm{WAP}}_2\approx 0.12$，它是 AP 层面的系数，不是粒子层面的权重。

2.4 层次对照表

层次	符号	数量	含义	谁在用它
残差	$r_j^{(i)}$	每粒子 × 每 AP 一个	粒子对某 AP 的测距偏差	算似然
AP 权重	$\widetilde{\mathrm{WAP}}_j$	每 AP 一个	该 AP 在似然乘积中的话语权	WPF 独有，乘在似然指数上
粒子似然	$L^{(i)}$	每粒子一个	粒子对所有 AP 的综合解释能力	更新粒子权重
粒子权重	$\tilde{\omega}^{(i)}$	每粒子一个	该粒子位置假设的可信度	重采样 + 输出估计

一句话回答：

PF 对每个粒子计算的是“该粒子对所有 AP 残差的高斯似然”，并把它乘进该粒子的标量权重里；WPF 对 NLOS/远端锚点降的是“该 AP 在似然乘积中的指数系数 $\widetilde{\mathrm{WAP}}_j$”，也就是这个 AP 对粒子权重更新的投票权，而不是直接去改某个粒子的权重。

所以 WPF 的“加权”和 WLS 的“加权”也不是一回事：WLS 加权改的是线性方程组的行权重，WPF 加权改的是似然乘积中每个 AP 项的指数。两者都叫“加权”，但作用对象和数学位置完全不同。

---

3. 定位精度的限制因素

3.1 HDOP（水平精度因子）

HDOP（Horizontal Dilution of Precision，水平精度因子）是衡量定位几何构型在水平方向上对测距误差放大程度的指标。它不直接表示“误差有多大”，而是表示“AP/锚点布局把测距误差在水平面上放大了多少倍”。

3.1.1 从公式看 HDOP

定位解算（如 LS/WLS）的协方差矩阵为 $\sigma^2(G^TG)^{-1}$，HDOP 从其对角元中提取：

$$\text{HDOP} = \sqrt{ (G^TG)^{-1}_{11} + (G^TG)^{-1}_{22} }$$

若使用加权（WLS/WNLS），则换成 $(G^TWG)^{-1}$，此时称为加权 HDOP：

$$\text{HDOP} = \sqrt{ \operatorname{trace}\left((G^TWG)^{-1}\right) }$$

$G$ 是几何矩阵（或设计矩阵），每一行代表目标到对应 AP 的方向余弦向量，刻画锚点相对于目标的方位关系。

3.1.2 与定位误差的关系

$$\sigma_{\text{水平}} \approx \text{HDOP} \times \sigma_{\text{测距}}$$

• HDOP $=1$：水平误差约等于测距误差，几何构型理想；
• HDOP $=5$：同样的测距误差，水平定位误差被放大 5 倍；
• HDOP 越大，水平精度越差；HDOP 越小，几何构型在水平面上越好。

3.1.3 HDOP 在整个 DOP 体系中的位置

指标	约束的维度	关系
GDOP	3D 位置 + 时间	最综合
PDOP	三维位置	$PDOP^2=HDOP^2+VDOP^2$
HDOP	水平面（x, y）	本文主题
VDOP	垂直高度（z）	三维定位中不可忽略

在三维定位中，HDOP 小不代表 VDOP 也小，必须分别关注。

3.1.4 HDOP 在 PF/WPF 场景中的意义

1. 判断当前 AP 几何是否适合水平定位：如果参与定位的 AP 在水平方向上分布很集中（例如都在同一侧），即使测距很准，HDOP 也会很大，水平坐标会很不确定。
2. 作为 AP 筛选/加权依据：现有算法用残差筛 AP；HDOP 可从几何层面补充。可以在 WPF 中，把 HDOP 相关因子引入粒子权重，让几何贡献差的 AP 组合被自动降权。
3. 解释“残差好但定位差”：有时所有 AP 残差都不大，但解算出的水平位置依然抖动，原因就是 AP 几何构型差导致 HDOP 偏高，残差指标本身无法反映这一点。

3.1.5 一句话总结

HDOP = 水平方向上，几何构型对测距误差的放大倍数。 它由 AP 与目标的相对方位决定，与测距本身准不准无关；HDOP 越小，说明当前锚点布局在水平面上越“有区分度”，同样的测距误差能换来越高的水平定位精度。

3.2 噪声估计与定位精度

“定位准需要噪声估计准”这个想法方向是对的，但只说对了一半。更准确的说法是：噪声估计准是“定位准”的重要前提，但不是唯一条件；而且不同算法对噪声估计的敏感程度完全不同。

3.2.1 为什么“噪声估计准”确实关键

（1）WLS/WNLS 里，噪声估计直接决定权重

WLS 的权重就是 $w_i=1/\sigma_i^2$。如果 $\sigma_i$ 估小了，这个 AP 会被过度信任；估大了，会被过度冷落。权重设错，WLS 就不再是 BLUE（最佳线性无偏估计），甚至可能比不加权的 LS 还差。WNLS 同理，协方差矩阵 $\boldsymbol{\Sigma}$ 估不准，就无法逼近 CRLB。

（2）PF/WPF 里，噪声估计决定似然的“尖锐程度”

粒子似然是：

$$\mathcal{N}(r;0,\sigma^2) \propto \exp\left(-\frac{r^2}{2\sigma^2}\right)$$

• $\sigma$ 估得太小：似然曲线极窄，残差稍大的粒子权重瞬间趋近 0。结果是粒子退化极快，少数粒子垄断权重，滤波“过度自信”，一旦真值不在这些粒子附近就彻底跑偏；
• $\sigma$ 估得太大：似然曲线太平坦，好粒子和坏粒子的权重差不多，滤波“反应迟钝”，收敛慢，输出估计方差大。

（3）两阶段 AP 剔除的阈值也依赖噪声水平

8 m 的残差阈值，本质上是在问“这个残差是正常噪声还是 NLOS 偏差”。如果实际噪声 $\sigma=0.5$ m，8 m 是 16 倍标准差，很安全；如果实际噪声 $\sigma=3$ m，8 m 只有不到 3 倍标准差，正常测量也可能被误杀。阈值合理性直接取决于噪声估计。

3.2.2 为什么“噪声估计准”还不够

（1）几何构型差，噪声再准也救不了

$$\sigma_{\text{位置}} \approx \text{HDOP} \times \sigma_{\text{测距}}$$

如果 AP 在水平面上几乎共线，HDOP 可能是 10 甚至更大。此时即使 $\sigma$ 估得极准，位置误差仍然被几何放大 10 倍。噪声估计准只保证“没有额外放大”，不保证“放大倍数小”。

（2）噪声的“质”比“量”更重要

噪声估计通常只估了方差，但真实误差可能是：

• NLOS 系统性正偏差：不是零均值，估方差没用，需要偏差建模或鲁棒估计；
• 多径引起的非高斯重尾：方差能描述一部分，但尾部行为需要重尾分布（如 Student-t）或 M 估计；
• 异常值/粗差：单个离谱测量，方差估计会被污染，需要先剔除再估。

（3）模型失配比噪声估计误差更致命

如果运动模型写错（目标急转弯但用匀速模型）、观测模型写错（测距有固定偏差但模型假设零均值），那么噪声估计再准，滤波也会系统性偏离。模型误差是偏差，噪声估计误差是方差，前者往往影响更大。

（4）算法本身的收敛性/初值问题

NLS/WNLS/PF 都需要初值或粒子覆盖。初值差、粒子数不足、重采样过早贫化，这些问题的后果不会因为噪声估计准而消失。

3.2.3 更准确的表述

定位准需要：噪声估计合理 + 几何构型良好 + 误差模型匹配 + 算法收敛正常。噪声估计准是必要非充分条件；而且“准”不等于“精确”，更关键的是“不严重低估”——低估噪声比高估噪声危险得多。

为什么“不严重低估”比“精确估计”更实际：

• 低估 $\sigma$ → 过度自信 → 粒子退化、WLS 权重极端化 → 灾难性失败；
• 高估 $\sigma$ → 保守 → 收敛慢、精度略降 → 性能平滑退化，不会崩。

所以在工程上，宁可把 $\sigma$ 估得略大一点（加安全裕量），也不要估小。这也是 PF 里常给似然方差设下限、WLS 里给权重设上限的原因。

3.2.4 结合 PF/WPF 项目的落地建议

1. 在线估计噪声，而不是用固定值：用新息序列（残差序列）的滑动方差估计 $\sigma_j$，或者用鲁棒尺度估计（如 MAD）避免被 NLOS 污染；
2. 给似然方差设下限：$\sigma_j\leftarrow\max(\sigma_j,\sigma_{\min})$，防止某帧噪声估得过小导致粒子瞬间退化；
3. 区分“方差”和“偏差”：WPF 的距离反比降权处理的是“远端 AP 更可能有偏差”，这是偏差层面的鲁棒化；噪声方差估计处理的是“零均值波动”，两者互补，不能互相替代；
4. 用 HDOP 监控几何：如果 HDOP 突然变大，说明当前 AP 组合几何变差，此时即使噪声估计准，也应考虑放宽残差阈值或增加 AP，而不是硬信当前解；
5. 两阶段剔除的阈值应随噪声估计自适应：把固定 8 m 改成 $k\cdot\hat{\sigma}$（如 $4\hat{\sigma}$ 到 $5\hat{\sigma}$），噪声水平变化时阈值自动跟着变。

3.2.5 一句话总结

“噪声估计准”是定位准的重要拼图，但它管的是“权重和似然设得对不对”，管不了“几何构型好不好、误差分布是不是高斯、模型有没有写错”。工程上最危险的不是噪声估计不够精确，而是严重低估噪声导致算法过度自信；所以策略应是“合理估计 + 安全裕量 + 与几何监控和鲁棒处理配合”，而不是追求噪声参数的绝对精确。

---

4. 噪声建模：从反推到数据拟合

4.1 从测距反推噪声性质

这个方向是对的，而且在定位领域已有成熟的研究脉络。可以拆成两个可操作的问题：

1. 从测距结果本身能否“反推”出噪声的性质（方差、分布、甚至成因）？
2. 噪声是否与测距算法本身相关，从而可以“按算法分别建模”？

答案是：两者都可以，而且结合起来确实能让定位更准。

4.1.1 从残差/新息反推噪声性质

在 PF/WPF 中，已经有一个天然的“探测器”：粒子预测位置与实测测距之间的残差（新息）。

在线方差辨识：最基础的做法是对残差序列做滑动方差估计，动态更新每个 AP 的 $\sigma_j$。更稳健的是用 MAD（中位数绝对偏差）代替标准差，避免被 NLOS 帧污染。在卡尔曼框架下，这等价于用新息协方差 $S_k=HP_{k|k-1}H^T+R_k$ 反推测量噪声协方差 $R_k$，已有工作用 Sage-Husa 自适应滤波实时估计 $R$。

分布性质辨识：只看方差不够，因为 NLOS 噪声是有偏的，不是零均值高斯。利用标准化新息检验统计量 $d_k=v_k^TS_k^{-1}v_k$ 可以判断当前残差是否“异常”；如果 $d_k$ 持续偏大，说明该 AP 的噪声不再是简单的高斯白噪声，可能进入了 NLOS 状态。更进一步，可以用 Allan 方差分析残差序列的时间相关性，区分白噪声与有色噪声。已有 UWB 定位研究表明，在有色噪声场景下，自适应粒子滤波（通过调整似然分布）的定位效果优于标准 PF 和 EKF。

4.1.2 噪声与测距算法的相关性

噪声确实与测距算法相关，这不是猜想，而是物理事实：

• TOA 的误差主要来自时钟同步精度和信号带宽，LOS 下近似零均值高斯，NLOS 下叠加正偏差；
• RSS 的误差随距离增大而增大（路径损耗模型的不确定性），方差本身是距离的函数；
• TDOA 的噪声结构更复杂，因为差分操作会引入 AP 之间的噪声相关性。

因此，不要用一个统一的 $\sigma$ 覆盖所有 AP。更合理的做法是：为每个 AP 维护一个“噪声指纹”，包含方差、偏置先验、分布类型（高斯/重尾），并根据其测距体制（TOA/RSS/TDOA）初始化不同的先验。在线更新时，只更新与当前残差一致的参数，而不是把所有 AP 的噪声混在一起估计。

4.1.3 噪声“成因”的反演：从估参数到分类状态

反推“成因”在工程上通常落地为状态分类，而不是解析出具体的物理机制。最实用的是 LOS/NLOS 二分类：

利用残差特征（标准化新息、残差的滑动均值/方差、信号质量指标如 RSSI 或首径功率）训练一个轻量分类器，在线判断每个 AP 当前处于 LOS 还是 NLOS。一旦分类为 NLOS，就切换该 AP 的似然模型：从零均值高斯切换到带偏置的重尾分布（如 Student-t），或者直接大幅提高其等效方差。这比 WPF 中固定的距离反比降权更精细——因为它区分了“远端但 LOS”和“远端且 NLOS”两种情况。

贝叶斯非参数方法提供了更彻底的方案：用变分贝叶斯（VB）将噪声的统计参数（均值、方差、甚至自由度）视为随机变量，与粒子状态联合估计。这样不需要预先指定噪声是高斯还是重尾，VB 会自动从数据中推断出后验的噪声参数。代价是计算量高于固定噪声模型，但对“噪声性质未知”的场景非常合适。

4.1.4 融入 PF/WPF 框架的工程化方案

不需要推翻现有架构，可以在三个位置嵌入“噪声反演”：

① 在 WPF 的降权因子中引入“噪声类型”维度

目前的 WAP 只用了距离反比。可以扩展为：

$$\mathrm{WAP}_j = f(d_j)\cdot g(\hat{\sigma}_j)\cdot h(\text{NLOS概率}_j)$$

其中 $\hat{\sigma}_j$ 和 NLOS 概率都来自在线辨识。这样降权不再只依赖“远不远”，还依赖“当前噪声大不大、是否被污染”。

② 用新息序列在线更新 $\sigma_j$，替代固定噪声参数

在每一轮 PF/WPF 更新后，用该轮粒子的加权残差方差更新 $\sigma_j$（加遗忘因子和平滑）。这直接回应了“噪声估计准定位才准”——让噪声估计在线自适应，而不是离线标定一次。

③ 两阶段 AP 剔除的阈值自适应化

把固定的 8 m 阈值改为 $k\cdot\hat{\sigma}_j$（例如 $4\hat{\sigma}_j$ 到 $5\hat{\sigma}_j$）。当在线辨识到某 AP 噪声水平升高时，剔除阈值自动收紧或放宽，而不是用一个硬编码的 8 m 应对所有场景。

4.1.5 边界与提醒

这套思路的收益是有条件的：当 NLOS 偏差是主导误差时，噪声类型辨识和自适应方差带来的增益最大；当几何构型本身很差（HDOP 很高）时，即使噪声估计完美，定位精度仍受几何限制。此外，在线辨识需要一定的数据积累（残差样本），在目标刚启动或 AP 切换频繁时，初期估计可能不稳定，需要设置保守的先验或安全裕量。

4.1.6 总结

从测距残差反推噪声性质（方差、分布、LOS/NLOS 状态）是可行的，噪声与测距算法强相关因此应按 AP 分别建模，将辨识出的噪声参数反馈到 WPF 的降权因子、似然方差和剔除阈值中，可以在不改变主体架构的前提下提升定位鲁棒性。代价是增加了在线估计的计算和调参复杂度，收益在 NLOS 主导的场景中最显著。

4.2 用数据拟合噪声

可以，而且已有数据恰好是做这件事最宝贵的资源。用数据拟合噪声，本质上就是把“反推噪声性质”从在线估计升级为离线建模。一旦有了可靠的离线噪声模型，PF/WPF 的似然函数、WPF 的降权因子、两阶段剔除的阈值都可以从“拍脑袋”变成“有数据支撑”。

4.2.1 先分类，再拟合：LOS 和 NLOS 是两种完全不同的噪声

如果把所有测距误差混在一起拟合一个分布，得到的参数会毫无物理意义。必须先把数据分成 LOS 和 NLOS 两类，分别建模。

LOS 噪声：通常可以用零均值高斯建模，核心参数是标准差 $\sigma_L$。如果数据里能看到轻微偏置，也可以拟合一个非零均值 $\mu_L$，但通常量级很小。

NLOS 噪声：不是零均值高斯，而是正偏、重尾。已有研究常用两种形式：

• 对数正态分布：适合描述 NLOS 引起的额外正偏差，单峰时用一个对数正态即可，复杂遮挡场景可以用两个对数正态的加权和来拟合双峰或多峰形态；
• 重尾分布（如 Student-t）：如果 NLOS 数据在尾部“拖得很长”，高斯拟合会严重低估极端误差的概率，重尾分布能更真实地反映这种特性。

操作上：用数据画出两类误差的直方图，肉眼判断是单峰还是双峰、是否明显右偏。然后用 scipy.stats 或 fitdist 对候选分布做拟合，用 KS 检验或 AIC/BIC 比较哪个分布更匹配。不要预设结论——数据说是什么分布，就用什么分布。

4.2.2 拟合“距离相关”的噪声参数：方差不是常数

WPF 用距离反比降权，已经隐含了“远端 AP 更不可靠”的假设。数据拟合可以把这种定性假设变成定量的函数关系。

一个可落地的流程：

1. 采集覆盖全场景的数据：目标走遍所有可行位置，记录每个 AP 的测距值和对应的真实距离；
2. 计算每个采样点的测距误差 $e=d_{\text{测}}-d_{\text{真}}$；
3. 按真实距离分箱（例如每 1 m 一个箱），把误差分配到对应箱中；
4. 对每个箱计算误差的均值和方差，得到序列 $\{\mu_b\}$ 和 $\{\sigma_b^2\}$；
5. 对均值序列和方差序列分别做多项式拟合。

关键经验结论是：均值更适合线性拟合，方差更适合二次拟合，因为方差随距离增长的速度比均值更快。也就是说，可以得到类似

$$\sigma^2(d)=ad^2+bd+c$$

的函数，直接替代固定 $\sigma_j$。

如果数据量不足以做分箱拟合，退而求其次：为每个 AP 单独估计一个方差，而不是所有 AP 共用一个 $\sigma$。不同 AP 的部署位置不同，噪声水平本来就不应该相同。

4.2.3 训练 LOS/NLOS 分类器：用残差和信号特征判状态

噪声分布拟合解决的是“已知状态后噪声长什么样”，还需要解决“当前帧这个 AP 处于什么状态”。

最直接的特征就是残差：NLOS 锚点的平均残差通常系统性地高于 LOS 锚点。现有的两阶段 PF 剔除已经在隐式地做这件事，但可以把固定阈值 8 m 替换为基于分类器概率的软决策。

更丰富的特征来自接收信号本身：峰度、偏度、上升时间、最强路径能量、总能量、RMS 时延扩展等，在文献中被反复证明对 LOS/NLOS 区分有效，部分方法的分类准确率可达 93%–99%。方法上可以从简单的 LS-SVM（同时做分类和偏差回归）到随机森林、DNN，选择取决于数据量和算力预算。

落地建议：如果不想引入复杂的机器学习管线，只用残差特征 + 一个轻量分类器（如逻辑回归或小决策树）就能比固定阈值更自适应。分类器的输出是一个 NLOS 概率，直接接入下一步的似然切换。

4.2.4 把拟合结果馈入 PF/WPF：三个接入点

接入点 1：似然函数从“单一高斯”切换为“状态依赖的分布”

这是最直接的收益。现有 PF 的自然更新用高斯似然。拟合之后，对每个 AP 根据分类器输出的 NLOS 概率选择或混合似然：

• 判为 LOS → 用拟合好的高斯 $\mathcal{N}(\mu_L,\sigma_L^2)$；
• 判为 NLOS → 用拟合好的对数正态或重尾分布；
• 概率模糊时 → 用概率加权混合似然，避免硬切换的抖动。

这比 WPF 中“不管状态一律距离反比降权”更精细：它区分了“远端但 LOS”和“远端且 NLOS”。

接入点 2：WAP 降权因子中加入“拟合噪声水平”维度

现在的 $\mathrm{WAP}_j=1/(d_j+\epsilon)$ 只用了距离。拟合之后可以扩展为：

$$\mathrm{WAP}_j \propto \frac{1}{d_j+\epsilon} \cdot g(\hat{\sigma}_j) \cdot h(\text{NLOS概率}_j)$$

其中 $g(\cdot)$ 让拟合方差大的 AP 进一步降权，$h(\cdot)$ 让被判为 NLOS 的 AP 大幅降权。距离、噪声水平、状态三类信息同时进入降权决策，比单一距离反比更稳健。

接入点 3：两阶段剔除的阈值从“固定 8 m”改为“数据驱动的自适应阈值”

用拟合出的 $\sigma(d)$ 或分类器的 NLOS 概率来动态设定阈值。例如：阈值 $=k\cdot\hat{\sigma}(d_j)$（$k$ 取 4–5），或者“NLOS 概率超过 0.8 且残差超过 $3\hat{\sigma}$”才剔除。阈值不再是一个脱离场景的数字，而是从数据分布中推导出来的。

4.2.5 数据里的“真值”从哪来

拟合噪声需要测距误差，而测距误差需要真实距离。如果数据是在已知坐标的参考点上采集的（目标放在标定点上，或高精度全站仪/激光跟踪仪提供真值），那是最理想的。如果真值本身来自某个定位系统的输出，那么拟合出的“噪声”会混入定位系统的误差——拟合之前必须把真值的来源和精度说清楚，否则拟合的是“测距误差 + 真值误差”的混合体。

另一个实用技巧：用高精度参考系统（如光学动捕）只采集一小段标定数据来拟合噪声参数，然后把参数用到全部数据上，不需要全程都有真值。

4.2.6 总结

可以拟合，而且值得拟合。 路径是：先按 LOS/NLOS 分类，分别拟合分布形态（高斯/对数正态/重尾），再拟合距离相关的方差函数（分箱+多项式），同时训练一个基于残差和信号特征的轻量状态分类器；最后把“状态概率 + 拟合参数”接入 PF 的似然函数、WPF 的降权因子和两阶段剔除的自适应阈值。代价是离线建模的工作量和真值数据的质量要求，但收益是把目前依赖经验设置的参数全部替换为数据驱动的估计，在 NLOS 主导的场景中，定位鲁棒性的提升通常是显著的。