一、什么是卡尔曼滤波？
卡尔曼滤波（Kalman Filter, KF） 是一种最优递归估计算法，用于在存在噪声的情况下，从一系列不精确的测量中估计动态系统的内部状态。
二、系统数学模型
1. 状态方程（系统动态模型）
xₖ = Fₖ xₖ₋₁ + Bₖ uₖ + wₖ
2. 观测方程（量测模型）
zₖ = Hₖ xₖ + vₖ
其中：
- xₖ：k 时刻的真实状态向量（n 维），如位置、速度；
- zₖ：k 时刻的观测向量（m 维），如传感器读数；
- uₖ：k 时刻的控制输入（可选）；
- Fₖ：状态转移矩阵（n×n）；
- Bₖ：控制输入矩阵（n×p）；
- Hₖ：观测矩阵（m×n）；
- wₖ：过程噪声，服从 wₖ ～ N(0, Qₖ)；
- vₖ：观测噪声，服从 vₖ ～ N(0, Rₖ)；
- Qₖ：过程噪声协方差矩阵；
- Rₖ：观测噪声协方差矩阵。
假设 wₖ 与 vₖ 相互独立，且均为零均值高斯白噪声。
三、卡尔曼滤波算法步骤
设 x̂ₖ|ₖ 表示基于 k 时刻及之前所有观测对状态的最佳估计，Pₖ|ₖ 为其误差协方差矩阵。
初始化（k = 0）
x̂₀|₀ = 初始状态估计
P₀|₀ = 初始误差协方差矩阵
对每个时间步 k = 1, 2, 3, ...
第一步：预测（Predict）
- 状态预测
x̂ₖ|ₖ₋₁ = Fₖ x̂ₖ₋₁|ₖ₋₁ + Bₖ uₖ - 协方差预测
Pₖ|ₖ₋₁ = Fₖ Pₖ₋₁|ₖ₋₁ Fₖᵀ + Qₖ
第二步：更新（Update）
- 计算卡尔曼增益
Kₖ = Pₖ|ₖ₋₁ Hₖᵀ (Hₖ Pₖ|ₖ₋₁ Hₖᵀ + Rₖ)⁻¹ - 状态更新
x̂ₖ|ₖ = x̂ₖ|ₖ₋₁ + Kₖ (zₖ − Hₖ x̂ₖ|ₖ₋₁) - 协方差更新
Pₖ|ₖ = (I − Kₖ Hₖ) Pₖ|ₖ₋₁
其中：
- x̂ₖ|ₖ₋₁：先验估计（预测值）
- x̂ₖ|ₖ：后验估计（修正值）
- zₖ − Hₖ x̂ₖ|ₖ₋₁：称为"新息"（Innovation），表示观测与预测的残差
- Kₖ：卡尔曼增益，自动权衡模型预测与传感器观测的可信度
四、符号说明
表格
| 符号 | 含义 |
|---|---|
| xₖ | k 时刻真实状态 |
| x̂ₖ | ₖ |
| zₖ | k 时刻观测值 |
| Fₖ | 状态转移矩阵 |
| Hₖ | 观测矩阵 |
| Qₖ | 过程噪声协方差 |
| Rₖ | 观测噪声协方差 |
| Pₖ | ₖ |
| Kₖ | 卡尔曼增益 |
| I | 单位矩阵 |
| ᵀ | 转置（上标 T） |
| ⁻¹ | 逆（上标 -1） |
✅ 所有上标（如 ᵀ、⁻¹）均为 Unicode 标准字符，在 Word 中显示正常。
五、直观理解
- 预测阶段：根据物理模型推算下一状态，但不确定性增加（P 变大）。
- 更新阶段 ：用实际观测修正预测。
- 若传感器很准（R 小）→ K 大 → 更相信观测；
- 若模型很准（Q 小）→ K 小 → 更相信预测。
- 卡尔曼增益 K 自动平衡两者，实现最优融合。
六、关键性质
- 最优性：在线性高斯假设下，给出最小均方误差（MMSE）估计。
- 递归性：只需当前估计和新观测，无需历史数据。
- 实时性：计算高效，适合嵌入式系统。
- 不确定性量化：通过 Pₖ|ₖ 实时反映估计可信度。
七、总结
卡尔曼滤波的核心思想是：
用模型预测未来，用观测修正错误，通过最优加权得到最可靠的状态估计。
其公式简洁、效果强大，是现代导航、控制、机器人等领域的基石算法。