设想你带队爬山，大雾弥漫，手机 GPS 说“再走 200 米就到”，计步器说“已经走了 3500 步，应该快到顶了”，指南针却指着正北方——而地图上山顶明明在北偏东 30 度。三个人各说各话，到底该信谁？多传感器融合就是这场旅途中的“智囊团”：它不偏信任何一方，而是综合所有人的报告，算出最可靠的位置。本文围绕三种融合算法展开：扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）和粒子滤波（PF）。你可以把它们理解成三种风格的向导——EKF 习惯把弯路当作一小段直线来估算；UKF 会派几个探子沿着误差圈的关键方向去探路，再把见闻汇总；粒子滤波则干脆撒下一把豆子，让豆子们用“投票”的方式告诉你人在哪里。三种方案精度不同、开销不同，适合的地形也不同。读完本文，你将能像挑选交通工具一样，为你的机器人或自动驾驶系统选对融合算法。

岗位关联

掌握这三种滤波算法的原理与取舍，能帮你在自动驾驶、机器人定位等场景中快速完成技术选型，避免“用重型武器打蚊子”的资源浪费——这是一线 AI 算法工程师的基本功。

目录
  1. 可视化演示
  2. 融合概述：一群向导的智慧
  3. 坐标标定：先统一手表与地图
  4. 卡尔曼基础：预测加修正
  5. 扩展卡尔曼：切线法走弯路
  6. 无迹卡尔曼：探子采样法
  7. 粒子滤波：撒豆成兵
  8. 对比权衡：三种方案的成本账
  9. 选型决策：按地形配装备
  10. AI 应用场景
  11. Python 演示
  12. 常见误区
  13. 面试题

可视化演示

通过三种滤波器对比、状态估计过程和性能权衡图三个维度，动态展示EKF、UKF与粒子滤波的原理差异与适用场景，直观理解传感器融合算法的选择策略。

本节配有交互动效（进入视口后自动加载，约 3KB）。

三种滤波器对比 EKF: 线性化近似，计算快 UKF: Sigma点，精度高 PF: 粒子采样，非高斯

融合概述：一群向导的智慧

旅行团里三位“向导”各有专长也各有短板：GPS 擅长绝对位置但偶尔飘移，计步器勤快却会累积漂移，指南针只管方向不管距离。单独听谁都可能走偏；但如果把三份“不完美”的报告放在一起，反而能拼出更接近真相的估计。多传感器融合正是利用多个传感器的测量，通过概率方法综合出最优状态估计——这就是“三个臭皮匠，顶个诸葛亮”的数学版本。

融合的价值来自两个层面。信息冗余指多个传感器测同一个物理量（如 GPS 和超宽带都测位置），互相校验、降低单点失效风险；信息互补指它们观测不同的物理量（如里程计测位移、陀螺仪测角速度），拼起来才能完整描述状态。融合的基本操作是“加权平均”，关键在权重：方差小、更可靠的传感器应该获得更大的话语权。

假设两个传感器分别给出位置估计 $\hat{x}_1$ 和 $\hat{x}_2$，方差为 $\sigma_1^2$ 和 $\sigma_2^2$，最优融合是一个加权平均：

就像旅行团开会时，谁的指南针更稳谁的发言权重就更大。传感器融合的本质，就是给“靠谱的人”更多话语权。后面三种算法解决的，都是同一个问题：在动态系统中，如何不断更新这个权重、把不完美信息合成为最优答案。

x^=σ22 x^1+σ12 x^2σ12+σ22\hat{x} = \frac{\sigma_2^2\,\hat{x}_1 + \sigma_1^2\,\hat{x}_2}{\sigma_1^2 + \sigma_2^2}
  1. 分子第一项用 $\sigma_2^2$ 作为 $\hat{x}_1$ 的权重——当第二个传感器越不可靠（方差越大），给第一个估计的信任就越高。
    σ22 x^1\sigma_2^2\,\hat{x}_1
  2. 分子第二项对称地，用 $\sigma_1^2$ 作为 $\hat{x}_2$ 的权重。
    σ12 x^2\sigma_1^2\,\hat{x}_2
  3. 分母归一化$\sigma_1^2 + \sigma_2^2$ 确保两个权重相加等于 1。
    σ12+σ22\sigma_1^2 + \sigma_2^2
  4. 极限直觉当 $\sigma_1 \ll \sigma_2$ 时，$\hat{x} \approx \hat{x}_1$，融合结果自动倒向“看得准”的那一方。
    x^≈x^1\hat{x} \approx \hat{x}_1

