雷达原理-卡尔曼滤波器基础学习

如何理解卡尔曼滤波器？

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

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

卡尔曼滤波就是：

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

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

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

场景类比：雷达跟踪飞机

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

卡尔曼滤波核心概念

1. 系统状态模型

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

$$x=[\mathrm{位}{\mathrm{置}}_x,\mathrm{位}{\mathrm{置}}_y,\mathrm{速}{\mathrm{度}}_x,\mathrm{速}{\mathrm{度}}_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=\left[\begin{array}{c}\mathrm{位}\mathrm{置}\\ \mathrm{速}\mathrm{度}\end{array}\right]$$

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

匀速直线运动公式：

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

假设状态向量为：

$$x_k={\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]}_k=F{\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]}_{k-1}$$

那么状态转移矩阵 F 是：

$$F=\left[\begin{array}{cc}1& \mathrm{\Delta }t\\ 0& 1\end{array}\right]$$

解释：

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

假设：

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

$$x_{k-1}=\left[\begin{array}{c}10\\ 2\end{array}\right],\mathrm{\Delta }t=1$$$$F=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]\Rightarrow x_k=F{x}_{k-1}=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]\left[\begin{array}{c}10\\ 2\end{array}\right]=\left[\begin{array}{c}12\\ 2\end{array}\right]$$

→ 预测下一时刻的位置是 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=\left[\begin{array}{cc}1000& 0\\ 0& 1000\end{array}\right]$$

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

    $$P_0=\left[\begin{array}{cc}1& 0\\ 0& 1000\end{array}\right]$$

• 是否调整？

  • 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 \mathrm{\Delta }t$$

• 匀加速运动：

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

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

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

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

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

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

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

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

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

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

  $$x_k=x_{k-1}+v\cdot \mathrm{\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}（\mathrm{通}\mathrm{过}\mathrm{状}\mathrm{态}\mathrm{矩}\mathrm{阵}F\mathrm{去}\mathrm{预}\mathrm{测}）$$

协方差预测（用到 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^{-}$$

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