如何理解卡尔曼滤波器？

卡尔曼滤波（Kalman Filter，KF）是一个用于估计系统状态的递归算法，适用于线性系统+高斯噪声情形。它的核心思想是：

融合“预测”和“观测”，在噪声存在的情况下对动态目标状态进行最优估计。

卡尔曼滤波就是：

“我知道目标昨天的位置和速度（预测），今天我观测到一个不太准的位置（测量），我要综合这两者，用数学方法给出一个更准的现在的位置和速度（估计）。”

为什么会观测到不太准的位置？

因为现实世界中，测量总会有噪声，比如雷达测量的距离可能受天气、设备精度等影响。

场景类比：雷达跟踪飞机

  • 目标：跟踪一架飞机的位置和速度
  • 问题：雷达测得位置不准（测量有噪声）；目标也不一定匀速飞行（运动有噪声）
  • 解法：卡尔曼滤波根据过去的状态“预测”位置，又结合当前雷达观测，进行“校正”

卡尔曼滤波核心概念

1. 系统状态模型

目标的状态用一个向量表示，比如二维空间：

$$
x = [位置_x, 位置_y, 速度_x, 速度_y]^T
$$

系统状态随时间演化：
$$
x_k = F x_{k-1} + w_k
$$

其中：

  • F 是状态转移矩阵（如匀速模型）。
  • w_k 是过程噪声，表示系统的不确定性（如飞机突然转向）

状态转移矩阵 F

状态转移矩阵（记作 F）描述的是：

从上一个时刻 $x_{k-1}$，系统是如何过渡到当前时刻 $x_k$ 的。

也就是说，它描述目标的“运动规律”或“预测模型”。

公式如下：

$$
x_k = F x_{k-1} + B u_k + w_k
$$

  • $F$：状态转移矩阵（你要问的）
  • $x_{k-1}$：上一个时刻的状态
  • $x_k$：当前时刻的状态
  • $B u_k$：控制项（如果有控制输入）
  • $w_k$：过程噪声（建模随机扰动，通常假设是高斯）

一个经典的例子：一维匀速运动

我们想跟踪一个物体在一维直线上的运动，它的状态是：

$$
x = \begin{bmatrix} 位置 \ 速度 \end{bmatrix}
$$

设时间间隔为 $\Delta t$，也就是每隔 1 秒更新一次状态。

匀速直线运动公式：

$$
\text{新位置} = \text{旧位置} + \text{速度} \cdot \Delta t\text{新速度} = \text{旧速度}（速度不变）
$$

假设状态向量为：

$$
x_k = \begin{bmatrix} x \\ \dot{x} \end{bmatrix}_k = F \begin{bmatrix} x \\ \dot{x} \end{bmatrix}_{k-1}
$$

那么状态转移矩阵 F 是：

$$
F =
\begin{bmatrix}
1 & \Delta t \\
0 & 1 \\
\end{bmatrix}
$$

解释：

  • 位置 = 位置 + 速度 × 时间
  • 速度保持不变

假设：

  • 上一时刻：位置 10 米，速度 2 米/秒
  • 时间间隔：1 秒

$$
x_{k-1} =
\begin{bmatrix}
10 \
2 \
\end{bmatrix},
\quad \Delta t = 1
$$

$$
F = \begin{bmatrix}
1 & 1 \\
0 & 1 \\
\end{bmatrix}
\Rightarrow
x_k = F x_{k-1} = \begin{bmatrix}
1 & 1 \\
0 & 1 \\
\end{bmatrix}
\begin{bmatrix}
10 \\
2 \\
\end{bmatrix}
= \begin{bmatrix}
12 \\
2 \\
\end{bmatrix}
$$

→ 预测下一时刻的位置是 12，速度仍然是 2

2. 观测模型

我们能观测到的是测量值：
$$
z_k = H x_k + v_k
$$

其中：

  • H 是观测矩阵（把状态映射为观测）
  • v_k 是测量噪声（例如雷达不准）

