卡尔曼滤波

知识储备

适合小白学习：https://blog.csdn.net/navsense_support/article/details/146231513

进阶及高阶：（1）https://zhuanlan.zhihu.com/p/649997859
(2) https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2 (维基百科上的，有点复杂)

卡尔曼滤波

在工程应用中，滤波器有很多种，而卡尔曼滤波器实际上是一种高效率的递归时域滤波器（自回归滤波器），先说一下其使用的前提（选择滤波器首先要考虑的）：

• 当前时刻状态只和上一时刻状态有关。
• 模型和系统均满足线性关系。
• 引入的噪声符合零均值高斯分布。

我先对该前提条件进行分析：

第一个条件说明了该滤波器的时域性，不适合频域滤波；

第二个条件很大程度上限制了其应用场景，毕竟线性条件终归是少数，但可以通过泰勒展开等方式将非线性条件转化为线性（如EKF），这样就说明了卡尔曼滤波器是学习其他滤波器的基础。（个人感觉这是其这么重要最主要的原因之一）

第三个条件说明了所引入的噪声类型（其实就是实际操作时的误差分布）。

总的逻辑图如下所示：

下面就是公式推导了：

（可以分为状态空间与测量空间两部分进行）

其推导本质是系统本身会有一个“按照模型推出来的状态”，传感器又会给出一个“带噪声的测量值”，卡尔曼滤波要做的就是在二者之间取一个合理的加权结果。

1. 状态空间模型

卡尔曼滤波的核心目标是：根据上一时刻的状态估计值和当前时刻的观测值，递推得到当前时刻更准确的状态估计值。

假设系统状态满足线性递推关系：

$$x_k=A{x}_{k-1}+B{u}_k+w_k$$

其中：

• $x_k$：第 $k$ 时刻的真实状态；
• $A$：状态转移矩阵，用来描述系统状态如何从上一时刻变化到当前时刻；
• $B$：控制矩阵；
• $u_k$：控制输入；
• $w_k$：过程噪声，通常假设 $w_k\sim N(0,Q)$。

观测方程写成：

$$z_k=H{x}_k+v_k$$

其中：

• $z_k$：第 $k$ 时刻的观测值；
• $H$：观测矩阵，用来把状态空间映射到观测空间；
• $v_k$：观测噪声，通常假设 $v_k\sim N(0,R)$。

2. 预测阶段

由于当前时刻的真实状态 $x_k$ 无法直接得到，所以先用上一时刻的最优估计值 ${\hat{x}}_{k-1}$ 来预测当前状态：

$${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_k$$

这里的 ${\hat{x}}_k^{-}$ 表示“先验估计”，也就是还没有结合当前观测值时的估计。

估计值本身存在误差，定义估计误差协方差矩阵为：

$$P_k=E[(x_k-{\hat{x}}_k)(x_k-{\hat{x}}_k{)}^T]$$

预测阶段对应的误差协方差为：

$$P_k^{-}=A{P}_{k-1}{A}^T+Q$$

这个式子的含义是：上一时刻的不确定性经过状态转移矩阵 $A$ 传播到当前时刻，同时系统运动过程中还会额外引入过程噪声 $Q$。

3. 更新阶段

当第 $k$ 时刻的观测值 $z_k$ 到来后，需要将预测值和观测值融合。先计算预测出来的观测值：

$${\hat{z}}_k=H{\hat{x}}_k^{-}$$

实际观测值和预测观测值之间的差称为残差：

$$y_k=z_k-H{\hat{x}}_k^{-}$$

如果残差很大，说明预测值和测量值差距较大；如果残差很小，说明模型预测和传感器观测比较一致。

将先验估计修正为后验估计：

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

其中 $K_k$ 是卡尔曼增益，它决定了“更相信预测值”还是“更相信观测值”。如果 $K_k$ 较大，说明观测值权重更高；如果 $K_k$ 较小，说明预测值权重更高。

4. 卡尔曼增益推导

为了求出最优的 $K_k$，目标是让更新后的估计误差协方差尽可能小。先写出后验估计误差：

$$e_k=x_k-{\hat{x}}_k$$

代入更新公式：

$$\begin{array}{rl}{e}_k& =x_k-\left[{\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})\right]\\ & =x_k-{\hat{x}}_k^{-}-K_k(H{x}_k+v_k-H{\hat{x}}_k^{-})\\ & =(I-K_{k}H)(x_k-{\hat{x}}_k^{-})-K_k{v}_k\end{array}$$