坐标标定：先统一手表与地图

三位向导各执一词，是因为他们的“起点”和“钟表”不一样：GPS 说的是经纬度，计步器说的是“出发后走了多少步”，指南针说的是方位角。不先对齐基准，融合就成了对空喊话。传感器融合也有同样的前置条件——时间同步和空间标定。

时间同步解决“你看到的是 0.1 秒前，我看到的是现在”的时序错位。激光雷达一帧 10ms、相机一帧 33ms，如果直接拿两个不同时刻的数据去融合，等于把“昨天的高度”和“今天的位置”算在一起。空间标定则解决坐标系不一致：每个传感器都有自己的坐标系，雷达点在雷达系、相机点在相机系、IMU 在 IMU 系，必须通过外参（相对位姿）变换到同一个车体或世界坐标系。

空间对齐的数学工具是齐次变换矩阵，它把旋转和平移打包成一个 4×4 矩阵：

这就像开会前先把所有人的“参照物”统一成同一张地图、同一块手表。跳过这一步，后面的 EKF、UKF、粒子滤波再精巧，也只是“拿错误地图导航”，越融越乱。

pB=[RABtAB01×31]pAp_B = \begin{bmatrix} R_{AB} & t_{AB} \\ 0_{1\times3} & 1 \end{bmatrix} p_A
  1. $p_A$传感器 A 坐标系下的原始测量坐标，是变换的输入。
    pA∈R3p_A \in \mathbb{R}^3
  2. 旋转矩阵 $R_{AB}$3×3 矩阵，描述 B 系相对 A 系的姿态旋转。
    RAB∈R3×3,RABTRAB=IR_{AB} \in \mathbb{R}^{3\times3}, \quad R_{AB}^T R_{AB} = I
  3. 平移向量 $t_{AB}$3×1 向量，表示 B 系原点在 A 系中的位置。
    tAB∈R3t_{AB} \in \mathbb{R}^3
  4. 齐次变换把旋转与平移拼成 $4\times4$ 矩阵后，连续坐标变换只需一次矩阵乘法。
    [Rt01]−1=[RT−RTt01]\begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}^{-1} = \begin{bmatrix} R^T & -R^T t \\ 0 & 1 \end{bmatrix}

卡尔曼基础：预测加修正

最老练的向导从不只看眼前一步。他会先根据现在的速度和方向“猜”下一步到哪——预测；等看到真实地标后，再按推测与实际的偏差大小修正判断——更新。卡尔曼滤波（Kalman Filter, KF）把这套经验提炼成数学框架，循环执行这两个动作。

KF 有两个核心假设：系统是线性的（位置、速度的变化可以写作矩阵乘法），噪声是高斯的（误差呈“中间密、两边疏”的钟形曲线）。只要满足这两个假设，KF 就是线性最优估计。它的全部智慧浓缩在 5 个方程里：

老向导信步预测、看到地标后小幅或大幅扭正方向，一切在“误差服从正态分布”的假设里自洽。可一旦山路变得弯弯曲曲——系统非线性了——直线推算就不够用了，于是有了 EKF。

x^k−=Ax^k−1+BukPk−=APk−1AT+QKk=Pk−HT(HPk−HT+R)−1x^k=x^k−+Kk(zk−Hx^k−)Pk=(I−KkH)Pk−\begin{aligned}\hat{x}_{k}^{-} &= A\hat{x}_{k-1} + Bu_k \\P_{k}^{-} &= AP_{k-1}A^{T} + Q \\K_{k} &= P_{k}^{-}H^{T}(HP_{k}^{-}H^{T} + R)^{-1} \\\hat{x}_{k} &= \hat{x}_{k}^{-} + K_{k}(z_{k} - H\hat{x}_{k}^{-}) \\P_{k} &= (I - K_{k}H)P_{k}^{-}\end{aligned}
  1. 状态预测把上一时刻状态按线性矩阵 $A$ 推进，$u_k$ 是外部控制输入（如油门、转向）。
    x^k−=Ax^k−1+Buk\hat{x}_{k}^{-} = A\hat{x}_{k-1} + Bu_k
  2. 协方差预测不确定度按同样的线性变换放大，$Q$ 是过程噪声协方差。
    Pk−=APk−1AT+QP_{k}^{-} = AP_{k-1}A^{T} + Q
  3. 卡尔曼增益$K_k$ 是一个“信任调节旋钮”，在模型不确定度 $P_k^-$ 与观测噪声 $R$ 之间取平衡。
    Kk=Pk−HT(HPk−HT+R)−1K_{k} = P_{k}^{-}H^{T}(HP_{k}^{-}H^{T} + R)^{-1}
  4. 状态更新用“观测值与预测值的残差”乘以增益来修正估计。
    x^k=x^k−+Kk(zk−Hx^k−)\hat{x}_{k} = \hat{x}_{k}^{-} + K_{k}(z_{k} - H\hat{x}_{k}^{-})
  5. 协方差更新修正后不确定度下降，供下一轮预测使用。
    Pk=(I−KkH)Pk−P_{k} = (I - K_{k}H)P_{k}^{-}

