第二章：经典跟踪滤波器——卡尔曼滤波原理、扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）、粒子滤波入门
各位同学，咱们今天聊聊跟踪滤波器的“四大金刚”。
做雷达跟踪，说白了就是跟噪声斗智斗勇。目标明明在动，你测回来的数据却像喝醉了酒——东倒西歪。怎么办？滤波。
我个人习惯把滤波器比作“数据按摩师”。它把粗糙的测量值，揉捏成平滑的轨迹。今天这四种滤波器，就是按摩界的不同流派。咱们一个一个看。
2.1 卡尔曼滤波：一切的基础
卡尔曼滤波，简称KF。它是所有跟踪算法的老祖宗。1960年提出，到现在60多年了，依然活跃在一线。
它的核心思想就五个字：预测+更新。
核心公式（离散线性系统）：
预测步：
x̂(k|k-1) = F·x̂(k-1|k-1) + B·u(k)
P(k|k-1) = F·P(k-1|k-1)·Fᵀ + Q
更新步：
K(k) = P(k|k-1)·Hᵀ·(H·P(k|k-1)·Hᵀ + R)⁻¹
x̂(k|k) = x̂(k|k-1) + K(k)·(z(k) - H·x̂(k|k-1))
P(k|k) = (I - K(k)·H)·P(k|k-1)看着公式多，其实就两件事：先猜目标下一步到哪，再用测量值修正这个猜测。
我在项目中遇到过一个问题：Q矩阵（过程噪声）设得太小，滤波器“太自信”，结果目标一机动就丢了。后来我学乖了，Q矩阵宁可设大一点，让滤波器保持“谦虚”。
避坑指南：
我曾经在调试某型雷达时，发现滤波发散。查了两天，最后发现是P矩阵初始化设成了零矩阵。记住：P(0)不能为零，否则卡尔曼增益永远为零，滤波器根本不工作。
2.2 扩展卡尔曼滤波（EKF）：对付非线性
现实世界哪有那么多线性系统？目标做圆周运动，雷达测距测角，全是非线性。这时候KF就歇菜了。
EKF的思路很粗暴：把非线性函数线性化。怎么线性化？求导，用泰勒展开取一阶项。
EKF核心改动：
预测步：
x̂(k|k-1) = f(x̂(k-1|k-1), u(k)) // 非线性状态转移
P(k|k-1) = F_j·P(k-1|k-1)·F_jᵀ + Q // F_j是f的雅可比矩阵
更新步：
K(k) = P(k|k-1)·H_jᵀ·(H_j·P(k|k-1)·H_jᵀ + R)⁻¹ // H_j是h的雅可比矩阵
x̂(k|k) = x̂(k|k-1) + K(k)·(z(k) - h(x̂(k|k-1)))
P(k|k) = (I - K(k)·H_j)·P(k|k-1)说白了，就是把非线性函数在估计点附近“掰直”，然后用KF那一套。
你想想看，这样做有什么问题？对，近似有误差。如果非线性很强，一阶近似就不够用了。我见过有人用EKF做纯角度跟踪，目标稍微远一点，滤波就发疯。为什么？因为角度测量的非线性太强，雅可比矩阵算出来都是病态的。
注意：EKF要求系统模型可微。如果你的模型有跳变、有死区，EKF会很难受。我曾经在项目中遇到一个带饱和特性的执行器，EKF直接发散。后来换了UKF才搞定。
2.3 无迹卡尔曼滤波（UKF）：不线性化，直接采样
EKF的线性化近似，说白了有点“糙”。UKF换了个思路：我不近似函数，我近似概率分布。
怎么近似？用一组确定的采样点（Sigma点）来代表状态分布。把这些点通过非线性函数传过去，再统计输出点的均值和协方差。
UKF核心步骤：
1. 生成Sigma点（2n+1个点，n是状态维数）
χ₀ = x̂
χᵢ = x̂ + (√((n+λ)P))ᵢ, i=1..n
χᵢ = x̂ - (√((n+λ)P))ᵢ₋ₙ, i=n+1..2n
2. 通过非线性函数传播
γᵢ = f(χᵢ)
3. 计算预测均值和协方差
x̂⁻ = ΣWᵢᵐ·γᵢ
P⁻ = ΣWᵢᶜ·(γᵢ - x̂⁻)(γᵢ - x̂⁻)ᵀ + Q
4. 更新步类似，用测量函数再传一次UKF的好处是：不需要算雅可比矩阵。对于强非线性系统，UKF的精度通常比EKF高一个数量级。
我记得有一次做无人机跟踪，目标做大过载机动。EKF跟丢了三次，换成UKF后稳稳的。为什么？因为UKF能捕捉到非线性变换后的真实均值和协方差，而EKF只是线性近似。
个人经验：UKF的计算量大约是EKF的2-3倍。如果你的系统非线性不强，或者计算资源紧张，EKF仍然是好选择。别盲目追求“高级”。
2.4 粒子滤波（PF）：终极武器
前面三种滤波器，都假设噪声是高斯分布。如果噪声是非高斯的？比如多模态分布、重尾分布？这时候粒子滤波上场。
粒子滤波的思想更直接：用一堆随机粒子来近似后验概率。每个粒子代表一个可能的状态，粒子越密集的地方，概率越大。
粒子滤波核心流程：
1. 初始化：生成N个粒子，权重均为1/N
2. 预测：每个粒子按状态方程传播
3. 更新：根据测量值计算每个粒子的权重
4. 重采样：去掉权重小的粒子，复制权重大的粒子
5. 估计：加权平均得到状态估计说白了，粒子滤波就是“广撒网，重点捕捞”。粒子数越多，精度越高，但计算量也越大。
我在项目中用过粒子滤波做多目标跟踪。场景是：两个目标交叉飞行，雷达回波混在一起。卡尔曼滤波直接懵了，因为后验分布是双峰的。粒子滤波却能同时跟踪两个峰，效果很好。
注意：粒子滤波的计算量很大。N=1000的粒子滤波，计算量大约是UKF的100倍。实时系统慎用。我曾经在嵌入式平台上试过，帧率直接掉到1Hz。后来优化到N=200，勉强能用。
2.5 四种滤波器对比
| 滤波器 | 适用场景 | 优点 | 缺点 | 计算量 |
|---|---|---|---|---|
| KF | 线性高斯系统 | 简单、快速、最优 | 只适用于线性 | O(n³) |
| EKF | 弱非线性高斯系统 | 实现简单，应用广泛 | 线性化误差，需算雅可比 | O(n³) |
| UKF | 强非线性高斯系统 | 精度高，无需雅可比 | 计算量稍大 | O(n³) |
| PF | 非线性非高斯系统 | 适用任意分布 | 计算量大，粒子退化 | O(N·n²) |
2.6 知识体系总览
下面这张图，是我自己画的四种滤波器的关系图。你一看就明白：
2.7 实战中的选择策略
说了这么多理论，到底怎么选？我给大家一个实战口诀：
- 线性系统、计算资源紧张 → 无脑KF
- 弱非线性、模型可微 → EKF，实现简单
- 强非线性、高斯噪声 → UKF，精度优先
- 非高斯噪声、多模态分布 → PF，但要做好计算量评估
我个人习惯是：先用KF试试，不行就上UKF。EKF我反而用得少，因为算雅可比矩阵太容易出错。有一次我算错了一个偏导数，整个团队排查了三天。从那以后，能用UKF的地方我绝不用EKF。
避坑指南：
我曾经在项目中遇到一个情况：UKF和EKF结果差不多，但UKF计算量大了两倍。后来发现，系统的非线性其实很弱，EKF完全够用。所以，别迷信“高级算法”。先分析你的系统非线性程度，再选滤波器。
好了，四种滤波器的核心内容就这些。记住：没有最好的滤波器，只有最合适的滤波器。下一章咱们聊聊多目标跟踪中的数据关联问题——那才是真正的“坑”。
可可精品资料库