第2章：卡尔曼滤波入门：状态空间模型、预测与更新、协方差矩阵的意义

各位同学，欢迎来到第二讲。

上一章我们聊了INS的基本原理，知道了陀螺和加速度计是怎么工作的。但有个问题一直悬着——传感器有噪声，积分会漂移，怎么办？

答案就是卡尔曼滤波。

说实话，我当年刚接触卡尔曼滤波时，被那一堆矩阵公式搞得晕头转向。后来在项目里摔了几次跟头，才慢慢摸到门道。今天我就用最直白的方式，把卡尔曼滤波的核心讲清楚。

2.1 状态空间模型：把问题装进盒子里

卡尔曼滤波的第一步，是把你的系统描述成「状态空间模型」。说白了，就是用数学语言告诉滤波器：你现在有什么、下一步会变成什么样。

一个典型的状态空间模型包含两个方程：

• 状态方程：描述系统状态如何随时间演化
• 观测方程：描述传感器测量值与状态的关系

拿INS来说，我们的状态通常包括：位置、速度、姿态角，还有陀螺和加速度计的零偏。写成矩阵形式就是：

x_k = F * x_{k-1} + B * u_k + w_k
z_k = H * x_k + v_k

这里：

• x_k 是k时刻的状态向量
• F 是状态转移矩阵
• u_k 是控制输入（比如加速度计读数）
• w_k 是过程噪声
• z_k 是观测值（比如GPS位置）
• H 是观测矩阵
• v_k 是观测噪声

我在做车载组合导航时，遇到过一个问题：状态向量里到底该包含哪些量？我的经验是——能估计的尽量估计，但不要贪多。比如陀螺零偏一定要加，但加速度计刻度因子如果变化不大，可以先忽略。否则状态维数太高，滤波器容易发散。

2.2 预测与更新：两步走，步步为营

卡尔曼滤波的精髓就两个字：预测和更新。像个循环，每来一个新数据就执行一次。

2.2.1 预测步（Time Update）

预测步做什么？用上一时刻的最优估计，推算当前时刻的状态和协方差。

x_pred = F * x_est + B * u
P_pred = F * P_est * F^T + Q

这里：

• x_pred 是预测状态
• P_pred 是预测协方差
• Q 是过程噪声协方差矩阵

嗯，这里要注意：Q 矩阵的设置非常关键。它代表你对模型的信任程度。Q设得太大，滤波器会过于相信测量值，噪声大；Q设得太小，滤波器反应迟钝，跟不上真实变化。

2.2.2 更新步（Measurement Update）

当新的测量值到来时，我们用预测值和测量值做一个加权平均，得到最优估计。

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 是卡尔曼增益
• R 是观测噪声协方差矩阵
• z - H*x_pred 是残差（也叫新息）

卡尔曼增益K决定了预测和测量谁更可信。K大，说明测量值可靠，更新幅度大；K小，说明预测值可靠，更新幅度小。

2.3 协方差矩阵的意义：不确定性的量化

协方差矩阵是卡尔曼滤波的灵魂。它量化了我们对状态估计的不确定性。

对角线元素是方差，代表每个状态量的不确定度。非对角线元素是协方差，代表不同状态量之间的相关性。

举个例子：

P = [ σ_xx^2   σ_xy^2 ]
    [ σ_yx^2   σ_yy^2 ]

如果σ_xy很大，说明x和y的估计误差高度相关。这在INS中很常见——比如位置误差和速度误差就是强相关的。

我习惯在调试时把协方差矩阵的对角线元素画出来。如果某个状态的方差突然增大，说明那个时刻的观测质量可能有问题。如果方差一直不收敛，那就要检查模型或者参数了。

你想想看，如果没有协方差矩阵，我们怎么知道当前估计有多可靠？卡尔曼滤波之所以强大，就是因为它不仅给出估计值，还给出估计的置信度。

2.4 核心逻辑框架图

下面我用一张SVG图来总结卡尔曼滤波的完整流程：

这张图把卡尔曼滤波的闭环流程展示得很清楚。从初始状态出发，先预测，再更新，输出最优估计，然后进入下一时刻。周而复始，这就是卡尔曼滤波的「递推」本质。

2.5 本章小结

今天我们讲了三个核心概念：

• 状态空间模型：把INS问题抽象成数学形式
• 预测与更新：两步走的递推框架
• 协方差矩阵：不确定性的量化工具

说实话，这些概念光看公式是不够的。我建议你动手写一个一维的卡尔曼滤波程序，比如估计一个匀速运动物体的位置。跑起来，看看协方差怎么变化，看看增益怎么调整。只有亲手调过参数，才能真正理解卡尔曼滤波的脾气。

---

公众号:蓝海资料掘金营，微信deep3321