卡尔曼滤波及其扩展

2025-03-25

2025-04-21

算法

卡尔曼滤波（Kalman Filter, KF）及其扩展版本——扩展卡尔曼滤波（Extended Kalman Filter, EKF）和无迹卡尔曼滤波（Unscented Kalman Filter, UKF）——是用于动态系统状态估计的经典方法。本笔记将详细介绍这三种滤波器的原理、数学推导和算法流程。

基本卡尔曼滤波（KF）

问题背景与基本假设

卡尔曼滤波的目标是估计一个动态系统的状态 $x_k$，该状态随时间 $ k $ 变化。由于我们无法直接观测 $ x_k $，只能通过含噪声的观测 $ z_k $ 间接获取状态信息。系统基于以下假设：

线性动态系统：状态转移和观测过程可以用线性方程描述。
高斯噪声：系统噪声和观测噪声均为零均值的高斯白噪声。

系统的数学模型如下：

状态转移方程：

$x_k = A x_{k-1} + B u_{k-1} + w_{k-1}$

$ x_k $：时刻 $ k $ 的真实状态向量（未知）。
$ A $：状态转移矩阵，描述状态从 $ k-1 $ 到 $ k $ 的线性演化。
$ B $：控制输入矩阵，将控制输入映射到状态空间。
$ u_{k-1} $：控制输入向量，表示外部输入对系统的作用。
$ w_{k-1} $：过程噪声，服从高斯分布 $ w_{k-1} \sim N(0, Q) $，其中 $ Q $ 为过程噪声协方差矩阵，描述过程噪声的不确定性。

示例说明

假设一个简单的一维运动系统：物体在匀加速运动中，状态包括位置 $ p $ 和速度 $ v $，控制输入是加速度 $ a $。状态向量为 $ x_k = [p_k, v_k]^T $，控制输入为 $ u_k = a_k $。

离散化后（时间步长为 $ \Delta t $）的状态转移方程为：
$p_k = p_{k-1} + v_{k-1} \Delta t + \frac{1}{2} a_{k-1} \Delta t^2$ $v_k = v_{k-1} + a_{k-1} \Delta t$
写成矩阵形式：
$x_k = \begin{bmatrix} p_k \\ v_k \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p_{k-1} \\ v_{k-1} \end{bmatrix} + \begin{bmatrix} \frac{1}{2} \Delta t^2 \\ \Delta t \end{bmatrix} a_{k-1} + w_{k-1}$

状态转移矩阵$ A = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} $描述位置和速度的自然演化（即使没有加速度，位置仍随速度变化）。$ A $ 是系统的内在动力学模型，负责状态的自然演化。$ A x_{k-1} $ 表示无外部输入时的状态转移。

控制输入矩阵$ B = \begin{bmatrix} \frac{1}{2} \Delta t^2 \\ \Delta t \end{bmatrix} $将加速度 $ a_{k-1} $ 映射到位置和速度的变化。$ B $ 是外部控制的桥梁，将控制输入引入状态空间。$ B u_{k-1} $ 表示加速度对状态的贡献。

观测方程：

$z_k = H x_k + v_k$

$ z_k $：时刻 $ k $ 的观测向量，通过传感器获取。
$ H $：观测矩阵，将真实状态映射到观测空间。
$ v_k $：观测噪声，服从高斯分布 $ v_k \sim N(0, R) $，其中 $ R $ 为观测噪声协方差矩阵，描述观测噪声的不确定性。

目标：根据观测 $ z_k $ 和先前的估计，递归地计算状态 $ x_k $ 的最佳估计。??

概率框架与贝叶斯估计

卡尔曼滤波基于贝叶斯估计，利用先验信息和观测数据更新状态估计。核心是计算状态 $ x_k $ 的后验概率分布 $ p(x_k | z_{1:k}) $，其中 $ z_{1:k} $ 表示从时刻 1 到 $ k $ 的所有观测。

贝叶斯定理（Bayes’ Theorem）

贝叶斯定理是概率论中的一个基本公式，用于在有新证据时更新某一事件发生的概率。其核心思想是：通过已知的条件概率反推出感兴趣的概率。

条件概率的定义

设 $A$、$B$ 是两个概率大于 0 的事件，定义条件概率为：

$P(A \mid B) = \frac{P(A \cap B)}{P(B)} \tag{1}$

同理：

$P(B \mid A) = \frac{P(A \cap B)}{P(A)} \tag{2}$

