从零开始理解卡尔曼滤波

1. 直观理解

想象你在雾中观察一辆行驶的汽车：

1. 你有不完美的眼睛（传感器）：只能看到模糊位置，有误差
2. 你有大脑模型（系统模型）：知道汽车会继续前进
3. 卡尔曼滤波是大脑决策过程：结合模糊观察和自身预测，得出最佳位置估计

卡尔曼滤波的精髓在于平衡两种信息源：

• 预测值： 基于物理规律（如运动模型）的估计
• 测量值： 来自传感器的实际读数

当测量值很可靠时，我们更相信它；当预测模型很准确时，我们更相信预测。卡尔曼滤波通过数学方法自动找到最佳平衡点。

2. 数学公式理解

步骤1：预测

1.1 预测状态（运动模型）

x̂ₖ⁻：当前时刻的预测状态（如预测的位置/速度） F：状态转移矩阵（描述状态如何变化，如位置+速度×时间） x̂ₖ₋₁：上一时刻的最佳估计 B：控制输入矩阵（可选，如加速度控制） uₖ：控制输入（如加速度、转向等）

示例：小车匀速运动时，当前预测位置 = 上一位置 + 速度 × 时间间隔（纯基于物理模型，没有考虑实际测量值，模型越准确，预测越可靠）

1.2 预测不确定性（协方差更新）

Pₖ⁻：预测状态的不确定性（协方差） Pₖ₋₁：上一时刻的不确定性 Q：过程噪声（模型不完美的程度）

直观理解：

• 预测时间越长，不确定性越大（Q的作用，熵增原理）
• 模型不完美会增加不确定性
• 协方差矩阵 P 的对角线元素是各状态变量的方差
  • 位置方差 ：位置估计的波动范围
  • 速度方差 ：速度估计的波动范围
• 非对角线元素表示状态变量间的协方差（相关程度）

步骤2：更新

2.1 计算卡尔曼增益

Kₖ：卡尔曼增益（平衡预测值和测量值，决定信任预测还是测量） H：观测矩阵（状态如何转换为测量值） R：测量噪声（传感器误差）

直观理解：

当测量噪声R很大 → 增益K小 → 更相信预测 当预测不确定P⁻很大 → 增益K大 → 更相信测量

2.2 更新状态（观测模型）

x̂ₖ：最终的最佳状态估计 zₖ：实际测量值 (zₖ - H x̂ₖ⁻)：测量残差（预测与测量的差异）

直观理解：

2.3 更新不确定性

Pₖ：更新后的不确定性（比预测时更小） I：单位矩阵

直观理解：

3. 图解卡尔曼滤波流程