KF、EKF、ESKF的区别与联系

背景

滤波：去除噪声还原真实数据的一种数据处理方法，被广泛应用于对信号准确度有需求的众多领域，例如军事、航天、通信等等。
常用滤波方法：以低通滤波、带通滤波、高通滤波为代表的按照频率滤波的方式；以卡尔曼滤波、粒子滤波为代表的贝叶斯滤波系列。此外，常用的还有滑动窗口滤波、均值滤波和中值滤波等方法。

贝叶斯滤波系列

贝叶斯滤波：基于贝叶斯公式推导得到的一种滤波理论框架，其本身不是一种具体的实现方法，基于其理论产生了一系列滤波方法，例如卡尔曼滤波系列和粒子滤波。
卡尔曼滤波：一种针对线性系统、将噪声假设为高斯噪声的贝叶斯滤波具体实现形式，具有实现简单、效率高等优点。
贝叶斯公式：

卡尔曼滤波的五条公式：

KF、EKF、ESKF的原理与区别

贝叶斯滤波

首先确定存在系统方程：
$$x_k=f(x_{k-1},u_k,w_k)$$
以及观测方程：
$$y_k=h(x_k,v_k)$$
其中，x表示系统状态，y表示观测量，w表示系统传递噪声，v表示观测误差。
根据贝叶斯公式：

结合已有系统得到贝叶斯公式如下：
$$\begin{array}{rl}p(x_k|y_{1:k})& =\frac{p(y_k|x_k,y_{1:k-1})p(x_k,y_{1:k-1})}{p(y_{1:k})}\\ & =\frac{p(y_k|x_k,y_{1:k-1})p(x_k|y_{1:k-1})}{p(y_k|y_{1:k-1})}\\ & =\frac{p(y_k|x_k)p(x_k|y_{1:k-1})}{\int p(y_k|x_k)p(x_k|y_{1:k-1})d{x}_k}\end{array}$$
以上推导过程假设了系统具有马尔可夫性，因此$p(y_k|x_k,y_{1:k-1})=p(y_k|x_k)$
$p(x_k|y_{1:k-1})$：先验概率密度，通过上一次的系统状态结合系统方程计算得到。
$p(y_k|x_k,y_{1:k-1})$：似然概率密度，由观测方程计算得到
$p(x_k|y_{1:k})$：后验概率密度，即我们所需要得到的结果
其中先验概率密度可以进一步展开为：
$$\begin{array}{rl}p(x_k|y_{1:k-1})& =\int (x_k,x_{k-1}|y_{1:k-1})d{x}_{k-1}\\ & =\int p(x_k|x_{k-1},y_{1:k-1})p(x_{k-1}|y_{1:k-1})d{x}_{k-1}\\ & =\int p(x_k|x_{k-1})p(x_{k-1}|y_{1:k-1})d{x}_{k-1}\end{array}$$
以上公式中包含积分运算，对于一般的非线性、非高斯系统，很难得到后验概率的解析解，因此，需要结合具体的假设完成其实现形式。

卡尔曼滤波（KF，Kalman Filter）

基础假设：

基于基础假设后，线性化的系统方程和观测方程具化为：
$$\begin{gathered} x_k=F_k{x}_{k-1}+B_k{u}_k+w_k \\ {y}_k=H_k{x}_k+v_k \end{gathered}$$
先验概率密度具化为：
$$p(x_k|y_{1:k-1})=\int p(x_k|x_{k-1})p(x_{k-1}|y_{1:k-1})d{x}_{k-1}$$
其中：
$$\begin{gathered} p(x_k|x_{k-1})\sim N(F_k{x}_{k-1}+B_k{u}_k,W_k) \\ p(x_{k-1}|y_{1:k-1})\sim N({\mu }_{k-1},P_{k-1}) \end{gathered}$$
代入积分得到（积分过程忽略）：
$$\begin{gathered} p(x_k|y_{1:k-1})\sim N({\hat{\mu }}_k,{\hat{P}}_k) \\ {\hat{\mu }}_k=F_k{\mu }_{k-1}+B_k{u}_k \\ {\hat{P}}_k=F_k{P}_{k-1}{F}_k^T+W_k \end{gathered}$$
于是就得到了先验概率分布。后验概率密度为：
$$\begin{array}{r}p(x_k|y_{1:k})=\frac{p(y_k|x_k)p(x_k|y_{1:k-1})}{\int p(y_k|x_k)p(x_k|y_{1:k-1})d{x}_k}\end{array}$$
其中：
$$p(y_k|x_k)\sim N(H_k{x}_k,V_k)$$
代入积分计算得到（积分过程忽略）：
$$\begin{gathered} p(x_k|y_{1:k-1})\sim N({\mu }_k,P_k) \\ {\mu }_k={\hat{\mu }}_k+K(y_k-H_k{\hat{\mu }}_k) \\ {P}_k={\hat{P}}_k-K{H}_k{\hat{P}}_k \\ K={\hat{P}}_k{H}_k^T(H_k{\hat{P}}_k{H}_k^T+V_t{)}^{-1} \end{gathered}$$
于是就得到了卡尔曼滤波的五个主要公式。

