3. 离散卡尔曼滤波：标准五公式推导、噪声协方差调参、初值设定
各位同学，今天咱们来啃一块硬骨头——离散卡尔曼滤波的标准五公式。
说实话，我刚入行那会儿，看到这五个公式就头大。什么预测、更新，什么协方差传递，感觉像天书。后来在项目里摔打了几次，才慢慢品出味道来。今天我就把这五公式掰开了揉碎了讲给你听。
3.1 卡尔曼滤波在干什么？
先问个问题：你有一个IMU，测角速度、加速度，但噪声大。你还有一个GPS，测位置、速度，但更新慢。怎么把两者结合起来？
卡尔曼滤波干的就是这件事——用IMU做短时间的高频预测，用GPS做低频的修正。说白了，就是「取长补短」。
我习惯把卡尔曼滤波想象成一个「带反馈的闭环系统」：
- 预测步：用IMU数据往前推状态
- 更新步：用GPS数据把状态拉回来
嗯，就这么简单。但公式里藏着不少细节。
核心思想：卡尔曼滤波不是「消除噪声」，而是「在噪声中估计出最可能的状态」。
3.2 标准五公式推导
先给出离散系统的状态方程和观测方程：
x_k = F * x_{k-1} + B * u_k + w_k
z_k = H * x_k + v_k其中：
- x_k：k时刻的状态向量（比如位置、速度、姿态）
- F：状态转移矩阵（IMU的积分模型）
- B：控制输入矩阵
- u_k：控制输入（比如加速度计读数）
- w_k：过程噪声，服从 N(0, Q)
- z_k：观测向量（比如GPS位置）
- H：观测矩阵
- v_k：观测噪声，服从 N(0, R)
好，五公式来了。我按「预测-更新」的顺序给你列出来：
预测步（时间更新）
公式1：状态预测
x̂_k|k-1 = F * x̂_{k-1|k-1} + B * u_k这个公式在干什么？用上一时刻的最优估计，加上IMU的测量值，推算出当前时刻的「先验估计」。说白了就是「猜」。
公式2：协方差预测
P_k|k-1 = F * P_{k-1|k-1} * F^T + Q这个公式在估计「猜的准不准」。P是状态协方差矩阵，代表不确定性。Q是过程噪声协方差，代表IMU的噪声水平。
我的经验：Q矩阵是调参的关键。Q设大了，滤波器会「相信」观测更多；Q设小了，滤波器会「相信」IMU更多。我在做车载组合导航时，Q矩阵调了整整两周才找到感觉。
更新步（量测更新）
公式3：卡尔曼增益计算
K_k = P_k|k-1 * H^T * (H * P_k|k-1 * H^T + R)^{-1}卡尔曼增益K，是滤波器的「灵魂」。它决定了：
- 当观测噪声R很大时，K变小，滤波器更相信预测
- 当预测协方差P很大时，K变大，滤波器更相信观测
公式4：状态更新
x̂_k|k = x̂_k|k-1 + K_k * (z_k - H * x̂_k|k-1)这就是「修正」步骤。括号里的 (z_k - H * x̂_k|k-1) 叫「新息」或「残差」——观测值和预测值的差异。乘以K后，加到预测状态上。
公式5：协方差更新
P_k|k = (I - K_k * H) * P_k|k-1更新完状态后，不确定性降低了。这个公式就是「降低不确定性」的数学表达。
注意：公式5有几种等价形式，比如 Joseph 形式。我建议初学者先用标准形式，等理解了再换。我曾经在代码里用错了形式，结果滤波器发散，查了三天bug。
3.3 噪声协方差调参
调参是卡尔曼滤波的「玄学」部分。我见过很多工程师，公式背得滚瓜烂熟，一调参就抓瞎。
先说说Q和R的物理意义：
| 参数 | 物理意义 | 调大效果 | 调小效果 |
|---|---|---|---|
| Q（过程噪声） | IMU的噪声水平、模型误差 | 滤波器响应快，但噪声大 | 滤波器平滑，但响应慢 |
| R（观测噪声） | GPS等传感器的噪声水平 | 滤波器更相信IMU | 滤波器更相信GPS |
调参的「黄金法则」：
- 先定R，再调Q：R可以从传感器手册里查到，或者做静态实验统计。Q只能靠经验。
- Q矩阵的对角线元素：对应各状态的过程噪声。比如位置的过程噪声可以设小一点，速度的设大一点。
- 用残差检验：如果残差序列均值不为零，说明模型有问题。如果残差方差太大，说明Q或R设小了。
避坑指南：我曾经在无人机项目里，把Q设得特别小，结果滤波器完全不相信GPS，飞机飘了100米才反应过来。后来我把Q放大了一个数量级，效果立竿见影。
3.4 初值设定
初值设定是很多人忽略的环节。其实初值设不好，滤波器可能直接发散。
需要设定的初值有两个：
1. 状态初值 x̂₀
- 位置：可以用GPS的第一帧数据
- 速度：可以用GPS的差分，或者设为0（如果静止启动）
- 姿态：可以用加速度计和磁力计计算初始姿态
2. 协方差初值 P₀
- P₀代表对初始状态的信任程度
- 如果初始状态很准（比如GPS固定解），P₀可以设小一点
- 如果初始状态不准（比如刚开机），P₀要设大一点
经验值：我一般把P₀设成对角矩阵，对角线元素取状态量可能误差的平方。比如位置误差10米，P₀(1,1)=100。这样滤波器会快速收敛。
嗯，这里要注意：P₀设得太小，滤波器会「固执己见」，很难收敛到真值。设得太大，初始阶段会有较大波动，但很快会收敛。
3.5 知识体系总览
下面这张图，是我自己画的卡尔曼滤波知识体系。你看一眼，就能把今天讲的内容串起来：
这张图把今天的内容都串起来了。左边是预测步，右边是更新步，下面是调参和初值设定。你把这个图记在脑子里，卡尔曼滤波的框架就清晰了。
3.6 一个小例子
最后，我给你一个简单的代码框架，方便你理解五公式的调用顺序：
# 初始化
x = x0 # 状态初值
P = P0 # 协方差初值
# 主循环
for each time step k:
# 预测步
x = F @ x + B @ u_k
P = F @ P @ F.T + Q
# 如果有观测
if GPS_available:
# 更新步
K = P @ H.T @ inv(H @ P @ H.T + R)
x = x + K @ (z_k - H @ x)
P = (I - K @ H) @ P
# 输出结果
output(x)你看，代码就这么几行。但每一行背后，都是我今天讲的那些「门道」。
最后一句：卡尔曼滤波不难，难的是「理解它为什么这么设计」。你把这个五公式的物理意义吃透了，后面学扩展卡尔曼滤波、无迹卡尔曼滤波，都会轻松很多。
公众号:蓝海资料掘金营，微信deep3321