KF 的几个关键点

术语 解释
状态向量 x 要估计的真实值，如位置、速度
状态转移矩阵 F 描述状态如何从前一时刻演化
观测矩阵 H 描述如何从状态映射出观测值， 展现能观测到的值
协方差矩阵 P 表示估计值的不确定性
卡尔曼增益 K 预测值和观测值之间的加权比例

协方差矩阵

最常见的 3 个关键协方差矩阵：

1. 状态估计协方差矩阵 P（最核心）

  • 含义：表示我们对当前状态估计（如位置、速度）的不确定性。（值越大，意味着对预测结果越没有信心）

  • 维度：与状态向量 x 同维度，如位置+速度就是 2×2。

  • 行为：

    • 初始值很大，表示我们一开始“很不确定”。
    • 随着测量不断修正，P 会逐步变小，表示我们“越来越确信”。
    • 是滤波器自己对估计结果的信心，不断更新（越小表示越相信自己的估计）

  • 初始值设定：

    • 如果你一开始对状态估计不确定（如初始速度不知道），设成较大：

      $$
      P_0 =
      \begin{bmatrix}
      1000 & 0 \
      0 & 1000
      \end{bmatrix}
      $$

    • 如果你知道初始位置很准但速度不确定，可以设置为：

      $$
      P_0 =
      \begin{bmatrix}
      1 & 0 \
      0 & 1000
      \end{bmatrix}
      $$

  • 是否调整？

    • P 是自动更新的，不需要你手动优化。

这里的协方差，比如左边斜对角的值代表特征之间的相关性，右边斜对角的值代表特征自身的方差。如这里0表示位置和速度之间没有相关性，位置的方差是1000，速度的方差是1000。

2. 过程噪声协方差矩阵 Q

  • 含义：表示系统模型中由于不可控因素（如摩擦、风、加速度变化）带来的随机扰动。

  • 行为：

    • 加在预测阶段，使得 P 不会无限减小。
    • 控制预测时的“扩散程度”。

  • 如果设置 Q = 0，KF 就会过于自信，忽略真实世界的“抖动”或“误差”。

  • 怎么理解？

    • 表示你的系统模型（如匀速运动）有多大的不确定性，比如：

      • 风的扰动
      • 摩擦不同步
      • 目标突然加速等

  • 初始值设定方式：

    • 如果你相信系统运动模型比较准 → 设 Q 小一些（如 Q = 0.01）
    • 如果目标运动变化很不稳定 → 设 Q 大一些（如 Q = 1 或更高）

  • 优化建议：

    • 先手动尝试不同的值，看误差大小。
    • 可以通过历史轨迹拟合/估计（如用系统残差反推）获得更优的 Q。
    • 高级方法：用 EM 算法自动估计 Q。

如何理解运动模型准不准？

卡尔曼滤波的“系统运动模型”是你假设的目标运动方式，比如：

  • 匀速直线运动：

    $$
    x_k = x_{k-1} + v \cdot \Delta t
    $$

  • 匀加速运动：

    $$
    x_k = x_{k-1} + v_{k-1} \cdot \Delta t + \frac{1}{2} a \cdot \Delta t^2
    $$

如果真实目标确实这么动（如在轨道上匀速滑行），就可以说系统运动模型很准！

为什么模型准时 Q 应该小？

Q 是过程噪声协方差矩阵，表示系统运动过程的不可控扰动，比如：

  • 忽然加速、减速
  • 突然转弯
  • 风、坡道、轮胎打滑等干扰

如果你“相信模型非常准”，意味着你觉得这些突发情况不常发生，因此：

你希望滤波器更信任模型的预测，不去太敏感地响应突然变化
→ 所以要设置 Q 较小！

假设你在跟踪一个小火车：

  • 它沿着轨道匀速前进，外界扰动几乎没有 → 模型非常可靠

  • 每秒你预测位置更新为：

    $$
    x_k = x_{k-1} + v \cdot \Delta t
    $$

  • 如果设置 Q = 0.01，表示：

    “我认为预测误差很小，所以我非常信任这个预测位置。”