扩展卡尔曼：切线法走弯路

老向导遇到弯曲山路，不会整体画错地图，而是“每走一小段，就把脚下这段弯道近似成一条直线”。只要弯得不太急，切线近似的误差就很小——这就是扩展卡尔曼滤波（Extended Kalman Filter, EKF）的哲学：把非线性函数在当前估计点附近做一阶泰勒展开。

EKF 的关键是用雅可比矩阵代替线性模型中的常数矩阵。雅可比是“切线斜率”从一元函数到多元函数的推广，记录了状态每个分量对输出每个分量的敏感度。运动模型 $f$ 和观测模型 $h$ 在当前状态处求偏导，就得到线性化后的矩阵：

拿到 $F_k$ 和 $H_k$ 后，EKF 的预测更新与 KF 结构相同，只是状态通过原始非线性函数推进：

切线法在路径“微弯”时又快又准，但若遇到急转弯，一阶近似的误差就会明显放大，甚至导致滤波器发散。于是那位更谨慎的向导 UKF 提出了新方案：不猜切线，直接派探子去走真实的路。

F_{k} = \left.\frac{\partial f}{\partial x}\right|_{\hat{x}_{k-1}}, \qquadH_{k} = \left.\frac{\partial h}{\partial x}\right|_{\hat{x}_{k}^{-}}
  1. 非线性预测把上一状态直接扔进原始非线性函数 $f$，而不是线性矩阵乘法。
    x^k−=f(x^k−1,uk)\hat{x}_{k}^{-} = f(\hat{x}_{k-1}, u_k)
  2. 协方差线性化传播用雅可比 $F_k$ 把不确定度推进一步，$Q$ 是过程噪声。
    Pk−=FkPk−1FkT+QP_{k}^{-} = F_k P_{k-1} F_k^{T} + Q
  3. 更新阶段与 KF 同构，只是把 $H$ 换成 $H_k$，观测预测用 $h(\hat{x}_{k}^{-})$。
    x^k=x^k−+Kk(zk−h(x^k−))\hat{x}_{k} = \hat{x}_{k}^{-} + K_{k} (z_{k} - h(\hat{x}_{k}^{-}))

无迹卡尔曼：探子采样法

另一位向导拒绝“画切线”。他挑选一小队探子，让他们沿着当前误差范围的关键方向分散出发，走一小段真实的路，回来后把探子们的实际落点汇总。这样既不用计算雅可比，又保留了真实路况中的弯曲信息——这就是无迹卡尔曼滤波（Unscented Kalman Filter, UKF）。

UKF 的核心是无损变换（Unscented Transform）：选取一组确定性采样点，称为 σ 点，通常 $2n+1$ 个（$n$ 是状态维数）。这些点经过非线性函数传播后，再通过加权统计还原出输出分布的均值和协方差。由于 σ 点携带了分布的二阶信息，UKF 的近似精度通常高于 EKF 的一阶线性化，而且不用求导。

σ 点的选取方式很优雅：一个点在均值上，其余点在协方差矩阵的关键方向上对称分布：

把这些点扔进非线性函数 $g$ 传播后，再按权重合成为新的均值与协方差：

探子们“用脚步丈量真实路线”，比切线近似更贴近实际分布，精度约二阶，计算量却远小于后面的粒子滤波。那为什么还需要粒子滤波？因为探子终究只是几个人——遇到悬崖峭壁、岔路丛生的复杂地形，小队人马就不够用了。

