卡尔曼滤波器的预测-更新方程推导及最小均方误差估计

核心问题与解决方案

卡尔曼滤波器解决的核心问题，是从一系列带有噪声的观测数据中，估计一个动态系统的真实状态。简单来说，它能帮你在迷雾中（数据噪声）看清一个移动物体（系统状态）的轨迹。

它的解决方案分为两个交替进行的步骤：预测与更新。本文将手把手推导这两个步骤背后的核心方程，并解释为什么它能给出“最小均方误差”意义上的最佳估计。

1. 建立基础模型

在开始之前，我们需要用两个简单的线性方程来描述系统：

1. 状态方程：描述系统真实状态如何随时间变化。
   $$x_k = F_k x_{k-1} + B_k u_k + w_k$$

2. 观测方程：描述我们如何观测到带有噪声的状态。
   $$z_k = H_k x_k + v_k$$

下面逐一解释每个符号：

• $x_k$：$k$ 时刻的真实状态向量。这是我们最终想求出的值。例如，它可以包含位置和速度。
• $F_k$：状态转移矩阵。它描述了系统从 $k-1$ 时刻到 $k$ 时刻的物理规律。例如，一个匀速运动模型，$F_k$ 会将“上一时刻的位置加上速度乘以时间”这个规律编码进去。
• $B_k u_k$：控制输入项。$u_k$ 是已知的控制量（如油门力度），$B_k$ 是控制矩阵。如果系统不受外部主动控制，这一项可以为零。
• $w_k$：过程噪声。它代表了模型无法预测的微小扰动（如风速变化、系统内部摩擦）。我们假设它服从均值为0，协方差矩阵为 $Q_k$ 的高斯分布。
• $z_k$：$k$ 时刻的实际观测值。这是传感器（如GPS）读到的数据。
• $H_k$：观测矩阵。它将状态空间 $x_k$ 映射到观测空间 $z_k$。例如，如果 $x_k$ 包含[位置，速度]，而 $z_k$ 只是位置，那么 $H_k$ 就是一个只提取位置分量的矩阵。
• $v_k$：观测噪声。它代表了传感器的误差。我们假设它服从均值为0，协方差矩阵为 $R_k$ 的高斯分布。

我们的任务就是，在已知 $F_k$, $B_k$, $H_k$, $Q_k$, $R_k$ 以及一系列观测 $z_1, z_2, ..., z_k$ 和控制 $u_1, u_2, ..., u_k$ 的情况下，估计出 $x_k$。

2. 预测步骤：利用模型进行推断

在获得 $k$ 时刻的观测 $z_k$ 之前，我们首先根据 $k-1$ 时刻的最优估计 $\hat{x}_{k-1}$ 来预测当前时刻的状态。

1. 预测状态：直接使用状态方程，但用估计值代替真实值。
   $$\hat{x}_k^- = F_k \hat{x}_{k-1} + B_k u_k$$

   • $\hat{x}_{k-1}$ 是 $k-1$ 时刻更新后的最优状态估计。
   • $\hat{x}_k^-$ 是 $k$ 时刻预测后的状态估计（上标“-”表示这是预测值，尚未被观测修正）。
   • 我们在这里忽略了过程噪声 $w_k$，因为它的均值是0。

2. 预测协方差：预测不确定性如何增长。不确定性用协方差矩阵 $P$ 来衡量，它的对角线元素代表各状态分量的方差（不确定性），非对角线元素代表分量间的相关性。
   $$P_k^- = F_k P_{k-1} F_k^T + Q_k$$

   • $P_{k-1}$ 是 $k-1$ 时刻更新后的估计协方差。
   • $F_k P_{k-1} F_k^T$：表示状态转移过程 $F_k$ 会如何传播原有的不确定性。
   • $+ Q_k$：非常重要的一步。表示由于存在无法预测的过程噪声 $w_k$，系统的不确定性在转移过程中会增加。

至此，预测步骤完成。我们得到了一个基于模型的、但尚未被最新观测数据修正的状态估计 $\hat{x}_k^-$ 及其不确定性 $P_k^-$。