两式右边的分子都是事件 $A$ 与 $B$ 同时发生的概率 $P(A \cap B)$。

贝叶斯定理的推导过程

从 (2) 式变形，解出联合概率：

$P(A \cap B) = P(B \mid A) \cdot P(A)$

代入式 (1) 中，得到：

$P(A \mid B) = \frac{P(B \mid A) \cdot P(A)}{P(B)}$

这就是贝叶斯定理的标准形式，其中：

$ P(A \mid B) $：后验概率（posterior），即在已知 $B$ 发生的前提下，事件 $A$ 发生的概率；
$ P(B \mid A) $：似然度（likelihood），即在 $A$ 已发生的前提下，$B$ 发生的概率；
$ P(A) $：先验概率（prior），即在未观察到 $B$ 前，事件 $A$ 的概率；
$ P(B) $：边际概率（marginal），即事件 $B$ 发生的总概率。

贝叶斯估计在卡尔曼滤波中的应用

根据贝叶斯定理：

$p(x_k | z_{1:k}) = \frac{p(z_k | x_k) p(x_k | z_{1:k-1})}{p(z_k | z_{1:k-1})}$

$p(x_k|z_{1:k})$：后验概率（posterior），表示在观测到 $z_{1:k}$ 后，状态 $x_k$ 的概率分布。
$p(z_k|x_k)$：似然概率（likelihood），表示在状态 $x_k$ 下观测到 $z_k$ 的概率。
$p(x_k|z_{1:k-1})$：先验概率（prior），表示在观测到 $z_k$ 之前，基于历史观测 $z_{1:k-1}$ 对状态 $x_k$ 的预测。
$p(z_k|z_{1:k-1})$：归一化常数（evidence），表示观测 $z_k$ 的边缘概率，用于归一化后验概率。

卡尔曼滤波通过以下两步实现贝叶斯估计：

预测（时间更新）：根据上一时刻的后验分布预测当前时刻的先验分布。
更新（测量更新）：利用贝叶斯定理结合观测更新为后验分布。

先验概率 $p(x_k | z_{1:k-1})$

贝叶斯定理中的部分：
$p(x_k | z_{1:k-1})$
卡尔曼滤波中的对应公式：
$p(x_k | z_{1:k-1}) \sim N(\hat{x}_{k|k-1}, P_{k|k-1})$
其中：
$\hat{x}_{k|k-1} = A \hat{x}_{k-1|k-1} + B u_{k-1}$ $P_{k|k-1} = A P_{k-1|k-1} A^T + Q$

似然概率 $p(z_k | x_k)$

贝叶斯定理中的部分：
$p(z_k | x_k)$
卡尔曼滤波中的对应公式：
$p(z_k | x_k) \sim N(H x_k, R)$

归一化常数 $p(z_k | z_{1:k-1})$

贝叶斯定理中的部分：
$p(z_k | z_{1:k-1})$ $p(z_k | z_{1:k-1}) = \int p(z_k | x_k) p(x_k | z_{1:k-1}) dx_k$
卡尔曼滤波中的对应公式：
$p(z_k | z_{1:k-1}) \sim N(H \hat{x}_{k|k-1}, H P_{k|k-1} H^T + R)$
其中：
$S_k = H P_{k|k-1} H^T + R$ $\tilde{y}_k = z_k - H \hat{x}_{k|k-1}$

后验概率 $p(x_k | z_{1:k})$

贝叶斯定理中的部分：
$p(x_k | z_{1:k})$
卡尔曼滤波中的对应公式：
$p(x_k | z_{1:k}) \sim N(\hat{x}_{k|k}, P_{k|k})$
其中：
$K_k = P_{k|k-1} H^T S_k^{-1}$ $\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k \tilde{y}_k$ $P_{k|k} = (I - K_k H) P_{k|k-1}$

协方差与协方差矩阵：
协方差是衡量两个随机变量之间线性相关性的统计量。对于随机变量 $ X $ 和 $ Y $，协方差定义为：
$\text{Cov}(X, Y) = E[(X - E[X])(Y - E[Y])]$

正协方差：$ X $ 和 $ Y $ 趋于同方向变化。

负协方差：$ X $ 和 $ Y $ 趋于反方向变化。

协方差为零：$ X $ 和 $ Y $ 无线性相关性。