χ0=xˉ,χi=xˉ±(n+λ)Px\chi_0 = \bar{x}, \qquad \chi_i = \bar{x} \pm \sqrt{(n+\lambda) P_x}
  1. σ 点传播每个 σ 点独立通过非线性函数 $g$，到达新位置。
    χi′=g(χi)\chi_i' = g(\chi_i)
  2. 加权均值用均值权重 $W_i^m$ 对新位置加权求和，得到输出均值。
    xˉ′=∑i=02nWimχi′\bar{x}' = \sum_{i=0}^{2n} W_i^{m} \chi_i'
  3. 加权协方差用协方差权重 $W_i^c$ 计算各点相对新均值的离差平方和，再叠加过程噪声 $Q$。
    Px′=∑i=02nWic(χi′−xˉ′)(χi′−xˉ′)T+QP_x' = \sum_{i=0}^{2n} W_i^{c} (\chi_i' - \bar{x}')(\chi_i' - \bar{x}')^{T} + Q

粒子滤波：撒豆成兵

最“笨”也最猛的办法：向导把一大袋豆子撒在地图上，每颗豆子是一个猜测。队伍每走一段，豆子跟随机型移动一步；看到真实地标后，离地标近的豆子被做上记号（加权），远的豆子被捡走，好豆子被多复制几份。豆子不断“提纯”，最后最密集的地方就是答案——这就是粒子滤波（Particle Filter, PF）。

粒子滤波用一组带权重的随机样本（粒子）近似任意后验分布，不要求线性，也不要求高斯。它同样分预测、更新两步，但预测靠“粒子随状态方程散开”，更新靠“权重与观测似然成正比”，再用重采样抑制粒子退化：

豆子们不挑路况，哪怕地图上有两条岔路、误差呈双峰分布，豆子也能分成两堆各自投票——这是 EKF/UKF 做不到的。但豆子要够多才够准，计算开销也随之猛增。下一节我们算算这笔账。

w_{k}^{(i)} = w_{k-1}^{(i)} \times p(z_{k} | x_{k}^{(i)}), \qquad\bar{w}_{k}^{(i)} = \frac{w_{k}^{(i)}}{\sum_{j=1}^{N} w_{k}^{(j)}}, \qquadN_{eff} = \frac{1}{\sum_{i=1}^{N} (w_{k}^{(i)})^{2}}
  1. 权重更新旧权重乘以观测似然 $p(z_k | x_k^{(i)})$——如果粒子 $x^{(i)}$ 真是当前位置，出现观测 $z_k$ 的概率有多大。
    wk(i)=wk−1(i)×p(zk∣xk(i))w_{k}^{(i)} = w_{k-1}^{(i)} \times p(z_{k} | x_{k}^{(i)})
  2. 权重归一化让所有粒子权重之和等于 1，形成合法概率分布。
    wˉk(i)=wk(i)∑j=1Nwk(j)\bar{w}_{k}^{(i)} = \frac{w_{k}^{(i)}}{\sum_{j=1}^{N} w_{k}^{(j)}}
  3. 有效粒子数衡量“还有多少粒子的意见真正有用”，数值越小代表退化越严重。
    Neff=1∑i=1N(wk(i))2N_{eff} = \frac{1}{\sum_{i=1}^{N} (w_{k}^{(i)})^{2}}
  4. 重采样触发当 $N_{eff}$ 低于阈值（如 $N/2$），按权重比例放回抽样，淘汰低权重粒子、复制高权重粒子。
    Neff<N/2⇒resampleN_{eff} < N/2 \Rightarrow \text{resample}

对比权衡：三种方案的成本账

EKF 像自行车，灵巧便宜，适合平缓路；UKF 像轿车，稍贵但更稳，弯道也不怕；粒子滤波像越野车，能翻山越岭，但油钱吓人。选什么，要看地形、看预算、看时限。三种算法在精度、计算成本、鲁棒性三个维度上各有侧重。

从精度看：KF 只适用于线性高斯；EKF 是一阶近似，弱非线性下够用；UKF 通过 σ 点传播二阶信息，通常比 EKF 更准；粒子滤波在粒子数趋于无穷时逼近真实后验，是非线性非高斯场景下的“渐近最优”。从计算成本看：KF 最省，EKF 需要额外算雅可比，UKF 需要传播 $2n+1$ 个 σ 点，粒子滤波要维护上千个粒子，开销最大。从鲁棒性看：粒子滤波最强，能处理多峰与非高斯；EKF/UKF 假设高斯分布，遇重尾或多峰易发散。

工程上常用一个总代价函数把权衡量化：

骑自行车上高速危险，开越野车在城区油耗爆表。没有绝对最好的算法，只有最匹配需求的算法——选型变成一道优化题。

