3. 状态估计基础：卡尔曼滤波原理、扩展卡尔曼滤波、无迹卡尔曼滤波、粒子滤波简介
做动态目标跟踪，说白了就是在跟不确定性打交道。传感器有噪声，目标运动有随机性，你永远没法拿到100%准确的状态。这时候，状态估计就派上用场了。
我个人习惯把状态估计理解成「猜谜游戏」——你手里有模型（目标大概会怎么动），有观测（传感器看到了什么），然后你猜目标现在到底在哪。卡尔曼滤波就是其中最经典的一套猜法。
3.1 卡尔曼滤波（KF）——线性高斯下的最优解
卡尔曼滤波是状态估计的基石。它假设系统是线性的，噪声是高斯分布的。在这个前提下，它给出了解析解，而且计算量极小。
核心思想就两步：预测 + 更新。
- 预测：用上一时刻的状态，根据运动模型，猜一下当前时刻的状态。同时，算一下这个猜测的不确定性（协方差）变大了多少。
- 更新：拿到传感器观测后，把观测和预测结果做个加权平均。权重由卡尔曼增益决定——谁的噪声小，就更信谁。
我在项目中遇到过一个问题：用卡尔曼滤波跟踪一个匀速直线运动的车辆，效果非常好。但一旦车辆急转弯，滤波结果就开始滞后。为什么？因为模型假设是线性的，急转弯是非线性行为，模型本身就不准了。
卡尔曼滤波五条核心公式（离散时间）：
// 预测
x_pred = F * x_prev + B * u
P_pred = F * P_prev * F^T + Q
// 更新
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_est = x_pred + K * (z - H * x_pred)
P_est = (I - K * H) * P_pred其中：x是状态向量，P是协方差矩阵，F是状态转移矩阵，H是观测矩阵，Q是过程噪声，R是观测噪声，K是卡尔曼增益。
我的经验：调Q和R是门手艺活。Q设大了，滤波结果会跟着观测剧烈抖动；Q设小了，又跟不上目标变化。我一般先根据物理模型估算Q，然后通过仿真微调。R通常可以从传感器手册里拿到，但实际使用中我习惯把R放大20%-30%，因为真实噪声往往比手册标称值大。
3.2 扩展卡尔曼滤波（EKF）——非线性问题的折中方案
现实世界哪有那么多线性系统？目标转弯、传感器测距测角，都是非线性关系。EKF的思路很简单：把非线性函数做一阶泰勒展开，用线性近似代替非线性。
说白了，就是在当前估计点附近，用切线代替曲线。然后套用标准卡尔曼滤波的框架。
嗯，这里要注意：EKF的近似只在局部有效。如果非线性程度很严重，或者初始误差很大，一阶近似就会失效，滤波可能直接发散。
我曾经在一个雷达目标跟踪项目里用过EKF。雷达观测是距离和角度，而状态是笛卡尔坐标系下的位置和速度。观测方程是非线性的，EKF勉强能用。但有一次目标做高机动转弯，EKF的估计误差突然变大，甚至出现了跳变。后来我分析发现，是因为雅可比矩阵在转弯点附近变化剧烈，一阶近似不够用了。
避坑指南：EKF需要计算雅可比矩阵，这玩意儿推导起来容易出错。我建议先用符号计算工具（比如SymPy）验证一下，或者直接用数值雅可比。另外，EKF对初值敏感，初始化不好，后面很难收敛。我习惯用前几帧观测做最小二乘初始化，而不是随便给个值。
3.3 无迹卡尔曼滤波（UKF）——用采样代替线性化
EKF的线性化近似，说白了就是「拍脑袋」——用一条直线代替曲线。UKF换了个思路：我不去近似函数，而是去近似概率分布。
具体怎么做？UKF选取一组确定的采样点（称为Sigma点），这些点能捕获当前状态分布的均值和协方差。然后把每个Sigma点通过非线性函数传播，再根据传播后的点重新计算均值和协方差。
你想想看，这比EKF聪明在哪？EKF只用一个点（均值点）做线性化，而UKF用了多个点来捕获分布的形状。对于非线性程度中等的问题，UKF的精度通常比EKF高一个量级，而且不需要计算雅可比矩阵。
| 对比项 | EKF | UKF |
|---|---|---|
| 核心思想 | 一阶泰勒展开线性化 | 无迹变换，采样传播 |
| 计算复杂度 | O(n³)，需计算雅可比 | O(n³)，无需雅可比 |
| 非线性处理能力 | 弱（一阶近似） | 中等（二阶精度） |
| 实现难度 | 中等（雅可比推导麻烦） | 简单（直接采样） |
| 适用场景 | 弱非线性，计算资源有限 | 中等非线性，精度要求高 |
我的建议：如果系统非线性不严重，EKF够用。如果非线性中等，或者你懒得推导雅可比，直接用UKF。UKF的Sigma点参数α、β、κ一般取α=1、β=2、κ=3-n（n为状态维度），这是针对高斯分布的最优设置。
3.4 粒子滤波（PF）——非线性非高斯下的终极武器
前面说的KF、EKF、UKF，都假设噪声是高斯分布。但现实中有很多情况不满足这个假设——比如多模态分布、重尾分布、或者状态空间本身就不是连续的。
粒子滤波的思路更暴力：用一堆随机粒子（样本）来近似任意分布。每个粒子代表一个可能的状态，粒子的权重代表这个状态的可能性。随着观测到来，权重更新，粒子重采样，最终粒子群收敛到真实状态附近。
说白了，粒子滤波就是用大量计算换灵活性。粒子数越多，近似越精确，但计算量也越大。
我记得在一个室内定位项目里，信号受多径效应影响，观测噪声呈现明显的非高斯特性。卡尔曼滤波家族全部失效，最后是粒子滤波搞定的。当时用了5000个粒子，在嵌入式平台上勉强跑到了10Hz。
避坑指南：粒子滤波有两个常见问题。第一是粒子退化——经过几次更新后，大部分粒子的权重变得极小，只有少数几个粒子有显著权重。解决办法是重采样，但重采样太频繁又会导致粒子多样性丧失。我一般用有效粒子数指标（Neff）来判断是否需要重采样，阈值设为粒子数的一半。第二是计算量，粒子数不能太少，否则精度不够。我建议从1000个粒子开始试，逐步增加直到精度满足要求。
3.5 四种滤波器的选择策略
在实际项目中，怎么选？我总结了一个简单的决策流程：
- 系统是线性的，噪声是高斯的 → 直接用标准卡尔曼滤波（KF），别折腾别的。
- 系统是非线性的，但非线性程度弱，噪声近似高斯 → EKF够用，计算量小。
- 系统非线性中等，或者你不想推导雅可比 → UKF，精度和实现难度的最佳平衡点。
- 噪声明显非高斯，或者分布是多模态的 → 粒子滤波，别无选择。
- 计算资源极度受限 → 优先考虑KF或EKF，粒子滤波慎用。
一句话总结：KF是基础，EKF是妥协，UKF是升级，PF是兜底。没有最好的滤波器，只有最合适的滤波器。
好了，这一章的内容就到这里。四种滤波器各有各的脾气，选对了事半功倍，选错了调试到崩溃。我个人建议初学者先把标准卡尔曼滤波吃透，再逐步接触EKF和UKF。粒子滤波嘛，等遇到非高斯问题再学也不迟。