协方差矩阵是协方差的推广，适用于随机向量。对于 $ n $ 维随机向量 $ \mathbf{x} = [x_1, x_2, \dots, x_n]^T $，协方差矩阵 $ \Sigma $ 为：
$\Sigma = E[(\mathbf{x} - E[\mathbf{x}])(\mathbf{x} - E[\mathbf{x}])^T]$

对角线元素 $ \Sigma_{ii} = \text{Var}(x_i) $：表示 $ x_i $ 的方差。

非对角线元素 $ \Sigma_{ij} = \text{Cov}(x_i, x_j) $：表示 $ x_i $ 和 $ x_j $ 的协方差。

在卡尔曼滤波中，协方差矩阵（如 $ P_{k|k-1} $ 和 $ P_{k|k} $）描述状态估计的不确定性，是算法的核心组成部分。

预测步骤（时间更新）

状态预测

根据状态转移方程：

$x_k = A x_{k-1} + B u_{k-1} + w_{k-1}$

由于 $ w_{k-1} $ 是零均值高斯噪声，取期望得到先验状态估计：

$\hat{x}_{k|k-1} = E[x_k | z_{1:k-1}] = A \hat{x}_{k-1|k-1} + B u_{k-1}$

$ \hat{x}_{k-1|k-1} $：时刻 $ k-1 $ 的后验状态估计，上一时刻更新后的估计值。

协方差预测

定义先验误差为：

$e_{k|k-1} = x_k - \hat{x}_{k|k-1}$

代入状态转移方程：

$e_{k|k-1} = x_k - \hat{x}_{k|k-1} = A x_{k-1} + B u_{k-1} + w_{k-1} - (A \hat{x}_{k-1|k-1} + B u_{k-1})$ $= A (x_{k-1} - \hat{x}_{k-1|k-1}) + w_{k-1} = A e_{k-1|k-1} + w_{k-1}$

$ e_{k-1|k-1} $：后验误差，即 $ x_{k-1} - \hat{x}_{k-1|k-1} $。

先验协方差矩阵 $ P_{k|k-1} $ 是先验误差的协方差：

$P_{k|k-1} = E[e_{k|k-1} e_{k|k-1}^T]$

展开：

$e_{k|k-1} e_{k|k-1}^T = (A e_{k-1|k-1} + w_{k-1})(A e_{k-1|k-1} + w_{k-1})^T$ $= A e_{k-1|k-1} e_{k-1|k-1}^T A^T + A e_{k-1|k-1} w_{k-1}^T + w_{k-1} e_{k-1|k-1}^T A^T + w_{k-1} w_{k-1}^T$

取期望：

$P_{k|k-1} = E[A e_{k-1|k-1} e_{k-1|k-1}^T A^T] + E[A e_{k-1|k-1} w_{k-1}^T] + E[w_{k-1} e_{k-1|k-1}^T A^T] + E[w_{k-1} w_{k-1}^T]$

由于 $ e_{k-1|k-1} $ 是上一时刻的后验误差，与当前时刻的过程噪声 $ w_{k-1} $ 独立，且 $ E[w_{k-1}] = 0 $，交叉项为零：

$E[A e_{k-1|k-1} w_{k-1}^T] = A E[e_{k-1|k-1}] E[w_{k-1}^T] = 0$ $E[w_{k-1} e_{k-1|k-1}^T A^T] = E[w_{k-1}] E[e_{k-1|k-1}^T] A^T = 0$

因此：

$P_{k|k-1} = A E[e_{k-1|k-1} e_{k-1|k-1}^T] A^T + E[w_{k-1} w_{k-1}^T] = A P_{k-1|k-1} A^T + Q$

更新步骤（测量更新）

观测残差

观测残差（或创新）是实际观测与预测观测之差：

$\tilde{y}_k = z_k - H \hat{x}_{k|k-1}$

创新协方差 $ S_k $ 是观测残差的协方差：

$S_k = E[\tilde{y}_k \tilde{y}_k^T]$

推导：

$\tilde{y}_k = z_k - H \hat{x}_{k|k-1} = H x_k + v_k - H \hat{x}_{k|k-1} = H (x_k - \hat{x}_{k|k-1}) + v_k = H e_{k|k-1} + v_k$ $S_k = E[(H e_{k|k-1} + v_k)(H e_{k|k-1} + v_k)^T]$ $= E[H e_{k|k-1} e_{k|k-1}^T H^T + H e_{k|k-1} v_k^T + v_k e_{k|k-1}^T H^T + v_k v_k^T]$