J=α⋅RMSE+β⋅TcalcJ = \alpha \cdot \mathrm{RMSE} + \beta \cdot T_{\text{calc}}
  1. RMSE均方根误差（Root Mean Square Error），衡量算法估计精度。
    RMSE=1N∑i=1N∥xi−x^i∥2\mathrm{RMSE} = \sqrt{\frac{1}{N}\sum_{i=1}^{N}\|x_i - \hat{x}_i\|^{2}}
  2. $T_{\text{calc}}$单帧计算耗时，衡量实时性。
    Tcalc=Tpredict+TupdateT_{\text{calc}} = T_{\text{predict}} + T_{\text{update}}
  3. $\alpha$、$\beta$业务权重——自动驾驶安全主导选大 $\alpha$，无人机电量有限选大 $\beta$。
    α,β≥0,α+β=1\alpha, \beta \ge 0, \quad \alpha + \beta = 1
  4. 决策规则对候选算法枚举取 $J$ 最小者，就是“性价比之王”。
    Algorithm∗=arg⁡min⁡AJ(A)\text{Algorithm}^* = \arg\min_{A} J(A)

选型决策：按地形配装备

出发前，领队要回答四个问题：路有多弯？雾有多大？队伍走多快？电台多久报一次方位？答案直接决定配哪套装备。对应到工程上就是：非线性强度、噪声分布、实时性要求、算力与状态维数。

选择建议可以写成分支规则：

  • 系统线性且噪声高斯 → 直接用 KF，计算最省，解析最优；
  • 弱非线性、噪声高斯、雅可比易求 → 选 EKF，工程实现最成熟；
  • 中强非线性、雅可比难求或求导昂贵 → 选 UKF，无需导数且精度更高；
  • 强非线性、多峰分布、全局定位或重定位 → 选粒子滤波，鲁棒性最强。

实践中还常用混合策略：导航主循环用 EKF 保证实时性，粒子滤波做全局定位与重定位，UKF 做故障诊断与航向修正。这样各取所长，而不是把宝押在一种算法上。

粒子滤波的算力账也要算清楚，它随粒子数的增长并不是线性的：

选装备不只看地形，还要兼顾钱包和脚程。工程选型的本质是在精度、实时性、资源之间做多目标权衡——不追求“最强”，而追求“刚好够用且不超预算”。

T(N)=c0+c1N+c2Nlog⁡NT(N) = c_0 + c_1 N + c_2 N \log N
  1. 固定开销 $c_0$初始化、地图加载、内存分配等与粒子数无关的耗时。
    T(N)=c0+c1N+c2Nlog⁡NT(N) = c_0 + c_1 N + c_2 N \log N
  2. $c_1 N$对 $N$ 个粒子执行状态转移，每个粒子各算一次非线性函数。
    Tpredict=c1NT_{\text{predict}} = c_1 N
  3. $c_2 N \log N$重采样阶段的排序或累积分布搜索复杂度。
    Tresample=c2Nlog⁡NT_{\text{resample}} = c_2 N \log N
  4. 含义粒子数翻 4 倍，计算开销远不止 4 倍，选择粒子数时必须掂量算力预算。
    lim⁡N→∞T(4N)T(N)=4+4c2log⁡4c1+c2log⁡N+⋯\lim_{N \to \infty} \frac{T(4N)}{T(N)} = 4 + \frac{4c_2 \log 4}{c_1 + c_2\log N + \cdots}

AI 应用场景

自动驾驶车辆定位

阶段：感知与定位（多传感器融合）

解决的问题：传统方案只依赖 GPS，在城市峡谷、隧道中信号丢失后定位瞬间崩溃；融合 GPS、IMU、轮速计与视觉，可在 GPS 失效时靠里程计+视觉持续估计位姿，误差不随信号中断而爆炸。

具体案例：某 L4 级自动驾驶车队在园区测试中，使用 EKF 融合 GPS/RTK 与 IMU 作为主定位，粒子滤波负责停车场内的全局重定位。当车辆驶入高架桥下 GPS 拒止区域，EKF 依靠 IMU 预积分维持 20cm 级定位，粒子滤波则在重新收到 GPS 时快速收敛到新位置，全程无轨迹跳变。

服务机器人室内定位

阶段：感知与定位（SLAM 与蒙特卡洛定位）

解决的问题：室内走廊普遍对称，激光雷达观测存在天然多峰分布（分不清走廊的哪一端）；EKF 假设单峰高斯，容易在对称环境中锁错位置。粒子滤波天然支持多峰分布，适合全局定位与绑架恢复。

