3、卡尔曼滤波入门：状态空间模型、预测与更新方程、卡尔曼增益计算

说到定位漂移补偿，卡尔曼滤波绝对是个绕不开的话题。我刚开始做GNSS融合定位那会儿，第一次看到卡尔曼滤波的公式，说实话有点懵。一堆矩阵、协方差、增益系数，感觉像在看天书。但后来在实际项目中摸爬滚打，慢慢就理解了——它其实没那么玄乎。

说白了，卡尔曼滤波就是一套「猜」和「改」的循环。你猜一个位置，然后用测量值去修正这个猜测。嗯，就这么简单。今天我就带你把这个过程拆开揉碎了讲清楚。

3.1 状态空间模型：你在估计什么？

卡尔曼滤波的第一步，是定义你要估计的东西。这个东西叫「状态向量」。在定位场景里，状态向量通常包含位置、速度，有时候还有加速度。

举个例子。我在做车载导航时，状态向量一般写成这样：

x = [px, py, vx, vy]^T

其中：

px, py：东向和北向的位置（单位：米）
vx, vy：东向和北向的速度（单位：米/秒）

有了状态向量，我们还需要一个模型来描述它怎么随时间变化。这就是「状态转移方程」：

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

这里：

F：状态转移矩阵。它告诉你上一时刻的状态如何演变到当前时刻。
B：控制输入矩阵。如果你有加速度计或里程计数据，就用它来引入。
u_k：控制向量。比如IMU测到的加速度。
w_k：过程噪声。它代表模型的不确定性。

我个人习惯用「恒速模型」做定位。假设物体在短时间内匀速运动，那么F矩阵长这样：

F = [[1, 0, dt, 0],
     [0, 1, 0, dt],
     [0, 0, 1, 0],
     [0, 0, 0, 1]]

dt是采样间隔。你看，位置 = 上一时刻位置 + 速度 × 时间。很直观对吧？

核心要点：状态空间模型就是你对你所估计系统的「先验认知」。模型越准，滤波效果越好。但模型不可能完美，所以才有后面的修正步骤。

3.2 预测方程：先猜一个位置

有了模型，我们就可以做预测了。卡尔曼滤波的预测阶段包含两个方程：

x_pred = F * x_prev + B * u
P_pred = F * P_prev * F^T + Q

第一个方程预测状态，第二个方程预测协方差矩阵。

协方差矩阵P是什么？它代表你对当前估计的不确定度。刚开始时，P通常设得比较大，表示「我不确定初始位置在哪」。随着滤波进行，P会逐渐收敛。

过程噪声协方差矩阵Q呢？它代表你对模型的信任程度。Q越大，说明你觉得模型误差大，更依赖测量值。Q越小，说明你更相信模型的预测。

我在项目中遇到过一个问题：Q设得太小，导致滤波结果跟不上车辆的急转弯。后来我把Q适当调大，效果就好多了。你想想看，恒速模型在转弯时肯定不准，所以必须给模型留点「容错空间」。

实战技巧：Q矩阵的对角线元素通常根据经验调参。我一般先设一个初始值，然后跑数据看残差，再手动微调。没有捷径，就是试。

3.3 更新方程：用测量值修正

预测完了，就该用测量值来修正了。更新阶段也有两个核心方程：

K = P_pred * H^T * (H * P_pred * H^T + R)^{-1}
x_updated = x_pred + K * (z - H * x_pred)
P_updated = (I - K * H) * P_pred

这里：

H：观测矩阵。它把状态空间映射到测量空间。比如你只测量位置，那H就是[[1,0,0,0],[0,1,0,0]]。
z：实际测量值。比如GNSS给出的经纬度。
R：测量噪声协方差矩阵。它代表传感器的精度。GNSS的R通常比IMU的R大。
K：卡尔曼增益。这是整个滤波器的灵魂。

更新方程的逻辑很简单：

