3. 线性卡尔曼滤波（KF）原理：状态空间模型、预测与更新方程、卡尔曼增益推导

各位同学，今天我们来啃一块硬骨头——线性卡尔曼滤波的核心原理。说实话，我刚入行那会儿，看到一堆矩阵公式就头大。但后来在项目中调了无数次滤波器之后，才真正理解这些公式背后的物理意义。说白了，卡尔曼滤波就是一套“预测-修正”的循环，跟咱们日常做决策的逻辑一模一样。

3.1 状态空间模型——你得先知道系统长啥样

要玩卡尔曼滤波，第一步就是把实际问题抽象成数学模型。我个人习惯用“状态空间”来描述系统。什么是状态？就是能完全描述系统当前状况的一组最小变量。

举个例子，我在做无人机导航时，状态向量通常包含：

  • 位置（x, y, z）
  • 速度（vx, vy, vz）
  • 姿态角（roll, pitch, yaw）

状态空间模型由两个方程组成：

状态方程（描述系统怎么演化）：

x(k) = A * x(k-1) + B * u(k-1) + w(k-1)

观测方程（描述传感器怎么测量）：

z(k) = H * x(k) + v(k)

这里：

  • x(k) 是k时刻的状态向量
  • A 是状态转移矩阵——说白了就是“上一时刻的状态怎么变成这一时刻的”
  • B 是控制输入矩阵，u是控制量（比如你给电机的油门）
  • w 是过程噪声，代表模型的不确定性
  • z(k) 是观测值，H是观测矩阵，v是观测噪声

重要提醒：过程噪声w和观测噪声v，我们都假设它们是高斯白噪声，均值是0，协方差分别是Q和R。这个假设是卡尔曼滤波能工作的前提。

3.2 预测与更新——两步走，稳得很

卡尔曼滤波的核心就两个步骤：预测（Predict）和更新（Update）。我当年在调试车载组合导航时，就是靠这两步反复迭代，才把定位误差从米级压到了厘米级。

3.2.1 预测步骤（时间更新）

预测就是根据上一时刻的状态，猜一下当前时刻的状态。公式如下：

// 状态预测
x_pred = A * x_est + B * u

// 协方差预测
P_pred = A * P_est * A^T + Q

这里：

  • x_pred 是预测的状态
  • P_pred 是预测的误差协方差——它代表你对预测结果的信心
  • Q 是过程噪声协方差，越大说明模型越不可靠

嗯，这里要注意：P_pred 在每次预测后都会变大，因为模型的不确定性在累积。你想想看，如果你只靠模型猜，时间越长，误差肯定越大，对吧？

3.2.2 更新步骤（测量更新）

更新就是拿传感器的测量值来修正预测值。公式如下：

// 卡尔曼增益
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 是卡尔曼增益——它是整个滤波器的灵魂
  • z - H*x_pred 叫“新息”（innovation），就是实际测量和预测测量之间的差异
  • R 是观测噪声协方差，越大说明传感器越不可靠

我的经验：调Q和R矩阵是门手艺活。Q设太大，滤波器会过度相信测量值，噪声大；Q设太小，滤波器反应迟钝。我曾经在一个惯性导航项目里，光调Q就花了两周时间。

3.3 卡尔曼增益推导——为什么它这么重要？

卡尔曼增益K，说白了就是一个“权重分配器”。它决定了你是更相信模型预测，还是更相信传感器测量。

我们来看K的表达式：

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

这个公式怎么来的？我简单说下思路：

  1. 我们希望估计误差的方差最小——这是最优估计的准则
  2. 把x_est = x_pred + K*(z - H*x_pred)代入误差协方差公式
  3. 对K求导，令导数为0，解出K

推导结果就是上面那个公式。你仔细看：

  • 如果R很大（传感器噪声大），分母变大，K变小——我更相信模型
  • 如果P_pred很大（模型误差大），分子变大，K变大——我更相信测量

