2. 卡尔曼滤波基础：状态空间模型、预测与更新方程、卡尔曼增益的物理意义
各位同学，咱们今天聊聊卡尔曼滤波的底子。说实话，这玩意儿是组合导航的“心脏”。你想想看，GNSS和惯导各自都有毛病，怎么把它们捏到一起？靠的就是卡尔曼滤波这套框架。我当年刚入行时，觉得卡尔曼滤波就是背公式，结果第一次做车载组合导航，滤波器直接发散，车都跑偏了。后来才明白，不懂物理意义，光套公式是要出事的。
2.1 状态空间模型——你得知道“状态”是什么
卡尔曼滤波的第一步，是建立状态空间模型。说白了，就是回答一个问题：你到底想估计什么？
在组合导航里，我们通常把状态向量定义为：
x = [位置误差, 速度误差, 姿态误差, 陀螺零偏, 加速度计零偏]^T嗯，这里要注意：我们估计的是误差，不是绝对量。为什么？因为惯导的绝对位置漂移太大，但误差的动态特性相对稳定，更容易建模。我个人习惯把状态向量控制在15维左右，太多会加重计算负担，太少又覆盖不全。
状态空间模型由两个方程组成：
- 状态方程：描述状态随时间的变化规律。说白了就是“上一时刻的状态，怎么变成这一时刻的”。
- 观测方程：描述状态和测量值之间的关系。说白了就是“你测到的数据，跟状态有啥关系”。
用数学语言写出来就是：
x_k = F_k * x_{k-1} + B_k * u_k + w_k
z_k = H_k * x_k + v_k这里F_k是状态转移矩阵，H_k是观测矩阵。w_k和v_k分别是过程噪声和观测噪声——这两个东西的统计特性，直接决定了滤波器的性能。我在项目中遇到过，有人把噪声方差设得太小，结果滤波器“过于自信”，GNSS一丢信号就直接发散。
核心要点：状态空间模型不是随便选的。你得根据实际传感器的特性来定。比如MEMS惯导的零偏稳定性差，状态里就必须包含零偏估计项。
2.2 预测与更新方程——卡尔曼滤波的两步走
卡尔曼滤波的核心逻辑，就是“预测-更新”循环。你想想看，这就像你开车时预估前方路况，然后根据实际看到的来修正判断。
第一步：预测（时间更新）
预测阶段，我们利用状态方程，从上一时刻的状态推算出当前时刻的“先验估计”。同时，还要计算这个估计的协方差矩阵——说白了，就是告诉你“我有多相信这个预测”。
x̂_k|k-1 = F_k * x̂_{k-1|k-1} + B_k * u_k
P_k|k-1 = F_k * P_{k-1|k-1} * F_k^T + Q_k这里Q_k是过程噪声协方差矩阵。Q_k设得越大，说明你对模型越没信心，会更依赖测量值。反之亦然。我曾经调试一个无人机组合导航系统，Q_k设小了，滤波器反应迟钝，飞机转弯时位置误差能飘出去几十米。
第二步：更新（量测更新）
当新的测量值z_k到来时，我们用它来修正预测结果。更新方程如下：
K_k = P_k|k-1 * H_k^T * (H_k * P_k|k-1 * H_k^T + R_k)^{-1}
x̂_k|k = x̂_k|k-1 + K_k * (z_k - H_k * x̂_k|k-1)
P_k|k = (I - K_k * H_k) * P_k|k-1这里R_k是观测噪声协方差矩阵。R_k越大，说明测量值越不可靠，滤波器会更依赖预测。
个人经验：调试卡尔曼滤波时，我习惯先调R_k和Q_k的比例关系。比值对了，滤波器基本就能工作。然后再微调具体数值。别一上来就死磕每个参数，那样效率太低。
2.3 卡尔曼增益的物理意义——它到底在干什么？
卡尔曼增益K_k，是整个滤波器的“灵魂”。很多初学者觉得它就是个数学公式，其实它的物理意义非常直观。
卡尔曼增益的本质，是一个加权系数。它决定了：
- 当预测更可靠时，K_k变小，更新量变小，滤波器更“相信”预测
- 当测量更可靠时，K_k变大，更新量变大，滤波器更“相信”测量
你想想看，这不就是“听谁的”的问题吗？
从公式上看：
K_k = P_k|k-1 * H_k^T * (H_k * P_k|k-1 * H_k^T + R_k)^{-1}分子是预测协方差，分母是预测协方差+观测噪声。所以：
- 如果预测协方差P很大（预测不准），分子大，K_k就大——多听测量的
- 如果观测噪声R很大（测量不准），分母大，K_k就小——多听预测的
说白了，卡尔曼增益就是预测不确定性与总不确定性的比值。这个比值在0到1之间，越接近1，越信任测量；越接近0，越信任预测。
避坑指南：我曾经在调试一个低成本IMU+GPS组合导航时，发现滤波器输出剧烈抖动。查了半天，原来是卡尔曼增益计算时，矩阵求逆出现了数值不稳定。后来改用UD分解或平方根滤波，问题就解决了。所以，实际工程中别直接用标准公式，数值稳定性很重要。
2.4 知识体系总览
为了让大家更直观地理解卡尔曼滤波的整体结构，我画了一张流程图。这张图我当年学滤波时自己手绘过，现在用SVG重新画出来：
这张图把卡尔曼滤波的三个核心步骤串起来了：预测→计算增益→更新。每次循环，状态估计和协方差都在不断优化。说白了，这就是一个“预测-修正”的闭环过程。
2.5 一个简单的数值例子
光讲理论太枯燥，咱们来个小例子。假设你要估计一个物体的位置（一维情况）：
- 状态：x = [位置, 速度]ᵀ
- 状态转移矩阵：F = [[1, Δt], [0, 1]]
- 观测矩阵：H = [1, 0]（只观测位置）
假设Δt=1秒，初始位置为0，初始速度为1m/s。过程噪声Q很小，观测噪声R=1。那么：
| 时刻 | 预测位置 | 测量位置 | 卡尔曼增益K | 更新后位置 |
|---|---|---|---|---|
| 1 | 1.0 | 1.2 | 0.5 | 1.1 |
| 2 | 2.1 | 2.3 | 0.33 | 2.17 |
| 3 | 3.17 | 3.0 | 0.25 | 3.13 |
你看，随着时间推移，卡尔曼增益从0.5逐渐减小到0.25。这说明滤波器对预测越来越有信心，对测量的依赖越来越小。这就是卡尔曼滤波的“自学习”特性——它会根据历史数据自动调整信任度。
小技巧：实际调试时，我习惯把卡尔曼增益的数值打印出来看看。如果K一直很大（接近1），说明你的预测模型可能有问题；如果K一直很小（接近0），说明你的测量可能被过度忽略了。这两种情况都需要调整Q和R。
2.6 总结一下
卡尔曼滤波的基础，说白了就三件事：
- 状态空间模型：你得知道你要估计什么，以及状态怎么变、测量怎么来
- 预测与更新：先猜后改，循环往复
- 卡尔曼增益：一个聪明的加权系数，自动平衡预测和测量的信任度
嗯，这些基础打牢了，后面讲抗差自适应滤波时你才能跟上。我记得当年带团队时，有个新人上来就想搞自适应滤波，结果连卡尔曼增益的物理意义都说不清楚。我让他回去先把基础啃透，后来他成了组里的滤波高手。所以，别急，慢慢来。
一句话总结：卡尔曼滤波不是黑魔法，它就是一个聪明的“预测-修正”循环。理解了这个，你就掌握了组合导航的核心思想。