具体案例：某商场扫地机器人采用 AMCL（自适应蒙特卡洛定位），本质是粒子滤波：3 万个粒子在环境地图上初始均匀撒开，激光帧匹配后按似然更新权重并重采样。机器人被顾客搬动后，粒子群先散开、再收敛，几秒内自动恢复正确位姿。

无人机姿态与组合导航

阶段：状态估计与导航控制

解决的问题：无人机大角度机动时，姿态动力学呈现强非线性，EKF 的线性化误差会随横滚角增大而显著上升；UKF 不依赖雅可比，能更好地跟踪大姿态角变化，同时计算量仍能跑在嵌入式飞控上。

具体案例：某农业植保无人机在强风环境下做大坡度转向，飞控中的 UKF 融合 IMU、气压计与光流传感器，姿态角估计误差比原 EKF 方案降低约 40%，在 GPS 信号遮挡的果园中仍能保持稳定悬停。

Python 演示

python78 行
import mathimport random random.seed(1)dt, q, r = 0.1, 0.05, 0.6A = 0.9  def truth(N):    xs = [1.0]    for _ in range(N):        xs.append(xs[-1] + A * math.sin(xs[-1]) * dt)    return xs  def ekf(zs):    x, P, es = 0.5, 1.0, []    for z in zs:        F = 1.0 + A * math.cos(x) * dt        x = x + A * math.sin(x) * dt        P = F * F * P + q        K = P / (P + r)        x = x + K * (z - x)        P = (1 - K) * P        es.append(x)    return es  def ukf(zs):    lam, x, P, es = 2.0, 0.5, 1.0, []    for z in zs:        s = math.sqrt((1 + lam) * P)        chi = [x, x + s, x - s]        wm = [lam / (1 + lam), 0.5 / (1 + lam), 0.5 / (1 + lam)]        y = [p + A * math.sin(p) * dt for p in chi]        x = sum(wm[i] * y[i] for i in range(3))        P = q + sum(wm[i] * (y[i] - x) ** 2 for i in range(3))        K = P / (P + r)        x = x + K * (z - x)        P = (1 - K) * P        es.append(x)    return es  def pf(zs, N=500):    parts = [random.gauss(0, 2) for _ in range(N)]    es = []    for z in zs:        for i in range(N):            parts[i] += A * math.sin(parts[i]) * dt + random.gauss(0, math.sqrt(q))        ws = [max(math.exp(-0.5 * (z - p) ** 2 / r), 1e-300) for p in parts]        total = sum(ws)        cs, c = [], 0.0        for w in ws:            c += w / total            cs.append(c)        new = []        for _ in range(N):            u = random.uniform(0, 1)            j = 0            while cs[j] < u:                j += 1            new.append(parts[j])        parts = new        es.append(sum(parts) / N)    return es  def rmse(a, b):    return math.sqrt(sum((x - y) ** 2 for x, y in zip(a, b)) / len(a))  if __name__ == "__main__":    X = truth(100)    Z = [x + random.gauss(0, math.sqrt(r)) for x in X]    print("EKF RMSE: %.3f" % rmse(ekf(Z), X))    print("UKF RMSE: %.3f" % rmse(ukf(Z), X))    print("PF  RMSE: %.3f" % rmse(pf(Z), X))
目的：在同一非线性系统上对比三种滤波算法的估计精度，帮助直观理解“非线性越强，越需要高级近似”。
  1. F = 1.0 + A * math.cos(x) * dt计算运动模型的雅可比，也就是当前点的“切线斜率”。
  2. x = x + A * math.sin(x) * dt让状态直接通过非线性函数推进，这是 EKF 与 KF 的关键差别。
  3. K = P / (P + r)卡尔曼增益，在模型不确定度与观测噪声之间取平衡。
  4. s = math.sqrt((1 + lam) * P)计算 σ 点与均值的距离，UKF 靠它采样。
  5. chi = [x, x + s, x - s]生成 3 个 σ 点：中心点和上下两侧。
  6. y = [p + A * math.sin(p) * dt for p in chi]让每个 σ 点通过真实非线性函数传播。
  7. x = sum(wm[i] * y[i] for i in range(3))对传播后的 σ 点加权求和，得到预测均值。
  8. ws = [max(math.exp(-0.5 * (z - p) ** 2 / r), 1e-300) for p in parts]计算每个粒子的观测似然权重，并设置下限避免除零。
  9. while cs[j] < u: j += 1轮盘赌重采样：按累积权重随机抽取粒子，权重高的粒子被多次复制。
  10. parts = new用重采样后的粒子集替换旧集合，完成“优胜劣汰”。