由于 $ e_{k|k-1} $ 和 $ v_k $ 独立，且 $ E[v_k] = 0 $：

$E[H e_{k|k-1} v_k^T] = H E[e_{k|k-1}] E[v_k^T] = 0$ $E[v_k e_{k|k-1}^T H^T] = E[v_k] E[e_{k|k-1}^T] H^T = 0$

因此：

$S_k = H E[e_{k|k-1} e_{k|k-1}^T] H^T + E[v_k v_k^T] = H P_{k|k-1} H^T + R$

$ S_k $ 是以下两部分的综合：

先验预测的不确定性：$ H P_{k|k-1} H^T $ 表示先验状态估计的不确定性通过观测矩阵映射到观测空间。

观测噪声的不确定性：$ R $ 表示传感器本身的噪声特性。

卡尔曼增益

卡尔曼增益 $ K_k $ 是先验估计和观测的权衡因子：

$K_k = P_{k|k-1} H^T S_k^{-1}$

它决定了观测残差对状态估计的修正程度。

卡尔曼增益的推导
卡尔曼增益 $ K_k $ 是状态估计的关键，目标是通过最小化后验误差协方差得到最优估计。这里使用最小均方误差（MMSE）方法推导 $ K_k = P_{k|k-1} H^T S_k^{-1} $。

后验状态估计定义为：

$\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k \tilde{y}_k$

后验误差：

$e_{k|k} = x_k - \hat{x}_{k|k} = (I - K_k H) e_{k|k-1} - K_k v_k$

后验协方差：

$P_{k|k} = E[e_{k|k} e_{k|k}^T] = (I - K_k H) P_{k|k-1} (I - K_k H)^T + K_k R K_k^T$

目标是最小化 $ P_{k|k} $ 的迹（$\text{tr}(P_{k|k})$），对 $ K_k $ 求导：

$\frac{\partial \text{tr}(P_{k|k})}{\partial K_k} = -2 (I - K_k H) P_{k|k-1} H^T + 2 K_k (H P_{k|k-1} H^T + R) = 0$

解得：

$K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1} = P_{k|k-1} H^T S_k^{-1}$

状态更新

后验状态估计通过先验估计加上残差的加权修正得到：

$\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k \tilde{y}_k$

协方差更新

后验协方差矩阵表示更新后的不确定性：

$P_{k|k} = E[(x_k - \hat{x}_{k|k})(x_k - \hat{x}_{k|k})^T]$

代入 $ \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k \tilde{y}_k $：

$x_k - \hat{x}_{k|k} = x_k - (\hat{x}_{k|k-1} + K_k (z_k - H \hat{x}_{k|k-1}))$ $= x_k - \hat{x}_{k|k-1} - K_k (H x_k + v_k - H \hat{x}_{k|k-1}) = (x_k - \hat{x}_{k|k-1}) - K_k H (x_k - \hat{x}_{k|k-1}) - K_k v_k$ $= (I - K_k H) e_{k|k-1} - K_k v_k$

计算协方差：

$P_{k|k} = E[((I - K_k H) e_{k|k-1} - K_k v_k)((I - K_k H) e_{k|k-1} - K_k v_k)^T]$

展开并利用 $ e_{k|k-1} $ 和 $ v_k $ 独立性，简化得：

$P_{k|k} = (I - K_k H) P_{k|k-1}$

完整算法流程

初始化：

初始状态估计：$ \hat{x}_{0|0} $
初始协方差矩阵：$ P_{0|0} $

对于每个时刻 $ k = 1, 2, \dots $：

预测：
- $ \hat{x}_{k|k-1} = A \hat{x}_{k-1|k-1} + B u_{k-1} $
- $ P_{k|k-1} = A P_{k-1|k-1} A^T + Q $
更新：
- $ \tilde{y}_k = z_k - H \hat{x}_{k|k-1} $
- $ S_k = H P_{k|k-1} H^T + R $
- $ K_k = P_{k|k-1} H^T S_k^{-1} $
- $ \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k \tilde{y}_k $
- $ P_{k|k} = (I - K_k H) P_{k|k-1} $

强化理解

预测

这一阶段利用系统模型从上一时刻的状态估计当前时刻的状态。