3. 更新步骤：融合观测进行修正

当我们拿到 $k$ 时刻的传感器读数 $z_k$ 后，就可以用它来修正我们的预测值，得到一个更精确的估计。

1. 计算观测残差：衡量预测值和实际观测之间的差距。
   $$y_k = z_k - H_k \hat{x}_k^-$$

   • $H_k \hat{x}_k^-$：是预测状态 $\hat{x}_k^-$ 对应的预测观测值。如果模型完美且无噪声，$z_k$ 应该等于 $H_k \hat{x}_k^-$。
   • $y_k$ 就是新息或残差，它包含了预测值与观测值之间的新信息。

2. 计算残差协方差：衡量残差自身的不确定性。
   $$S_k = H_k P_k^- H_k^T + R_k$$

   • $H_k P_k^- H_k^T$：将预测状态的不确定性 $P_k^-$ 投影到观测空间。
   • $+ R_k$：再次加上观测噪声的不确定性 $R_k$。残差的不确定性源于两方面：预测状态的不确定（第一项）和观测传感器的噪声（第二项）。

3. 计算卡尔曼增益：这是整个滤波器的灵魂。它决定了我们应该在多大程度上信任新的观测数据 $z_k$。
   $$K_k = P_k^- H_k^T S_k^{-1}$$

   • 核心逻辑：$K_k$ 是一个权重矩阵。
     • 如果观测噪声 $R_k$ 很小（传感器很准），$S_k$ 主要由 $R_k$ 主导，导致 $S_k^{-1}$ 变大，$K_k$ 变大。这意味着更信任观测 $z_k$。
     • 如果预测不确定性 $P_k^-$ 很大（模型很不准），$P_k^-$ 变大，$K_k$ 也变大，同样表示更信任观测。
     • 反之，则更信任预测值。
   • 意义：$K_k$ 自动地平衡了模型预测和传感器观测的信任度，无需人工调节。

4. 更新状态估计：用卡尔曼增益将观测残差 $$y_k$$ 融合到预测值中。
   $$\hat{x}_k = \hat{x}_k^- + K_k y_k$$

   • 最终状态估计 $\hat{x}_k$ = 之前的预测 $\hat{x}_k^-$ + 增益 $K_k$ * 残差 $y_k$。
   • 直观理解：如果残差为0，说明预测和观测完美吻合，估计值不变。否则，就按照增益 $K_k$ 指示的比例进行修正。

5. 更新估计协方差：估计更新后，系统的不确定性会减小。
   $$P_k = (I - K_k H_k) P_k^-$$

   • $I$ 是单位矩阵。
   • 意义：每一次成功的更新（融合新的观测），都会降低我们对状态估计的不确定性。$P_k$ 通常小于 $P_k^-$，表明融合观测后，我们对估计结果更有信心。

4. 最小均方误差估计的保证

为什么卡尔曼滤波器是“最优”的？因为它是在最小均方误差（MMSE） 准则下的线性最优估计。

1. 均方误差：我们用估计值的协方差矩阵 $P_k$ 来衡量估计误差的大小，其对角线元素的和（即矩阵的迹）就是均方误差。
2. 优化目标：卡尔曼滤波器的推导过程，其本质就是寻找一个线性增益 $K_k$，使得更新后的估计误差协方差 $P_k$ 的迹（Trace）最小化。数学上可以证明，$K_k = P_k^- H_k^T S_k^{-1}$$` 这个解确实能使 `$Trace(P_k)$` 达到最小。 3. **高斯假设**：在过程噪声 `$w_k$` 和观测噪声 `$v_k$` 均为高斯分布的假设下，卡尔曼滤波器给出的估计不仅是均方误差意义下的最优，它还等价于**最大后验概率（MAP）** 估计，即条件概率 `$p(x_k | z_{1:k})$ 的最大值。

因此，卡尔曼滤波器通过严格的数学推导，自动地实现了在给定模型和噪声统计特性下的最优信息融合。预测步负责利用系统的物理规律进行推断，更新步负责用最新的观测数据进行修正，两者循环往复，形成一个高效、最优的状态估计框架。