常见误区

误区“EKF 是 KF 的升级版，肯定更准。只要用得上，我老把 KF 换成 EKF，反正更先进。”

正解EKF 只是在非线性系统中把“线性化误差”当作代价换来的近似解。若系统本身是线性、噪声高斯，KF 才是解析最优的无偏估计；EKF 多余的线性化反而可能引入本不该有的误差。就像本来走直线，你每步还用切线去修正，反而画蛇添足。

为何常见：“扩展（Extended）”字眼容易让人误以为是全面超越，忽略了它只是一阶近似。

误区“PF 用那么多随机样本，怎么都比 EKF 和 UKF 准，所以所有定位任务我都先上 PF。”

正解有限粒子数下，高维状态会出现“维度灾难”——状态维度一高，粒子覆盖指数级稀疏，估计方差可能比 EKF 还大。在系统线性高斯时，KF 已有解析最优解，PF 只会白白烧算力。比喻：撒豆子在小院子里能找到钥匙，撒在一整座山上就稀稀拉拉，什么都找不着。

为何常见：看到 PF 能处理多峰、非高斯的“全能”宣传，忽略了样本数量与状态维度的关系。

误区“我多接几路传感器，数据直接丢进融合算法，结果肯定显著变好。”

正解没有时间同步和空间标定就融合，等于拿三个不同时刻、不同坐标系下的位置硬算平均。误差相互污染后，结果甚至不如单独用最好的那个传感器。比喻：三个人各说各话，还没统一“钟表”和“地图”就开会，吵到最后比一个人时更乱。

为何常见：工程里“堆数据”最容易做到，但数据对齐才是融合质量的前提，初学者最容易忽略这个前置步骤。

误区“UKF 不是能处理非线性吗？那它应该什么分布都能扛，非高斯也没问题。”

正解UKF 虽然通过 σ 点更精确地传播均值和协方差，但最终仍用高斯分布来近似状态分布；面对多峰、重尾分布时依然失真。真正不依赖高斯假设的是粒子滤波。比喻：探子再勤奋，汇报时也只给“平均位置 + 散布椭圆”，画不出两座山头的双峰。

为何常见：“无需雅可比”很容易被误解成“无需任何分布假设”。

误区“重采样能淘汰差粒子，那我每帧都重采样，粒子质量肯定越来越高。”

正解过度重采样会导致粒子多样性丧失——高权重粒子被反复复制，粒子群聚成一团后再也无法探索其他区域，也就失去了处理多峰分布的能力。常用策略是只有当 $N_{eff}$ 低于阈值（如 $N/2$）时才重采样。比喻：每走一步就把豆子全捡起来只留少数几颗的复制品，豆群反而失去多样性，碰到岔路时全体押错方向。

为何常见：看到重采样公式就以为“刷新”是好事，忽略了粒子多样性是粒子滤波的生命线。

面试题

Q1. 多传感器融合的主要价值是什么？

单选题 · easy

  • A.让每个传感器测量精度独立提升
  • B.综合利用冗余与互补信息更可靠← 正确答案
  • C.用高精度传感器替换低精度传感器
  • D.减少传感器数量以降低系统成本

答案：B。多传感器融合的核心是“冗余互补”：冗余用于互相校验，互补用于拼出完整状态。A 中“独立提升”不是融合能保证的；C、D 是降本手段，不是融合目的。就像旅行团整合 GPS、计步器和指南针，得到一个比任何单人都靠谱的答案。

Q2. 卡尔曼滤波最基本的假设是什么？

单选题 · easy

  • A.系统线性且噪声服从高斯分布← 正确答案
  • B.系统非线性且噪声服从均匀分布
  • C.观测数据必须来自至少两个传感器
  • D.状态变量必须是高维连续向量

答案：A。KF 的基石是线性系统 + 高斯噪声：线性保证矩阵运算闭环，高斯保证均值方差足以描述分布。B 是非线性，C 把多传感器混为必要条件（单传感器也能用 KF），D 与维度无关。好比老向导只在“路平、误差呈钟形”的前提下才敢用直线推算。

Q3. EKF 处理非线性系统的方式是什么？