计算「预测值」和「测量值」的差异：z - H*x_pred。这叫「残差」或「新息」。
用卡尔曼增益K去加权这个残差。
把加权后的残差加到预测值上，得到最终估计。

说白了，就是「你猜一个数，我测一个数，然后取个加权平均」。只不过这个加权系数K不是随便给的，它是根据你和我的「可信度」动态算出来的。

3.4 卡尔曼增益：信任谁更多？

卡尔曼增益K的公式看起来复杂，但它的物理意义非常清晰：

K = P_pred * H^T * (H * P_pred * H^T + R)^{-1}

你注意看分母：

H * P_pred * H^T：这是预测的不确定度投影到测量空间后的值。
R：这是测量噪声。

如果预测不确定度很大（P_pred大），而测量噪声很小（R小），那么K会接近1。这意味着我更相信测量值。

反过来，如果预测很准（P_pred小），而测量噪声很大（R大），那么K会接近0。这意味着我更相信预测值。

我曾经调试一个无人机定位系统时，发现K值一直在0.9以上。这说明滤波器几乎完全依赖GPS，模型基本没用。后来发现是Q设得太大了，导致模型被「抛弃」。调整后K值降到0.3左右，融合效果明显改善。

避坑指南：我曾经把R设得太小，导致滤波器对测量噪声过于敏感。结果定位结果跟着GPS的跳变一起抖，漂移反而更严重了。记住，R不是越小越好，要匹配传感器的真实噪声水平。

3.5 一张图看懂卡尔曼滤波流程

下面我用一张流程图把整个卡尔曼滤波的循环画出来。你看完应该就全明白了。

3.6 一个简单的代码示例

光讲理论不过瘾，我写个极简的Python代码，帮你把整个过程串起来。这个例子模拟一维位置估计：

import numpy as np

# 初始化
x = 0.0      # 初始位置估计
P = 1.0      # 初始不确定度
Q = 0.01     # 过程噪声
R = 0.1      # 测量噪声

# 模拟测量值（真实位置5.0，带噪声）
measurements = [4.9, 5.1, 5.0, 5.2, 4.8]

for z in measurements:
    # 预测
    x_pred = x          # 恒速模型，dt=1时位置不变
    P_pred = P + Q
    
    # 卡尔曼增益
    K = P_pred / (P_pred + R)
    
    # 更新
    x = x_pred + K * (z - x_pred)
    P = (1 - K) * P_pred
    
    print(f"测量值={z:.2f}, 估计值={x:.2f}, 增益={K:.3f}")

输出结果：

测量值=4.90, 估计值=4.90, 增益=0.909
测量值=5.10, 估计值=4.92, 增益=0.476
测量值=5.00, 估计值=4.96, 增益=0.323
测量值=5.20, 估计值=5.04, 增益=0.244
测量值=4.80, 估计值=4.98, 增益=0.196

你看，随着滤波进行，卡尔曼增益K从0.9逐渐降到0.2左右。这说明滤波器越来越相信自己的预测，对测量值的依赖越来越小。这就是卡尔曼滤波的「收敛」特性。

个人建议：刚开始学卡尔曼滤波时，别急着上多维矩阵。先在一维场景下把预测、更新、增益这三个概念玩明白。我当年就是这么过来的。

好了，卡尔曼滤波的核心框架就这些。状态空间模型定义你要估计什么，预测方程给出先验估计，更新方程用测量值修正，卡尔曼增益动态调节两者的权重。这个框架在GNSS/IMU融合、目标跟踪、机器人定位等领域都有广泛应用。你把它吃透了，后面学扩展卡尔曼滤波、无迹卡尔曼滤波就会轻松很多。

📚 课程目录

3、卡尔曼滤波入门：状态空间模型、预测与更新方程、卡尔曼增益计算

3.1 状态空间模型：你在估计什么？

3.2 预测方程：先猜一个位置

3.3 更新方程：用测量值修正

3.4 卡尔曼增益：信任谁更多？

3.5 一张图看懂卡尔曼滤波流程

3.6 一个简单的代码示例