$ \hat{x}_{k|k-1} = A \hat{x}_{k-1|k-1} + B u_{k-1} $
- 物理意义：
  $ \hat{x}_{k|k-1} $ 是当前时刻的先验状态估计，表示在未使用当前测量数据之前，根据系统动态模型预测的状态。
  - $ A \hat{x}_{k-1|k-1} $：
    通过状态转移矩阵 $ A $ 将上一时刻的最优状态 $ \hat{x}_{k-1|k-1} $（如位置、速度）映射到当前时刻。例如，若物体匀速运动，位置增加量为速度乘以时间间隔。
  - $ B u_{k-1} $：
    表示外部控制输入（如加速度、油门）对状态的影响，通过控制矩阵 $ B $ 映射到状态空间。若无控制输入，则此项为零。
  - 直观理解：
    比如一辆车，上一秒位置是10米，速度是2米/秒，预测下一秒位置是12米（假设无加速度控制）。
$ P_{k|k-1} = A P_{k-1|k-1} A^T + Q $
- 物理意义：
  $ P_{k|k-1} $ 是先验协方差矩阵，表示预测状态 $ \hat{x}_{k|k-1} $ 的不确定性。
  - $ A P_{k-1|k-1} A^T $：
    将上一时刻的后验协方差 $ P_{k-1|k-1} $（上一时刻状态的不确定性）通过状态转移传播到当前时刻。不确定性可能因系统动态而放大或缩小。
  - $ Q $：
    过程噪声协方差，表示系统模型的不完美性（如风速变化、路面不平引起的随机扰动）。
  - 直观理解：
    预测时间越长（或模型越不准），不确定性越大，$ P_{k|k-1} $ 的值会增加。例如，预测一辆车的未来位置时，风向变化会增加位置的不确定性。

更新

这一阶段利用当前测量数据修正预测值，得到更准确的状态估计。

$ \tilde{y}_k = z_k - H \hat{x}_{k|k-1} $
- 物理意义：
  $ \tilde{y}_k $ 是测量残差（或创新），表示实际测量值与预测测量值之间的偏差。
  - $ z_k $：
    当前时刻的传感器测量值（如GPS报告的位置）。
  - $ H \hat{x}_{k|k-1} $：
    通过观测矩阵 $ H $ 将预测状态映射到测量空间的预期测量值。例如，若状态包括位置和速度，而传感器只测位置，则 $ H $ 提取位置部分。
  - 直观理解：
    如果预测位置是12米，测量值是11.8米，残差 $ \tilde{y}_k = 11.8 - 12 = -0.2 $ 米，表示预测偏高。
$ S_k = H P_{k|k-1} H^T + R $
- 物理意义：
  $ S_k $ 是测量残差的协方差矩阵，表示残差 $ \tilde{y}_k $ 的不确定性，综合了预测和测量的不可靠性。
  - $ H P_{k|k-1} H^T $：
    预测状态的不确定性 $ P_{k|k-1} $ 在测量空间的投影，反映预测的可信度对残差的影响。
  - $ R $：
    测量噪声协方差，表示传感器测量的随机误差（如GPS的精度误差）。
  - 直观理解：
    $ S_k $ 越大，说明残差受预测或测量噪声影响越大，可信度越低。例如，若GPS精度差，$ R $ 大，$ S_k $ 增加。
$ K_k = P_{k|k-1} H^T S_k^{-1} $
- 物理意义：
  $ K_k $ 是卡尔曼增益，决定预测和测量在状态修正中的相对权重，反映了两者的可信度平衡。
  - $ P_{k|k-1} H^T $：
    表示预测状态的不确定性如何影响测量空间。若 $ P_{k|k-1} $ 大（预测不可靠），$ K_k $ 倾向于更大。
  - $ S_k^{-1} $：
    残差协方差的逆。若 $ S_k $ 大（残差不可信），$ S_k^{-1} $ 小，$ K_k $ 减小。
  - 直观理解：
    若预测位置不确定性高（$ P_{k|k-1} $ 大），而测量较可靠（$ R $ 小），$ K_k $ 接近1，更信任测量；反之，若测量噪声大，$ K_k $ 接近0，更信任预测。
$ \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k \tilde{y}_k $
- 物理意义：
  $ \hat{x}_{k|k} $ 是后验状态估计，结合预测和测量信息后的最优状态估计。
  - $ \hat{x}_{k|k-1} $：
    先验状态估计，作为更新的起点。
  - $ K_k \tilde{y}_k $：
    根据卡尔曼增益和测量残差对预测值的修正量。
  - 直观理解：
    预测位置12米，测量11.8米，若 $ K_k = 0.6 $，则 $ \hat{x}_{k|k} = 12 + 0.6 \cdot (11.8 - 12) = 11.88 $ 米，融合了两者的结果。
