卡尔曼滤波详解

1. 卡尔曼滤波能解决什么问题？

卡尔曼滤波用于解决含噪声的动态系统状态估计问题，例如：

• 通过GPS和IMU数据估计车辆位置

• 通过电压电流测量估计电池电量(SOC)

• 雷达追踪飞行器轨迹

它的核心优势是：

• 递归计算：只需前一时刻的状态，无需保存全部历史数据

• 最优估计：在高斯噪声假设下给出最小均方误差估计

• 实时性：计算量小，适合嵌入式系统

---

2. 卡尔曼滤波的五大核心公式

以下是卡尔曼滤波的完整数学表达，所有公式均可直接复制使用：

(1) 状态预测方程

 x̂ₖ|ₖ₋₁ = Fₖ x̂ₖ₋₁|ₖ₋₁ + Bₖ uₖ

• x̂ₖ|ₖ₋₁：k时刻的预测状态

• Fₖ：状态转移矩阵

• Bₖ：控制输入矩阵

• uₖ：控制量

 (2) 协方差预测方程

 Pₖ|ₖ₋₁ = Fₖ Pₖ₋₁|ₖ₋₁ Fₖᵀ + Qₖ

• Pₖ|ₖ₋₁：预测状态协方差

• Qₖ：过程噪声协方差

 (3) 卡尔曼增益计算

 Kₖ = Pₖ|ₖ₋₁ Hₖᵀ (Hₖ Pₖ|ₖ₋₁ Hₖᵀ + Rₖ)⁻¹

• Kₖ：卡尔曼增益

• Hₖ：观测矩阵

• Rₖ：观测噪声协方差

 (4) 状态更新方程

x̂ₖ|ₖ = x̂ₖ|ₖ₋₁ + Kₖ (zₖ - Hₖ x̂ₖ|ₖ₋₁)

• x̂ₖ|ₖ：更新后的状态估计

• zₖ：实际观测值

(5) 协方差更新方程

Pₖ|ₖ = (I - Kₖ Hₖ) Pₖ|ₖ₋₁

• Pₖ|ₖ：更新后的协方差矩阵

协方差（Covariance）在卡尔曼滤波中的作用

协方差矩阵 P 是卡尔曼滤波的核心组成部分，它直接反映了 状态估计的不确定性（即估计值与真实值之间的误差分布）：

 量化估计的不确定性

• Pk∣k​（更新后的协方差）：表示当前状态估计的置信度。

  • 对角线元素：各状态变量的方差（如位置、速度的误差大小）。

  • 非对角线元素：状态变量之间的相关性（如位置误差与速度误差的关系）。

• 物理意义：

  • 若 P 较小 → 估计较准确（不确定性低）。

  • 若 P 较大 → 估计不可靠（不确定性高）。