一个小例子：
有一辆小车，它从一个起点出发，你在之后需要每时每刻知道它的位置，小车上装有一个GPS可以对位置进行测量，但是GPS的测量不可能完全准确，它是有误差的，此时如何才能获取更精准的小车位置呢（如下图所示）？

首先确定系统方程：
$$x_t=x_{t-1}+(T_t-T_{t-1})u_t+w_t\text{\hspace{1em}(3)}$$
和观测方程
$$z_t=x_t+v_t\text{\hspace{1em}(1)}$$
式中$x_t$为t时刻小车真实位置，z为GPS测量值，T为时间，u为速度，w为传递噪声，v为观测噪声，符合标准正态分布，它反应的是测量与实际真值的误差，可以预先测量得到。直接套用KF的五个公式如下：

$$\begin{gathered} {\hat{x}}_t=x_{t-1}+(T_t-T_{t-1})u_t \\ {\hat{p}}_t=p_{t-1}+w_t \\ K=\frac{p_{t-1}+w_t}{p_{t-1}+w_t+v_t} \\ {x}_t={\hat{x}}_t+K(z_t-{\hat{x}}_t) \\ {p}_t={\hat{p}}_t-K{\hat{p}}_t=\frac{v_t(p_{t-1}+w_t)}{p_{t-1}+w_t+v_t} \end{gathered}$$
由此就得到t时刻的系统状态。

扩展卡尔曼滤波（EKF，Extended Kalman Filter）

卡尔曼滤波基于系统是线性系统的这一个假设，对其在实际的应用产生了很大的局限，因此扩展卡尔曼滤波推翻了这一假设，并提出了新的假设：

假设系统是连续变化的，即系统在一个迭代周期内变化很小。

基于以上假设，可以将非线性方程在局部线性化，对其进行一阶泰勒展开，由于$p(x_{k-1}|y_{1:k-1})\sim N({\mu }_{k-1},P_{k-1})$，于是系统方程和观测方程变为：
$$\begin{gathered} x_k=f({\mu }_{k-1},u_k,w_k)+\frac{df}{d{x}_{k-1}}(x_{k-1}-{\mu }_{k-1}) \\ {y}_k=h({\hat{\mu }}_k,v_k)+\frac{dh}{d{x}_k}(x_k-{\hat{\mu }}_k) \\ {F}_k=\frac{df}{d{x}_{k-1}} \\ {H}_k=\frac{dh}{d{x}_k} \end{gathered}$$
剩下的计算步骤与卡尔曼滤波一致。

Error-State卡尔曼滤波（ESKF）

本部分主要参考Quaternion kinematics for the error-state Kalman filter，下文提到文献均指代这个。由于ESKF整个推导过程极为繁琐，因此本文旨在尽可能简化的抽取梗概，从大体上理解ESKF，再结合参考文献中的细节，即可了解全貌。

直接法滤波与间接法滤波
直接法滤波：模型系统方程直接描述系统状态，不存在转化过程。一般使用的KF与EKF都属于直接法滤波。
间接法滤波：模型系统方程描述系统误差，需要通过转换得到系统状态。ESKF（Error-State Kalman Filter）是一种典型的间接法滤波，其预测和更新过程都是针对系统的误差状态，再将修正后误差状态修正系统状态。
流程如下：
ESKF的核心在于将状态分解为两个部分组成：
$$X_t=X+\delta X\text{\hspace{1em}(1)}$$
其中$X_t$为系统状态真值，$X$为Nominal state，$\delta X$为Error state，以下所有公式中均符合这个定义，结合下图理解可能更直观一点：

predict过程