$ P_{k|k} = (I - K_k H) P_{k|k-1} $
- 物理意义：
  $ P_{k|k} $ 是后验协方差矩阵，表示后验状态估计 $ \hat{x}_{k|k} $ 的不确定性。
  - $ (I - K_k H) $：
    一个缩减因子，反映测量信息如何减少预测的不确定性。
  - $ P_{k|k-1} $：
    先验协方差，作为更新的基础。
  - 直观理解：
    融合测量后，不确定性通常减小。例如，若 $ K_k $ 大（信任测量），$ P_{k|k} $ 显著小于 $ P_{k|k-1} $，表示状态估计更可信。

总结

KF 通过预测和更新递归优化状态估计，适用于线性高斯系统，具有高效性和最优性。

扩展卡尔曼滤波（EKF）

问题背景与基本假设

EKF 用于非线性动态系统的状态估计，假设噪声为高斯，通过局部线性化处理非线性。

数学模型与线性化

状态转移方程：

$x_k = f(x_{k-1}, u_{k-1}, w_{k-1})$

$ f(\cdot) $：非线性状态转移函数。

观测方程：

$z_k = h(x_k, v_k)$

$ h(\cdot) $：非线性观测函数。

线性化

状态转移函数在 $ \hat{x}_{k-1|k-1} $ 处： $F_k = \frac{\partial f}{\partial x} \bigg|_{x = \hat{x}_{k-1|k-1}, u_{k-1}, w_{k-1}=0}$
观测函数在 $ \hat{x}_{k|k-1} $ 处： $H_k = \frac{\partial h}{\partial x} \bigg|_{x = \hat{x}_{k|k-1}, v_k=0}$

预测步骤（时间更新）

状态预测

根据非线性状态转移方程：

$\hat{x}_{k|k-1} = f(\hat{x}_{k-1|k-1}, u_{k-1}, 0)$

$ \hat{x}_{k|k-1} $ 是先验状态估计，直接用非线性函数 $ f $ 计算，假设噪声均值为零。
协方差预测
定义先验误差： $e_{k|k-1} = x_k - \hat{x}_{k|k-1}$ 使用一阶泰勒展开近似： $x_k \approx f(\hat{x}_{k-1|k-1}, u_{k-1}, 0) + F_k (x_{k-1} - \hat{x}_{k-1|k-1}) + w_{k-1}$ $e_{k|k-1} \approx F_k e_{k-1|k-1} + w_{k-1}$ 先验协方差矩阵： $P_{k|k-1} = E[e_{k|k-1} e_{k|k-1}^T] = F_k P_{k-1|k-1} F_k^T + Q$

更新步骤（测量更新）

观测残差

$\tilde{y}_k = z_k - h(\hat{x}_{k|k-1}, 0)$

线性化后：

$\tilde{y}_k \approx H_k e_{k|k-1} + v_k$

创新协方差

$S_k = H_k P_{k|k-1} H_k^T + R$

卡尔曼增益

$K_k = P_{k|k-1} H_k^T S_k^{-1}$

状态更新

$\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k \tilde{y}_k$

协方差更新

$P_{k|k} = (I - K_k H_k) P_{k|k-1}$

完整算法流程

初始化：

$ \hat{x}_{0|0} $，$ P_{0|0} $

对于每个时刻 $ k = 1, 2, \dots $：

预测：
- $ \hat{x}_{k|k-1} = f(\hat{x}_{k-1|k-1}, u_{k-1}, 0) $
- $ F_k = \frac{\partial f}{\partial x} \bigg|_{x = \hat{x}_{k-1|k-1}, u_{k-1}, w_{k-1}=0} $
- $ P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q $
更新：
- $ \tilde{y}_k = z_k - h(\hat{x}_{k|k-1}, 0) $
- $ H_k = \frac{\partial h}{\partial x} \bigg|_{x = \hat{x}_{k|k-1}, v_k=0} $
- $ S_k = H_k P_{k|k-1} H_k^T + R $
- $ K_k = P_{k|k-1} H_k^T S_k^{-1} $
- $ \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k \tilde{y}_k $
- $ P_{k|k} = (I - K_k H_k) P_{k|k-1} $

总结

EKF 通过线性化处理非线性，适用于弱非线性系统，但强非线性时线性化误差可能影响精度。