因此后验误差协方差为：

$$P_k=E[e_k{e}_k^T]$$

在过程噪声和观测噪声相互独立的条件下，可得：

$$P_k=(I-K_{k}H)P_k^{-}(I-K_{k}H{)}^T+K_{k}R{K}_k^T$$

为了让估计误差最小，需要对 $P_k$ 的迹求关于 $K_k$ 的最小值，最终可以得到卡尔曼增益：

$$K_k=P_k^{-}{H}^T(H{P}_k^{-}{H}^T+R{)}^{-1}$$

这个式子很重要，可以从直观上理解：

• 当观测噪声 $R$ 较大时，说明传感器测量不可靠，$K_k$ 会变小，滤波器更相信模型预测；
• 当预测误差协方差 $P_k^{-}$ 较大时，说明模型预测不可靠，$K_k$ 会变大，滤波器更相信观测值。

最后，更新误差协方差：

$$P_k=(I-K_{k}H)P_k^{-}$$

更完整、更稳定的写法是 Joseph 形式：

$$P_k=(I-K_{k}H)P_k^{-}(I-K_{k}H{)}^T+K_{k}R{K}_k^T$$

实际程序中如果只是完成基础卡尔曼滤波，常用简化式即可；如果对数值稳定性要求更高，则建议使用 Joseph 形式。

5. 算法流程总结

综合上面的推导，卡尔曼滤波每一步可以总结为以下五个公式：

预测状态：

