卡尔曼滤波器五个公式 - 卡尔曼滤波五个公式深度解析

在自动化控制、信号处理、人工智能与机器人领域，卡尔曼滤波器五个公式构成了现代状态估计的基石。卡尔曼滤波（Kalman Filter）是一种基于线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。它特别适用于存在噪声干扰的动态系统。

很多人将卡尔曼滤波视为黑盒，但其实其核心逻辑完全由卡尔曼滤波五个公式紧密耦合而成：预测 → 更新 → 增益计算 → 修正 → 误差评估。这五个步骤构成一个完美闭环。理解它们，不仅是记忆数学表达式，更是理解如何在“不确定性”中寻找“确定性”的过程。

卡尔曼滤波器五个公式全景图

其中，x̂代表状态估计，P代表误差协方差矩阵，K为卡尔曼增益，Q为过程噪声协方差，R为测量噪声协方差，A为状态转移矩阵，B为控制输入矩阵，H为观测矩阵，u为控制输入，z为测量值。

状态预测公式：时间的推演

假设系统状态为向量 x，系统模型为线性方程 xₖ = A xₖ₋₁ + B uₖ + wₖ，其中 A 为状态转移矩阵，B 为输入矩阵，wₖ 为过程噪声。预测值 x̂ₖ|ₖ₋₁ 通过矩阵乘法运算得出。

这一步骤模拟了系统在无新观测时的自然演化趋势。例如，假如你知道汽车当前的速度和方向，你可以预测下一秒钟它大概在哪里。然而，由于道路摩擦、风阻等不可控因素（即过程噪声 wₖ），这个预测必然存在误差。因此，我们必须记录这个预测的不确定性，即预测误差协方差 Pₖ|ₖ₋₁。

伪代码实现

// 状态预测
x_pred = A  x_prev + B  u;  // 状态估计预测
P_pred = A  P_prev  A.transpose() + Q;  // 协方差预测

测量更新公式：现实的校正

当接收到实际测量值 zₖ 时，需计算观测噪声协方差 R。更新后的状态估计 x̂ₖ|ₖ 由预测值与测量值的加权平均构成。其中 yₖ 被称为残差（Innovation），代表了测量值与预测值之间的差异。

如果残差很大，说明预测可能偏离了现实，需要大幅修正；若残差很小，说明预测很准，只需微调。测量更新步骤完成了从“预测”到“估计”的转化。

卡尔曼增益公式：信任的平衡器

这是整个算法中最精妙的部分。卡尔曼增益 Kₖ 的计算依赖于过程噪声协方差 Q 和测量噪声协方差 R，体现系统对测量信息的信任程度：

• 当预测噪声较大时（P 大）：分子变大，Kₖ 趋近于 1，系统主要依赖测量值，因为预测太不准了。
• 当测量噪声较大时（R 大）：分母变大，Kₖ 趋近于 0，系统完全依赖预测，因为传感器全是杂音。

这是算法平衡预测与观测能力的核心枢纽，也是工程师调参的主要对象。

状态估计公式：最佳解的诞生

基于卡尔曼增益，最终的状态估计值 x̂ₖ|ₖ 即为四个变量（预测值、预测协方差、测量值、测量协方差）的综合结果。此步骤完成了从“预测”到“估计”的转化，给出了在观测数据下的最优解（在无偏最小方差意义下）。

此时的 x̂ₖ|ₖ 是我们对当前时刻系统状态最可信的判断。其最优性基于以下假设：

• 系统模型线性
• 噪声为高斯白噪声
• 过程噪声与测量噪声不相关

误差协方差公式：不确定性的收敛

误差协方差矩阵 Pₖ|ₖ，表示当前时刻状态估计的方差矩阵。随着算法迭代，该矩阵对角线元素逐渐减小，表明估计精度不断提升，系统越来越自信。同时，协方差矩阵也用于计算卡尔曼增益，形成动态反馈机制。

实战案例：无人机定高系统

在实际工程中，如何配置卡尔曼滤波器五个公式中的参数？以无人机定高为例：

// 初始化
x = [0.0, 0.0];  // [高度, 速度]
P = [[1.0, 0], [0, 1.0]];  // 初始不确定性
A = [[1, dt], [0, 1]];
B = [0, 0];  // 无控制输入
Q = [[0.01, 0], [0, 0.1]];
H = [1, 0];
R = 0.25;
loop:
  // 1. 预测
  x_pred = A  x;  // 状态预测
  P_pred = A  P  A.transpose() + Q;  // 协方差预测
  // 2. 更新
  z = sensor_read();  // 获取高度测量值
  y = z - H  x_pred;  // 残差
  S = H  P_pred  H.transpose() + R;  // 残差协方差
  K = P_pred  H.transpose() / S;  // 卡尔曼增益
  x = x_pred + K  y;  // 状态估计
  P = (I - K  H)  P_pred;  // 协方差更新
end loop

常见问题解答

网友们还关心

热门标签