4、卡尔曼滤波基础：状态空间模型、线性卡尔曼滤波（KF）推导、扩展卡尔曼滤波（EKF）原理
各位同学好，我是你们这门课的主讲。今天咱们来啃一块硬骨头——卡尔曼滤波。说实话，这玩意儿是IMU融合定位的基石，也是很多同学觉得最头疼的部分。我当年刚入行时，看了一周的理论推导，脑子里还是一团浆糊。后来在项目里摔了几次跟头，才真正理解它到底在干什么。
说白了，卡尔曼滤波就是一个“猜”的过程。你猜当前状态，然后拿测量值来修正。猜得不准没关系，它会告诉你猜得有多不准，然后自动调整。嗯，这就是它的核心思想。
4.1 状态空间模型——你得先知道“状态”是什么
在讲滤波之前，我们得先建立数学模型。我个人习惯把系统描述成两个方程：
- 状态方程：描述系统怎么演化。比如IMU的角速度积分出姿态。
- 观测方程：描述传感器怎么测量。比如GPS给出位置。
写成数学形式就是：
x_k = A * x_{k-1} + B * u_k + w_k
z_k = H * x_k + v_k这里：
x_k是k时刻的状态向量（位置、速度、姿态角等）A是状态转移矩阵u_k是控制输入（比如IMU的加速度）w_k是过程噪声，服从高斯分布 N(0, Q)z_k是观测值H是观测矩阵v_k是观测噪声，服从高斯分布 N(0, R)
关键点：Q和R这两个协方差矩阵，是卡尔曼滤波的“灵魂”。Q越大，说明你越不相信模型预测；R越大，说明你越不相信传感器测量。我在项目中调参时，经常在这两个矩阵上花大量时间。
你想想看，如果Q设得特别小，模型预测就特别“固执”，测量值很难把它拉回来。反过来，如果R设得特别小，测量值就主导一切，噪声会被放大。所以，调Q和R是个手艺活。
4.2 线性卡尔曼滤波（KF）推导——五步走，步步惊心
线性卡尔曼滤波假设系统是线性的，噪声是高斯白噪声。推导过程其实不复杂，就五步。我建议你把这五步背下来，面试时经常考。
4.2.1 预测阶段（两步）
- 状态预测：
x_pred = A * x_est + B * u - 协方差预测：
P_pred = A * P_est * A^T + Q
这里P是状态协方差矩阵，它衡量的是你对当前估计的不确定度。P越大，说明你越不确定。我记得第一次做IMU/GPS融合时，P矩阵初始化设得不对，导致滤波发散，车子明明停着，估计的速度却在乱跳。后来才发现是P_0设得太大了。
4.2.2 更新阶段（三步）
- 卡尔曼增益计算：
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
我的小技巧：卡尔曼增益K可以理解为“信任比例”。K接近1时，你更相信测量；K接近0时，你更相信预测。实际调试时，我会把K的值打印出来看看，如果K一直很大，说明模型预测可能有问题。
为什么会这样？因为K = P_pred * H^T * (H * P_pred * H^T + R)^{-1}。当R很小（测量噪声小）时，分母变小，K变大，测量值权重增加。当P_pred很大（预测不确定）时，分子变大，K也变大。逻辑上完全自洽。
4.3 扩展卡尔曼滤波（EKF）原理——非线性怎么办？
现实世界哪有那么多线性系统？IMU的旋转、车辆的转弯，都是非线性的。线性卡尔曼滤波搞不定，怎么办？
EKF的思路很简单：线性化。说白了，就是在当前估计点附近，用泰勒展开把非线性函数近似成线性函数。然后套用标准KF的框架。
状态方程和观测方程变成：
x_k = f(x_{k-1}, u_k) + w_k
z_k = h(x_k) + v_k这里f和h都是非线性函数。我们需要计算它们的雅可比矩阵：
F = ∂f/∂x在 x_{k-1} 处求导H = ∂h/∂x在 x_pred 处求导
然后用F代替A，用H代替H，剩下的步骤和线性KF一模一样。
注意：EKF的线性化是在当前估计点做的，如果系统非线性很强，或者初始估计偏差很大，线性化误差会累积，导致滤波发散。我曾经在车辆大角度转弯时遇到过这个问题，EKF估计的姿态角直接飞了。后来改用UKF（无迹卡尔曼滤波）才解决。
EKF的流程总结如下：
| 步骤 | 线性KF | EKF |
|---|---|---|
| 状态预测 | x_pred = A * x_est | x_pred = f(x_est, u) |
| 协方差预测 | P_pred = A * P_est * A^T + Q | P_pred = F * P_est * F^T + Q |
| 卡尔曼增益 | K = P_pred * H^T * (H * P_pred * H^T + R)^{-1} | K = P_pred * H^T * (H * P_pred * H^T + R)^{-1} |
| 状态更新 | x_est = x_pred + K * (z - H * x_pred) | x_est = x_pred + K * (z - h(x_pred)) |
| 协方差更新 | P_est = (I - K * H) * P_pred | P_est = (I - K * H) * P_pred |
你看，除了预测部分和观测残差的计算，其他都一样。所以我说，理解了线性KF，EKF就是换个马甲。
4.4 本章知识体系总览
为了让你更直观地理解本章的知识结构，我画了一张图：
这张图把KF和EKF的关系梳理得很清楚。左边是线性KF，右边是EKF。你看，EKF就是在KF的基础上，把线性模型换成了非线性模型，然后加了一步线性化。其他都一样。
避坑指南：我曾经在调试EKF时，雅可比矩阵算错了一个符号，结果滤波结果完全不对。后来我养成了一个习惯——每次算完雅可比，都用数值微分验证一下。具体做法是：给状态加一个小扰动，看输出变化，和解析雅可比对比。这个小技巧帮我省了不少时间。
好了，这一章的内容就到这里。卡尔曼滤波是IMU融合定位的核心工具，理解它的原理和推导过程，对你后续学习IMU/GPS组合导航、高精地图匹配会有很大帮助。记住那五步，记住Q和R的意义，记住EKF的线性化思想。这些概念会一直陪伴你的职业生涯。
可可精品资料库