$${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_k$$

预测误差协方差：

$$P_k^{-}=A{P}_{k-1}{A}^T+Q$$

计算卡尔曼增益：

$$K_k=P_k^{-}{H}^T(H{P}_k^{-}{H}^T+R{)}^{-1}$$

更新状态估计：

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

更新误差协方差：

$$P_k=(I-K_{k}H)P_k^{-}$$

不同滤波器的对比

前面推导的是最基础的线性卡尔曼滤波器（KF）。但在实际机器人、自动驾驶、导航定位等问题中，系统往往不是严格线性的，所以普通 KF 会衍生出很多变体。这里主要对比扩展卡尔曼滤波（EKF）、误差状态卡尔曼滤波（ESKF）和粒子滤波（PF）。

1. 普通卡尔曼滤波（KF）

普通卡尔曼滤波适用于线性系统：

$$x_k=A{x}_{k-1}+B{u}_k+w_k$$

$$z_k=H{x}_k+v_k$$

它的特点是公式清晰、计算量小、实时性好，而且在“线性系统 + 高斯噪声”的条件下可以得到最优估计。

但是它的限制也很明显：系统模型和观测模型必须是线性的。如果系统存在明显非线性，比如机器人姿态角变化、相机观测、IMU 积分等，直接使用普通 KF 就不太合适。

常见应用场景：

• 一维或二维匀速、匀加速目标跟踪；
• 传感器数据平滑；
• 线性系统状态估计；
• 对实时性要求高、模型较简单的控制系统。

2. 扩展卡尔曼滤波（EKF）

扩展卡尔曼滤波是普通卡尔曼滤波在非线性系统中的推广。它处理的状态方程和观测方程一般写成：

$$x_k=f(x_{k-1},u_k)+w_k$$

$$z_k=h(x_k)+v_k$$

其中 $f(\cdot )$ 和 $h(\cdot )$ 可以是非线性函数。EKF 的核心思想是：在当前估计点附近对非线性函数进行一阶泰勒展开，把非线性问题局部近似成线性问题。

例如状态转移矩阵不再是固定的 $A$，而是由雅可比矩阵代替：

$$F_k=\frac{\partial f}{\partial x}{|}_{{\hat{x}}_{k-1}}$$

观测矩阵也不再是固定的 $H$，而是：

$$H_k=\frac{\partial h}{\partial x}{|}_{{\hat{x}}_k^{-}}$$

所以 EKF 和 KF 的整体流程很像，仍然分为预测和更新两步，只是预测函数和观测函数换成了非线性函数，同时协方差传播中使用雅可比矩阵。

EKF 的优点是计算量仍然比较小，适合实时系统；缺点是一阶线性化会带来近似误差，如果系统非线性很强，或者初始估计偏差很大，滤波结果可能发散。

常见应用场景：

• 移动机器人定位；
• GPS/IMU 融合；
• SLAM 中的位姿估计；
• 无人车、无人机、轮式机器人等非线性运动模型。

3. 误差状态卡尔曼滤波（ESKF）

误差状态卡尔曼滤波可以看作 EKF 的一种改进形式，常用于惯性导航和多传感器融合。它和 EKF 最大的不同点在于：EKF 直接估计系统的完整状态，而 ESKF 主要估计“误差状态”。

例如真实状态可以表示为：

$$x=\hat{x}\oplus \delta x$$

其中：

• $\hat{x}$：名义状态，也就是当前认为比较接近真实值的状态；
• $\delta x$：误差状态，表示真实状态和名义状态之间的小偏差；
• $\oplus$：状态叠加运算，对于位置、速度可以理解为普通加法，对于姿态通常要用旋转群上的运算。

ESKF 的基本思路是：先用 IMU 等高频传感器传播名义状态，再用卡尔曼滤波估计误差状态，最后把误差补偿回名义状态中。补偿后通常还会把误差状态重置为零。

它的优势是误差状态通常比较小，因此线性化更准确，数值稳定性更好，尤其适合姿态、旋转等非线性较强的问题。

常见应用场景：

• IMU 预积分；
• 惯性导航系统；
• GPS/IMU/轮速计融合；
• 视觉惯性里程计（VIO）；
• 自动驾驶和机器人中的高频状态估计。

4. 粒子滤波（PF）

粒子滤波和卡尔曼滤波系列的思路差别更大。KF、EKF、ESKF 通常都依赖“高斯分布”假设，而粒子滤波不要求状态分布一定是高斯分布。

粒子滤波用大量粒子来表示状态的概率分布：

{xk(i),wk(i)}i=1N \{x_k^{(i)}, w_k^{(i)}\}_{i=1}^{N} {xk(i)​,wk(i)​}i=1N​

其中 $x_k^{(i)}$ 表示第 $i$ 个粒子的状态，$w_k^{(i)}$ 表示该粒子的权重。每个粒子都代表一种可能的状态，权重越大，说明这个粒子对应的状态越可能是真实状态。

粒子滤波的一般流程是：

1. 根据运动模型对粒子进行预测；
2. 根据观测值计算每个粒子的权重；
3. 对权重进行归一化；
4. 根据权重进行重采样，保留更可能的粒子；
5. 根据粒子集合得到最终估计结果。

粒子滤波的优点是可以处理强非线性、非高斯、多峰分布等复杂问题；缺点是计算量较大，粒子数量不足时容易退化，实时性通常不如卡尔曼滤波系列。

常见应用场景：

• 机器人全局定位；
• 非高斯噪声下的状态估计；
• 多假设目标跟踪；
• 地图匹配；
• 状态分布可能出现多个峰值的问题。

5. 总体对比

简单来说，这几种滤波器的关系可以这样理解：

• KF 是基础，适合线性高斯系统；
• EKF 是 KF 面向非线性系统的扩展，通过局部线性化继续使用卡尔曼滤波框架；
• ESKF 是 EKF 在误差状态上的一种常用形式，特别适合 IMU、姿态和导航问题；
• PF 不再局限于高斯假设，而是用粒子近似概率分布，适合更复杂但计算量更大的问题。

如果从适用范围看，大致可以总结为：

• 系统线性、噪声近似高斯：优先使用 KF；
• 系统轻度或中度非线性、要求实时性：常用 EKF；
• 涉及 IMU、姿态、惯性导航、多传感器融合：常用 ESKF；
• 系统强非线性、非高斯或存在多种可能状态：可以考虑 PF。

因此，普通卡尔曼滤波可以看作整个卡尔曼滤波器家族的基础；EKF 和 ESKF 主要是在非线性机器人系统中继续沿用“预测 + 更新”的思想；粒子滤波则是另一类基于概率采样的估计方法，适用范围更广，但计算成本也更高。