无迹卡尔曼滤波（UKF）

问题背景与基本假设

UKF 用于非线性动态系统，通过无迹变换捕捉非线性特性，避免线性化误差。假设噪声为高斯。

数学模型与无迹变换

状态转移方程：

$x_k = f(x_{k-1}, u_{k-1}, w_{k-1})$

观测方程：

$z_k = h(x_k, v_k)$

无迹变换

无迹变换通过一组精心选择的采样点（Sigma 点）传播非线性函数：

对于 $ n $ 维状态向量 $ x \sim N(\hat{x}, P) $，生成 $ 2n + 1 $ 个 Sigma 点。
将这些点通过非线性函数 $ f $ 或 $ h $ 传播，计算变换后的均值和协方差。
参数：
- $ \lambda = \alpha^2 (n + \kappa) - n $：缩放参数。
- $ \alpha $：控制 Sigma 点分布范围（通常取 $ 10^{-3} \sim 1 $）。
- $ \kappa $：次要缩放参数（通常取 0 或 $ 3 - n $）。
- $ \beta $：融入高阶信息（对于高斯分布，$ \beta = 2 $ 最优）。

UKF 的步骤分为预测和更新两部分，使用 Sigma 点传播非线性。

预测步骤（时间更新）

生成 Sigma 点

基于上一时刻的后验估计 $ \hat{x}_{k-1|k-1} $ 和 $ P_{k-1|k-1} $：

$\mathcal{X}_{k-1|k-1}^{(0)} = \hat{x}_{k-1|k-1}$ $\mathcal{X}_{k-1|k-1}^{(i)} = \hat{x}_{k-1|k-1} + (\sqrt{(n + \lambda) P_{k-1|k-1}})_i, \quad i = 1, \dots, n$ $\mathcal{X}_{k-1|k-1}^{(i)} = \hat{x}_{k-1|k-1} - (\sqrt{(n + \lambda) P_{k-1|k-1}})_{i-n}, \quad i = n+1, \dots, 2n$

$ \lambda = \alpha^2 (n + \kappa) - n $。
$ (\sqrt{(n + \lambda) P_{k-1|k-1}})_i $ 是矩阵平方根（如 Cholesky 分解）的第 $ i $ 列。

状态预测

将 Sigma 点通过状态转移函数传播：

$\mathcal{X}_{k|k-1}^{(i)} = f(\mathcal{X}_{k-1|k-1}^{(i)}, u_{k-1}, 0)$

加权平均计算先验状态估计：

$\hat{x}_{k|k-1} = \sum_{i=0}^{2n} W_m^{(i)} \mathcal{X}_{k|k-1}^{(i)}$

权重：

$W_m^{(0)} = \frac{\lambda}{n + \lambda}, \quad W_m^{(i)} = \frac{1}{2(n + \lambda)}, \quad i = 1, \dots, 2n$

协方差预测

计算先验协方差：

$P_{k|k-1} = \sum_{i=0}^{2n} W_c^{(i)} (\mathcal{X}_{k|k-1}^{(i)} - \hat{x}_{k|k-1})(\mathcal{X}_{k|k-1}^{(i)} - \hat{x}_{k|k-1})^T + Q$

权重：

$W_c^{(0)} = \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta), \quad W_c^{(i)} = \frac{1}{2(n + \lambda)}, \quad i = 1, \dots, 2n$

更新步骤（测量更新）

观测预测

将 Sigma 点通过观测函数传播：

$\mathcal{Z}_{k|k-1}^{(i)} = h(\mathcal{X}_{k|k-1}^{(i)}, 0), \quad i = 0, \dots, 2n$

预测观测均值：

$\hat{z}_{k|k-1} = \sum_{i=0}^{2n} W_m^{(i)} \mathcal{Z}_{k|k-1}^{(i)}$

创新协方差与交叉协方差

创新协方差：

$S_k = \sum_{i=0}^{2n} W_c^{(i)} (\mathcal{Z}_{k|k-1}^{(i)} - \hat{z}_{k|k-1})(\mathcal{Z}_{k|k-1}^{(i)} - \hat{z}_{k|k-1})^T + R$

状态-观测交叉协方差：

$P_{xz,k} = \sum_{i=0}^{2n} W_c^{(i)} (\mathcal{X}_{k|k-1}^{(i)} - \hat{x}_{k|k-1})(\mathcal{Z}_{k|k-1}^{(i)} - \hat{z}_{k|k-1})^T$