根据IMU中值积分模型，可直接得到X_t 的一阶导数：
$${\dot{X}}_t=U_t(X_t,u_m,i)\text{\hspace{1em}(2)}$$
$u_m,i$ 为IMU的测量值和各类噪声，下文不重复标注了，上式对应文献中式235。
提取所有状态量主成分，构建一个Nominal state的动力学模型：
$$\dot{X}=U(X,u_m)\text{\hspace{1em}(3)}$$
这个模型是自行构建的，理论上是可以根据需要调整的，注意观测，这个模型是完全不受噪声分量影响的，因为噪声分量的影响都放到了Error-state的动力学模型中。上式对应文献中式237。
有(1)(2)(3)就可以推导得到Error-state的动力学模型了，方法是对(1)进行求导，将(2)(3)带入进去求解即可，其中对于速度分量和旋转分量的推导有一些麻烦，因为设计对旋转量的求导，其他都好推。此时得到：
$$\dot{\delta X}=U_{\delta }(X,\delta x,u_m,i)\text{\hspace{1em}(4)}$$
上式对应文献中式261。
对(3)(4)积分可得到离散时间下的系统递推方程为：
$$\begin{gathered} X_{k+1}=f(X_k,u_m) \\ \delta X_{k+1}=f_{\delta }(X_k,\delta X_k,u_m,i)\text{\hspace{1em}(5)} \end{gathered}$$
积分过程参考文献4.6，上式对应文献中260与261。有了系统递推方程后，剩下的部分就跟EKF基本一模一样了，将Error-state方程线性化为：
$$\delta X_{x+1}=F_{\delta }(X,u_m)\delta X_k+F_{i}i\text{\hspace{1em}(6)}$$
这里$F_{\delta }$、$F_i$为$f_{\delta }$对$\delta X、i$的雅可比矩阵，实际上根据该文献的推导，在推导误差系统方程过程中，大量的高阶项已经被忽略，这里已经是一个线性模型，不需要额外的求导操作了。这就是predict部分最核心的公式了，有了这个线性模型，predict部分就退化为一般针对Error-state的EKF，套用EKF的predict公式即可：
$$\begin{gathered} \delta {\hat{X}}_{x+1}=F_{\delta }(X,u_m)\delta {\hat{X}}_k \\ {\hat{P}}_{k+1}=F_{\delta }{\check{P}}_k{F}_{\delta }^T+F_i{Q}_i{F}_i^T\text{\hspace{1em}(7)} \end{gathered}$$

update过程

假设观测方程为：
$$Y=h(X_t)+v\text{\hspace{1em}(8)}$$
套用EKF的update公式如下：
$$\begin{gathered} K={\hat{P}}_{k+1}{H}^T(H{\hat{P}}_{k+1}{H}^T+V{)}^{-1} \\ \delta {\check{X}}_{k+1}=K(Y-h({\hat{X}}_{t,k+1})) \\ {\check{P}}_{k+1}=(I-KH){\hat{P}}_{k+1}\text{\hspace{1em}(9)} \end{gathered}$$
整个过程其实跟EKF完全一样，ESKF与EKF的唯一不同是，式子中的H矩阵是h相对于Error-state的雅可比矩阵。换一种更容易理解的方式，因为$X_t=X\oplus \delta X$ ，Nominal state已知，$h(X_t)$自然就退化为针对Error-state的方程，结合(7)就是一个完全的EKF了。
剩下唯一一个问题就是求解H矩阵，这里直接采用链式法则求解即可：
$$H\triangleq {\frac{dh}{d{X}_t}}_{|X}{\frac{d{X}_t}{d\delta X}}_{|X}\text{\hspace{1em}(10)}$$
具体的计算过程参考该文献吧，这里不展开了。

为什么使用ESKF？

1、Error-State的值一般趋近于0，可以避免一些一阶部分可能出现的奇点、应用于惯导系统时的万向锁问题等，提供了在所有时间段内的线性有效性保证。

2、Error-State的值一般很小，可以保证在泰勒展开式中的二次部分忽略不计，使得雅可比矩阵的计算非常快速且简单，有些系统中的雅可比矩阵甚至可以被认为是一个常数或者状态幅值。

3、Error-State动力系统通常比较缓慢，因为所有较大的分量被集成在了nominal-state当中，这就意味着我们可以使卡尔曼滤波的更新过程频率低于预测过程，如下所示。

.

总结

KF、EKF、ESKF的联系

1、本质上都属于贝叶斯滤波，是贝叶斯滤波的具化形式之一，遵循贝叶斯滤波基础理论。
2、实现流程和计算方法大体一致，只在细节上由差别。
3、由于都基于马尔可夫性假设，都存在无法考虑都历史状态从而可能使稳定性下降的问题。

KF、EKF、ESKF的区别

1、KF于EKF是直接法滤波的代表，直接处理系统状态；ESKF是一种间接法滤波，处理系统误差状态。
2、EKF可广泛应用于非线性系统中，ESKF目前主要在惯导系统应用较多，KF只能应用于线性系统。
3、EKF由于每次迭代都需要计算雅可比矩阵，计算复杂度最高，ESKF次之，KF计算最快，对于现代计算机而言计算消耗很小。

与其他滤波方法的区别

1、粒子滤波：本质上也是贝叶斯滤波的一种，相比较于KF系列，其应用范围更广，不基于非线性系统和高斯噪声的假设，直接用采样的方式逼近系统状态真值，缺点是需要样本越多，精度越高，但计算量也越大。
2、滑动窗口滤波：相比较于KF系列，会考虑一定时间内的历史系统状态，对于局部的系统状态跳变有更好的适应性，但会造成输出延迟等问题。
3、均值滤波：一般应用于滑动窗口滤波中，计算量小且能够起到一定平滑的作用，但当状态分布不均衡时会造成输出偏移。