3. 卡尔曼滤波基础：状态空间模型、预测与更新方程、协方差矩阵的物理意义
卡尔曼滤波，说白了就是一套「猜得准」的数学框架。你给它一个不太准的测量值，再给它一个不太准的预测模型，它就能给你一个比两者都准的估计值。
我刚开始做组合导航时，总觉得卡尔曼滤波是个黑盒子。调了几个月的参数，才慢慢摸清楚它里面到底在干什么。今天咱们就把这个盒子拆开，看看里面到底装了啥。
3.1 状态空间模型：你在估计什么？
任何卡尔曼滤波的第一步，都是定义「状态」。状态就是你想要估计的那些量。
在导航里，状态通常包括：
- 位置：纬度、经度、高度
- 速度：东向、北向、天向速度
- 姿态：航向角、俯仰角、横滚角
- 偏差：陀螺零偏、加速度计零偏
这些状态会随时间变化。我们用状态方程来描述这种变化：
x(k) = F * x(k-1) + B * u(k) + w(k)其中：
- x(k)：k时刻的状态向量
- F：状态转移矩阵，描述状态如何从上一时刻演化过来
- B：控制输入矩阵
- u(k)：控制输入（比如加速度计读数）
- w(k)：过程噪声，代表模型的不确定性
然后我们还有观测方程：
z(k) = H * x(k) + v(k)- z(k)：观测值（比如GPS的位置）
- H：观测矩阵，把状态映射到观测空间
- v(k)：观测噪声
关键理解：状态空间模型就是把「真实世界」抽象成一组数学方程。你不需要知道所有细节，只需要知道状态怎么变、观测怎么来。
3.2 预测与更新：两步走的核心逻辑
卡尔曼滤波就两个步骤，循环往复：预测和更新。
3.2.1 预测步（Time Update）
预测步就是根据上一时刻的状态，猜一下当前时刻的状态会是什么样。
// 状态预测
x_pred = F * x_est + B * u
// 协方差预测
P_pred = F * P_est * F^T + Q这里有个新东西——Q矩阵，也就是过程噪声协方差矩阵。它代表你对模型的信任程度。Q越大，说明你觉得模型越不准，后面就会更依赖观测值。
我在项目中遇到过一个问题：Q设得太小，滤波器对模型过于自信，结果GPS信号一丢，位置就飘得离谱。后来把Q调大了一些，虽然单点精度略有下降，但整体鲁棒性好了很多。
3.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这里又出现一个新矩阵——R矩阵，观测噪声协方差矩阵。它代表你对传感器的信任程度。R越小，说明传感器越准，卡尔曼增益就会越大，修正力度也越大。
调参小技巧：如果你发现滤波结果抖动厉害，多半是R设得太小了。如果结果反应迟钝、跟不上真实变化，多半是Q设得太小了。我一般先固定R，再调Q，这样问题容易定位。
3.3 协方差矩阵的物理意义：你到底有多不确定？
协方差矩阵P，是卡尔曼滤波里最容易被忽视、却又最重要的东西。
它的物理意义很简单：代表你对当前估计的不确定度。
- 对角线元素：每个状态变量的方差。比如P[0][0]大，说明你对位置估计没信心。
- 非对角线元素：不同状态之间的相关性。比如位置和速度的协方差为负，说明位置偏大时速度可能偏小。
我举个例子你就明白了。假设你在做车辆导航：
| 状态 | 初始方差 | 物理含义 |
|---|---|---|
| 位置 (m) | 100 | 初始位置误差约10米 |
| 速度 (m/s) | 1 | 初始速度误差约1米/秒 |
| 陀螺零偏 (°/h) | 10 | 初始零偏不确定度约3°/h |
随着滤波进行，P矩阵会自适应变化：
- 有GPS时，位置方差迅速减小
- 进入隧道后，位置方差逐渐增大
- 重新收到GPS信号，方差又降下来
这就是卡尔曼滤波的优雅之处——它知道自己什么时候靠谱，什么时候不靠谱。
我曾经踩过的坑：初始化P矩阵时设得太小，比如位置方差设成0.01。结果滤波器觉得自己一开始就特别准，后面GPS数据进来也不怎么更新，整个滤波就「僵」住了。后来我养成了习惯：初始P设大一些，让滤波器先「谦虚」一点，慢慢收敛。
3.4 卡尔曼滤波完整流程（SVG流程图）
下面这张图把整个流程串起来了。我建议你多看几遍，直到能在脑子里「跑」起来。
3.5 调参实战：Q和R的物理直觉
说了这么多理论，咱们来点实际的。Q和R怎么调？
我的经验是：先理解物理意义，再动手调数值。
3.5.1 Q矩阵（过程噪声）
Q代表你对模型的信任程度。模型越不准，Q越大。
- IMU质量差：陀螺零偏不稳定，Q要设大一些
- 运动剧烈：车辆急转弯、急加速，模型跟不上，Q也要大
- 低速平稳：模型比较准，Q可以小一些
3.5.2 R矩阵（观测噪声）
R代表你对传感器的信任程度。传感器越准，R越小。
- RTK-GPS：厘米级精度，R可以设到0.01
- 单点GPS：米级精度，R设到1~10
- 手机GPS：10米级精度，R可能要设到100
一个实用的调参方法：先根据传感器手册的噪声指标算一个初始R，然后跑一段数据看残差（innovation）。如果残差均值不为零，说明模型有偏；如果残差方差远大于R，说明R设小了。我一般会写个小脚本，自动统计残差统计特性，然后反推R的合理值。
3.6 本章小结
卡尔曼滤波的核心就三件事：
- 状态空间模型：定义你要估计什么，以及它怎么变
- 预测与更新：两步循环，不断修正
- 协方差矩阵：告诉你每个估计有多靠谱
你想想看，这三个东西搞明白了，卡尔曼滤波就不再是黑盒子了。后面咱们讲组合导航的时候，你会发现所有的高级技巧，都是在这三个基础上搭起来的。