卡尔曼增益

$K_k = P_{xz,k} S_k^{-1}$

状态更新

观测残差：

$\tilde{y}_k = z_k - \hat{z}_{k|k-1}$

后验状态估计：

$\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k \tilde{y}_k$

协方差更新

$P_{k|k} = P_{k|k-1} - K_k S_k K_k^T$

完整算法流程

初始化：

初始状态估计：$ \hat{x}_{0|0} $
初始协方差矩阵：$ P_{0|0} $

对于每个时刻 $ k = 1, 2, \dots $：

预测：
- 生成 Sigma 点：$ \mathcal{X}_{k-1|k-1}^{(i)} $
- $ \mathcal{X}_{k|k-1}^{(i)} = f(\mathcal{X}_{k-1|k-1}^{(i)}, u_{k-1}, 0) $
- $ \hat{x}_{k|k-1} = \sum_{i=0}^{2n} W_m^{(i)} \mathcal{X}_{k|k-1}^{(i)} $
- $ P_{k|k-1} = \sum_{i=0}^{2n} W_c^{(i)} (\mathcal{X}_{k|k-1}^{(i)} - \hat{x}_{k|k-1})(\mathcal{X}_{k|k-1}^{(i)} - \hat{x}_{k|k-1})^T + Q $
更新：
- $ \mathcal{Z}_{k|k-1}^{(i)} = h(\mathcal{X}_{k|k-1}^{(i)}, 0) $
- $ \hat{z}_{k|k-1} = \sum_{i=0}^{2n} W_m^{(i)} \mathcal{Z}_{k|k-1}^{(i)} $
- $ S_k = \sum_{i=0}^{2n} W_c^{(i)} (\mathcal{Z}_{k|k-1}^{(i)} - \hat{z}_{k|k-1})(\mathcal{Z}_{k|k-1}^{(i)} - \hat{z}_{k|k-1})^T + R $
- $ P_{xz,k} = \sum_{i=0}^{2n} W_c^{(i)} (\mathcal{X}_{k|k-1}^{(i)} - \hat{x}_{k|k-1})(\mathcal{Z}_{k|k-1}^{(i)} - \hat{z}_{k|k-1})^T $
- $ K_k = P_{xz,k} S_k^{-1} $
- $ \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - \hat{z}_{k|k-1}) $
- $ P_{k|k} = P_{k|k-1} - K_k S_k K_k^T $
  总结

UKF 通过无迹变换处理非线性，精度高于 EKF，适用于强非线性系统，但计算复杂度较高。

三种滤波器的对比

滤波器	适用系统	方法	优点	缺点
KF	线性高斯系统	线性代数	计算高效、最优	不适用于非线性系统
EKF	弱非线性系统	线性化（雅可比）	扩展到非线性、计算较快	线性化误差、需计算导数
UKF	强非线性系统	无迹变换	精度高、无需导数	计算复杂度高

选择滤波器时需根据系统特性权衡精度和计算成本。

学习

基本卡尔曼滤波（KF）

问题背景与基本假设

状态转移方程：

观测方程：

概率框架与贝叶斯估计

贝叶斯定理（Bayes’ Theorem）

贝叶斯估计在卡尔曼滤波中的应用

先验概率 $p(x_k | z_{1:k-1})$

似然概率 $p(z_k | x_k)$

归一化常数 $p(z_k | z_{1:k-1})$

后验概率 $p(x_k | z_{1:k})$

预测步骤（时间更新）

状态预测

协方差预测

更新步骤（测量更新）

观测残差

卡尔曼增益

状态更新

协方差更新

完整算法流程

强化理解

预测

更新

总结

扩展卡尔曼滤波（EKF）

问题背景与基本假设

数学模型与线性化

状态转移方程：

观测方程：

线性化

预测步骤（时间更新）

状态预测

协方差预测

更新步骤（测量更新）

观测残差

创新协方差

卡尔曼增益

状态更新

协方差更新

完整算法流程

总结

无迹卡尔曼滤波（UKF）

问题背景与基本假设

数学模型与无迹变换

状态转移方程：

观测方程：

无迹变换

预测步骤（时间更新）

生成 Sigma 点

状态预测

协方差预测

更新步骤（测量更新）

观测预测

创新协方差与交叉协方差

卡尔曼增益

状态更新

协方差更新

完整算法流程

总结

三种滤波器的对比