单选题 · mid

  • A.用雅可比矩阵做一阶泰勒线性化← 正确答案
  • B.用大量随机样本直接逼近分布
  • C.用 σ 点传播状态分布的统计量
  • D.用神经网络近似状态转移函数

答案：A。EKF 在当前估计点对非线性函数求雅可比，即用“切线”局部近似曲线，再套用 KF 框架。B 是粒子滤波，C 是 UKF，D 不是传统滤波方法。就像每走一小段路就画一条切线代替弯道。

Q4. UKF 中选择 σ 点的主要目的是什么？

单选题 · mid

  • A.增加观测模型中的测量噪声← 正确答案
  • B.传递状态分布的均值与协方差
  • C.替代真实传感器完成数据采集
  • D.对粒子滤波做重要性重采样
  • E.判断何时需要执行重采样
  • F.确定卡尔曼增益的大小
  • G.计算 UKF 的 σ 点个数
  • H.标定传感器外参矩阵

答案：A。$N_{eff}$ 度量权值退化程度，值越小说明少数粒子垄断了权重，越需要重采样来恢复多样性。B 是 KF/EKF 的事，C 与 UKF 参数相关，D 是标定问题。就像看豆群中“还有几颗豆子真正有发言权”，太少就得重新撒豆。

Q5. 以下哪些场景更适合选择粒子滤波？

多选题 · mid

  • A.全局定位存在多峰概率分布← 正确答案
  • B.系统只有线性与高斯噪声
  • C.强非线性且观测易出现多峰← 正确答案
  • D.算力紧张的低成本嵌入式平台

答案：AC。粒子滤波的强项是多峰、非线性、非高斯场景，A、C 都是典型应用。B 中线性高斯场景用 KF 解析最优；D 中粒子滤波算力开销大，不适合算力紧张的平台。好比只有“地形复杂、岔路多”才值得动用撒豆大法。

Q6. 传感器融合前必须完成的对齐工作是什么？

单选题 · easy

  • A.时间同步与空间坐标系标定← 正确答案
  • B.传感器颜色与外壳风格统一
  • C.提高采样频率与刷新率一致
  • D.所有传感器更换同一品牌型号

答案：A。融合前必须先做时间同步（同一时刻的数据才能比较）和空间标定（统一坐标系）。C 中“刷新率一致”不是必须的，可通过插值对齐；B、D 是物理外观问题，与数据融合无关。就像开会前先统一手表和地图。

Q7. 关于 EKF、UKF、PF 三种算法的说法，正确的有哪些？

多选题 · hard

  • A.EKF 计算成本通常低于粒子滤波← 正确答案
  • B.UKF 对非线性传播的近似优于 EKF← 正确答案
  • C.粒子滤波在粒子数无穷时渐近最优← 正确答案
  • D.系统线性高斯时，PF 一定比 KF 准

答案：ABC。A 正确，EKF 只需传播一个均值和一个协方差，PF 要维护上千粒子；B 正确，UKF 通过 σ 点携带二阶信息；C 正确，这是 PF 的理论保证。D 错误：线性高斯下 KF 是解析最优，有限粒子数的 PF 反而有蒙特卡洛误差。好比地形平缓时别用越野车。

Q8. 粒子滤波中“重采样”的主要目的是什么？

单选题 · mid

  • A.抑制权值退化并集中计算资源← 正确答案
  • B.增加传感器融合的采样频率
  • C.消除系统模型中的过程噪声
  • D.减小粒子数量以降低计算开销

答案：A。经过多轮更新，少数粒子会垄断权重，大量粒子权重趋近于零，这就是权值退化。重采样按权重比例重新抽取粒子，把算力集中到高权重区域。B 与传感器无关，C 中过程噪声模型仍在，D 中粒子数通常不变。就像把好豆子复制、差豆子捡走，而不是减少豆子总数。

Q9. 工程上选择 EKF/UKF/PF 时，通常需要考虑哪些因素？

多选题 · hard

  • A.状态维数与初始化难易程度← 正确答案
  • B.实时性要求与算力内存预算← 正确答案
  • C.传感器外壳材质与外观颜色
  • D.系统非线性和噪声分布特性← 正确答案

答案：ABD。A 状态维数直接影响粒子滤波的维度灾难和 UKF 的 σ 点数量；B 实时性决定能否上 PF；D 是选择 EKF（弱非线性）还是 UKF/PF（强非线性、多峰）的依据。C 是物理外观，与算法选型无关。就像出发前看路况、看脚程、看钱袋，而不是看外套颜色。