卡尔曼滤波原理详解

2026年6月20日

什么是卡尔曼滤波？

卡尔曼滤波（Kalman Filter）是一种递归的、最优的状态估计算法。它能够从包含噪声的测量数据中，估计出动态系统的真实状态。1960 年由 Rudolf E. Kálmán 提出，广泛应用于导航、自动驾驶、目标跟踪、信号处理等领域。

核心思想：预测 + 更新

卡尔曼滤波的核心是一个两步循环：

第一步：预测（Predict）

根据系统模型，预测下一时刻的状态：

x^k∣k−1=Fk⋅x^k−1∣k−1Pk∣k−1=Fk⋅Pk−1∣k−1⋅FkT+Qk\begin{aligned} \hat{x}_{k|k-1} &= F_k \cdot \hat{x}_{k-1|k-1} \\ P_{k|k-1} &= F_k \cdot P_{k-1|k-1} \cdot F_k^T + Q_k \end{aligned}

第二步：更新（Update）

用实际测量值修正预测：

Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)Pk∣k=(I−KkHk)Pk∣k−1\begin{aligned} K_k &= P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1} \\ \hat{x}_{k|k} &= \hat{x}_{k|k-1} + K_k (z_k - H_k \hat{x}_{k|k-1}) \\ P_{k|k} &= (I - K_k H_k) P_{k|k-1} \end{aligned}

关键参数解读

参数符号含义调大效果
过程噪声协方差Q模型不确定性更信任测量值，响应更快但更抖动
测量噪声协方差R传感器噪声更信任模型预测，估计更平滑但响应慢
初始状态估计x̂₀滤波器初始猜测影响收敛速度
初始误差协方差P₀初始不确定度越大收敛越快，但初始阶段波动大

卡尔曼增益的作用

卡尔曼增益 KkK_k 是滤波器的”信噪比”。当 Kk→1K_k \to 1 时，滤波器几乎完全信任测量值；当 Kk→0K_k \to 0 时，滤波器几乎完全信任模型预测。

增益的计算公式体现了预测不确定性与测量不确定性之间的权衡：

Kk=Pk∣k−1Pk∣k−1+Rk(当 Hk=1 时)K_k = \frac{P_{k|k-1}}{P_{k|k-1} + R_k} \quad (\text{当 } H_k = 1 \text{ 时})

现在就去卡尔曼滤波仿真平台动手试试吧！

← 返回仿真平台