当观测值（比如 GPS）有偏差时，滤波器不会被轻易“拉偏”——它会主要信任自己的预测。

如果 Q 设大，会发生什么？

你在说：“我不太信自己预测得准，可能运动发生了突变（比如拐弯、刹车）”

此时卡尔曼滤波会：

  • 增强对观测值的依赖（即传感器说啥就信啥）
  • 减少对模型预测的信任

这在目标运动很复杂、频繁变化时（如：行人、动物、赛车）是合理的。

Q 越小，表示你越信任预测模型，越认为目标的运动方式是可以预期的；
Q 越大，表示你觉得模型靠不住，要更多依赖实际观测值。

3. 观测噪声协方差矩阵 R

  • 含义：表示观测值本身的测量误差。是滤波器对观测值误差的估计，即传感器精度的数学表达。

  • 行为：

    • 决定我们“信任预测多一点”还是“信任传感器多一点”。
    • 越大 → 表示观测很不可靠，KF 更信预测。
    • 越小 → 表示观测更精准，KF 更信传感器。

  • 初始值设定方式：

    • 看你用的传感器精度。比如：

      • GPS ±5米 → R ≈ 25
      • 激光雷达 ±0.1米 → R ≈ 0.01

  • 优化建议：

    • 初期可用经验值设定。
    • 可以从大量历史测量数据中计算真实的测量误差方差。
    • 多传感器融合时，可单独估计每种传感器的 R。

总结对比表

矩阵 全称 含义 作用阶段 通常大小
P 状态估计协方差 我对状态估计的信心（不确定性） 预测 + 更新 动态变化
Q 过程噪声协方差 模型预测的不确定性（系统噪声） 预测 通常较小
R 观测噪声协方差 传感器测量的不确定性 更新 固定或缓变
  • P 是滤波器自己对估计结果的信心，不断更新（越小表示越相信自己的估计）；
  • Q 是滤波器对模型预测误差的估计，表示系统内部未知的“突变”或干扰；
  • R 是滤波器对观测值误差的估计，即传感器精度的数学表达。

简单梳理

设：

  • $x_k$：当前时刻的状态
  • $\hat{x}_k^-$：预测的状态
  • $\hat{x}_k$：更新后的状态
  • $z_k$：测量值
  • $F$：状态转移矩阵
  • $H$：观测矩阵
  • $K_k$：卡尔曼增益

第一步：预测（Predict）

我们根据上一时刻的状态和模型预测当前状态：

状态预测：

$$
\hat{x}_k^- = F \cdot \hat{x}_{k-1} （通过状态矩阵F去预测）
$$

协方差预测（用到 P 和 Q）：

$$
P_k^- = F \cdot P_{k-1} \cdot F^T + Q
$$

  • P 是我们对上一步状态的不确定性
  • Q 是我们对模型预测的“额外担忧”
  • 两者一起影响当前这步预测的不确定性（P_k^-）

第二步：更新（Update）

我们用当前传感器观测值 $z_k$ 来修正预测。

计算卡尔曼增益（用到 P_k^- 和 R）：

$$
K_k = P_k^- \cdot H^T \cdot (H \cdot P_k^- \cdot H^T + R)^{-1}
$$

  • 如果 R 很小 → 测量非常可靠 → 卡尔曼增益大 → 更信观测值
  • 如果 P_k^- 很小 → 预测非常可靠 → 卡尔曼增益小 → 更信预测值

状态更新：

$$
\hat{x}_k = \hat{x}_k^- + K_k \cdot (z_k - H \cdot \hat{x}_k^-)
$$

用卡尔曼增益 K_k 来融合预测值和观测值，达到平衡。

协方差更新（更新 P）：

$$
P_k = (I - K_k \cdot H) \cdot P_k^-
$$

表示我们对当前状态的不确定性在更新之后如何变化（通常会变小）。