卡尔曼滤波原理详解

什么是卡尔曼滤波？

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

核心思想：预测 + 更新

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

第一步：预测（Predict）

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

$$\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}$$

• $\hat{x}_{k|k-1}$：预测的状态估计
• $P_{k|k-1}$：预测的误差协方差
• $F_k$：状态转移矩阵（描述系统如何演化）
• $Q_k$：过程噪声协方差（描述模型的不确定性）

第二步：更新（Update）

用实际测量值修正预测：

$$\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}$$

• $K_k$：卡尔曼增益（决定”信任”预测还是测量）
• $z_k$：实际测量值
• $H_k$：观测矩阵（将状态映射到测量空间）
• $R_k$：测量噪声协方差（描述传感器的不可靠性）

关键参数解读

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

卡尔曼增益的作用

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

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

$$K_k = \frac{P_{k|k-1}}{P_{k|k-1} + R_k} \quad (\text{当 } H_k = 1 \text{ 时})$$

---

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