为什么会这样？因为K本质上是在做“加权平均”。我当年在调试GPS/INS组合导航时，就靠这个特性，在GPS信号差的时候自动降低测量权重，系统照样跑得稳。

避坑指南：我曾经犯过一个错误——把R设得太小，结果滤波器完全跟着噪声跑，输出全是毛刺。后来才明白，R不是越小越好，它应该反映传感器的真实噪声水平。建议先用静态数据算一下传感器的标准差，再设R的初值。

3.4 完整算法流程——一张图看懂

下面我用一张流程图把整个KF的迭代过程展示出来。这张图是我做项目时一直贴在工位上的，每次调试卡壳了就看一眼。

线性卡尔曼滤波（KF）迭代流程 初始状态 x₀, P₀ 预测步骤（时间更新） x_pred = A·x_est + B·u P_pred = A·P_est·Aᵀ + Q 计算卡尔曼增益 K = P_pred·Hᵀ·(H·P_pred·Hᵀ + R)⁻¹ 更新步骤（测量更新） x_est = x_pred + K·(z - H·x_pred) P_est = (I - K·H)·P_pred 输出 x_est, P_est k = k + 1 迭代

3.5 代码实现——动手写一个最简单的KF

光说不练假把式。下面我给出一个一维卡尔曼滤波的C语言实现。这个代码我在STM32上跑过，用来滤波一个温度传感器的读数。

#include <math.h>

typedef struct {
    float x;  // 状态估计值
    float P;  // 估计误差协方差
    float Q;  // 过程噪声协方差
    float R;  // 观测噪声协方差
    float K;  // 卡尔曼增益
} KalmanFilter;

// 初始化滤波器
void KF_Init(KalmanFilter *kf, float init_x, float init_P, float Q, float R) {
    kf->x = init_x;
    kf->P = init_P;
    kf->Q = Q;
    kf->R = R;
    kf->K = 0.0f;
}

// 预测步骤（一维情况，A=1, B=0）
void KF_Predict(KalmanFilter *kf) {
    kf->P = kf->P + kf->Q;  // P_pred = P_est + Q
}

// 更新步骤（一维情况，H=1）
void KF_Update(KalmanFilter *kf, float z) {
    // 计算卡尔曼增益
    kf->K = kf->P / (kf->P + kf->R);
    
    // 更新状态估计
    kf->x = kf->x + kf->K * (z - kf->x);
    
    // 更新误差协方差
    kf->P = (1.0f - kf->K) * kf->P;
}

// 使用示例
int main() {
    KalmanFilter kf;
    KF_Init(&kf, 25.0f, 1.0f, 0.01f, 0.1f);
    
    float measurements[] = {24.5, 25.1, 24.8, 25.3, 25.0};
    for (int i = 0; i < 5; i++) {
        KF_Predict(&kf);
        KF_Update(&kf, measurements[i]);
        printf("测量值: %.2f, 滤波后: %.2f\n", measurements[i], kf.x);
    }
    return 0;
}

调试技巧：我建议你在调参时，先把Q和R设成1.0，然后观察滤波效果。如果响应太慢，减小Q；如果噪声太大，增大R。记住，Q和R的比值比绝对值更重要。

3.6 总结——KF的核心思想

线性卡尔曼滤波，说白了就是一套“预测-测量-修正”的闭环。它用状态空间模型描述系统，用预测方程外推状态，用更新方程融合测量，用卡尔曼增益自动调节权重。

我个人觉得，理解KF的关键不在于背公式，而在于理解“不确定性传递”这个概念。每次预测，不确定性增加；每次测量，不确定性减少。卡尔曼增益就是在这两者之间找平衡。

好了，这一章的内容就到这里。记住，纸上得来终觉浅，绝知此事要躬行。找个实际数据跑一遍，比看十遍公式都管用。

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

← 上一章 📖 返回目录 下一章 →