扩展卡尔曼滤波器

在估计理论中，扩展卡尔曼滤波器（extended Kalman filter），简称EKF，是卡尔曼滤波的非线性版本，会对平均和协方差的估测值进行线性化。在转移模型定义良好的情形下，一般会将扩展卡尔曼滤波器视为非线性状态观测器、导航系统和全球定位系统的de facto标准。

## 历史

建立卡尔曼滤波器数学基础的论文是在1959年到1961年之间发行的。若是线性系统模型，在转换系统和量测系统上有独立白色噪音，卡尔曼滤波是最佳的线性估测器。只是，大部分工程领域的系统属于非线性系统，因此许多人努力将滤波器应用在非线性系统上。大部分的研究是在艾姆斯研究中心完成的。扩展卡尔曼滤波器应用微积分学里的多变数泰勒级数展开，在工作点附近对模型线性化。若系统模型未知或是不准确，也可以用蒙地卡罗方法（特别是粒子滤波器）来进行估测。蒙地卡罗方法出现的时间比扩展卡尔曼滤波器要早，但在中等大小的状态空间中，其运算量会比扩展卡尔曼滤波器高很多。

## 公式

在扩展卡尔曼滤波器，状态转换模型和观测模型可以不是状态的线性函数，只要是可微函数即可。

$$\boldsymbol{x}_{k} = f(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k-1}) + \boldsymbol{w}_{k-1}$$

$$\boldsymbol{z}_{k} = h(\boldsymbol{x}_{k}) + \boldsymbol{v}_{k}$$

此处wk和vk是过程噪声和观测噪声，假设是平均为0的多元正态分布噪声，其协方差矩阵为Qk和Rk。uk是控制向量。

可以用函数f从过去的观测值中计算预测值，并且用h从预测状态中计算预测量测量。不过f和h无法直接计算协方差。会改计算偏微分矩阵（雅可比矩阵）。

在每一步时间，会用当时的预测状态计算雅可比矩阵。矩阵可以用在卡尔曼滤波器方程中。此作法在本质上在目前的预测附近对非线性函数线性化。

## 离散时间预测方程和更新方程

标示$\hat{\mathbf{x}}_{n\mid m}$表示$\mathbf{x}$在时间n的估测值，假设观测值一直到m ≤ n。

### 预测

预测的状态估测｜$\hat{\boldsymbol{x}}_{k|k-1} = f(\hat{\boldsymbol{x}}_{k-1|k-1}, \boldsymbol{u}_{k-1})$

预测的协方估测｜$\boldsymbol{P}_{k|k-1} = {{\boldsymbol{F}_{k}}} \boldsymbol{P}_{k-1|k-1}{ {\boldsymbol{F}_{k}^T}} + \boldsymbol{Q}_{k-1}$

### 更新

新息（Innovation）或测量残差｜$\tilde{\boldsymbol{y}}_{k} = \boldsymbol{z}_{k} - h(\hat{\boldsymbol{x}}_{k|k-1})$

新息或残余协方差｜$\boldsymbol{S}_{k} = {{\boldsymbol{H}_{k}}}\boldsymbol{P}_{k|k-1}{{\boldsymbol{H}_{k}^T}} + \boldsymbol{R}_{k}$

接近最佳化的卡尔曼增益｜$\boldsymbol{K}_{k} = \boldsymbol{P}_{k|k-1}{{\boldsymbol{H}_{k}^T}}\boldsymbol{S}_{k}^{-1}$

更新的状态估测｜$\hat{\boldsymbol{x}}_{k|k} = \hat{\boldsymbol{x}}_{k|k-1} + \boldsymbol{K}_{k}\tilde{\boldsymbol{y}}_{k}$

更新的协方差估计｜$\boldsymbol{P}_{k|k} = (\boldsymbol{I} - \boldsymbol{K}_{k} {{\boldsymbol{H}_{k}}})\boldsymbol{P}_{k|k-1}$

其中状态变换和估测矩阵可以用以下的雅可比矩阵定义

$${{\boldsymbol{F}_{k}}} = \left . \frac{\partial f}{\partial \boldsymbol{x} } \right \vert _{\hat{\boldsymbol{x}}_{k-1|k-1},\boldsymbol{u}_{k-1}}$$

$${{\boldsymbol{H}_{k}}} = \left . \frac{\partial h}{\partial \boldsymbol{x} } \right \vert _{\hat{\boldsymbol{x}}_{k|k-1}}$$

## 缺点和其他替代方案

卡尔曼滤波器是最佳的估测器，但扩展卡尔曼滤波器多半不是最佳估测器（若量测和状态转换模型都线性时，扩展卡尔曼滤波器是最佳估测器，但此时的扩展卡尔曼滤波器就是卡尔曼滤波器）此外，若状态的初始估测错误，或是过程的建模不正确，因为线性化的关系，滤波器会快速发散。另一个扩展卡尔曼滤波器的问题是其估测协方差矩阵常常会低估，因此若没有加入“稳定性噪声”，在统计观点上可能会有一致性 (统计)的风险。

另外也要考虑到非线性滤波问题在统计的本质上是无限维的，不适合用单一的平均和方差-协方差的估测器来完全表示最佳化滤波器。即使在非常简单的一维系统（例如立方感测器，观测值和实际状态之间的关系是三次方的）下，扩展卡尔曼滤波器也可能会有很差的性能，其最佳化滤波器可能是双模（两个最大值）的，其且其结构很复杂，无法有效的用单一的均值和方差估测器来表示，对于二次感测器也有类似问题。

针对这些案例，已找到投影滤波器是替代方案，已应用在导航上。其他通用的非线性滤波器（像是全粒子滤波器）也可以考虑。

虽然如此，扩展卡尔曼滤波器因为有还可接受的性能，仍是导航和GPS的业界标准。

## 广义的扩展卡尔曼滤波器

### 连续时间的扩展卡尔曼滤波器

其模型如下;

$$\begin{align} \dot{\mathbf{x}}(t) &= f\bigl(\mathbf{x}(t), \mathbf{u}(t)\bigr) \\ <pre> \mathbf{z}(t) &= h\bigl(\mathbf{x}(t)\bigr)(t) </pre> \end{align}$$

初始化

$$\hat{\mathbf{x}}(t_0)=E\bigl[\mathbf{x}(t_0)\bigr] \text{, } \mathbf{P}(t_0)=Var\bigl[\mathbf{x}(t_0)\bigr]$$

预测-更新

$$\begin{align} \dot{\hat{\mathbf{x}}}(t) &= f\bigl(\hat{\mathbf{x}}(t),\mathbf{u}(t)\bigr)+\mathbf{K}(t)\Bigl(\mathbf{z}(t)-h\bigl(\hat{\mathbf{x}}(t)\bigr)\Bigr)\\ \dot{\mathbf{P}}(t) &= \mathbf{F}(t)\mathbf{P}(t)+\mathbf{P}(t)\mathbf{F}(t)^{T}-\mathbf{K}(t)\mathbf{H}(t)\mathbf{P}(t)+\mathbf{Q}(t)\\ \mathbf{K}(t) &= \mathbf{P}(t)\mathbf{H}(t)^{T}\mathbf{S}(t)^{-1}\\ \mathbf{F}(t) &= \left . \frac{\partial f}{\partial \mathbf{x} } \right \vert _{\hat{\mathbf{x}}(t),\mathbf{u}(t)}\\ \mathbf{H}(t) &= \left . \frac{\partial h}{\partial \mathbf{x} } \right \vert _{\hat{\mathbf{x}}(t)} \end{align}$$

和离散时间的扩展卡尔曼滤波器不同，在连续时间的扩展卡尔曼滤波器里，预测和更新步骤是互相耦合的。

#### 离散时间量测

大部分的物理系统是连续时间模型，但会用数位处理器在离散时间量测，以进行状态估测。因此，系统模型和量测模型为

$$\begin{align} \dot{\mathbf{x}}(t) &= f\bigl(\mathbf{x}(t), \mathbf{u}(t)\bigr) + \mathbf{w}(t) &\mathbf{w}(t) &\sim \mathcal{N}\bigl(\mathbf{0},\mathbf{Q}(t)\bigr) \\ \mathbf{z}_k &= h(\mathbf{x}_k) + \mathbf{v}_k &\mathbf{v}_k &\sim \mathcal{N}(\mathbf{0},\mathbf{R}_k) \end{align}$$

其中$\mathbf{x}_k=\mathbf{x}(t_k)$.

初始化

$$\hat{\mathbf{x}}_{0|0}=E\bigl[\mathbf{x}(t_0)\bigr], \mathbf{P}_{0|0}=E\bigl[\left(\mathbf{x}(t_0)-\hat{\mathbf{x}}(t_0)\right)\left(\mathbf{x}(t_0)-\hat{\mathbf{x}}(t_0)\right)^T\bigr]$$

预测

$$\begin{align} \text{solve } &\begin{cases} \dot{\hat{\mathbf{x}}}(t) = f\bigl(\hat{\mathbf{x}}(t), \mathbf{u}(t)\bigr) \\ \dot{\mathbf{P}}(t) = \mathbf{F}(t)\mathbf{P}(t)+\mathbf{P}(t)\mathbf{F}(t)^T+ \mathbf{Q}(t) \end{cases}\qquad \text{with } \begin{cases} \hat{\mathbf{x}}(t_{k-1}) = \hat{\mathbf{x}}_{k-1|k-1} \\ \mathbf{P}(t_{k-1}) = \mathbf{P}_{k-1|k-1} \end{cases} \\ \Rightarrow &\begin{cases} \hat{\mathbf{x}}_{k|k-1} = \hat{\mathbf{x}}(t_k) \\ \mathbf{P}_{k|k-1} = \mathbf{P}(t_k) \end{cases} \end{align}$$

其中

$$\mathbf{F}(t) = \left. \frac{\partial f}{\partial \mathbf{x} } \right \vert _{\hat{\mathbf{x}}(t),\mathbf{u}(t)}$$

更新

$$\mathbf{K}_{k} = \mathbf{P}_{k|k-1}\mathbf{H}_{k}^{T}\bigl(\mathbf{H}_{k}\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{T} + \mathbf{R}_{k}\bigr)^{-1}$$

$$\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_{k}\bigl(\mathbf{z}_{k} - h(\hat{\mathbf{x}}_{k|k-1})\bigr)$$

$$\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_{k}\mathbf{H}_{k})\mathbf{P}_{k|k-1}$$

其中

$$\textbf{H}_{k} = \left . \frac{\partial h}{\partial \textbf{x} } \right \vert _{\hat{\textbf{x}}_{k|k-1}}$$

更新方程和离散时间中的方程相同。

### 高阶扩展卡尔曼滤波器

上述的算法是一阶的扩展卡尔曼滤波器（EKF）。若保留泰勒级数展开的高次项，即可建构高次的扩展卡尔曼滤波器。有论文曾提及二阶和三阶的扩展卡尔曼滤波器。但高阶扩展卡尔曼滤波器只有在量测误差小的前提下，才能看出其性能上的优势。

### 非加性噪声的形成和方程式

典型的扩展卡尔曼滤波器假设可加性过程以及可加性量测噪声。但在扩展卡尔曼滤波器实现时，不一定都能符合此一条件。考虑以下通用型式的系统：

$$\boldsymbol{x}_{k} = f(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k-1}, \boldsymbol{w}_{k-1})$$

$$\boldsymbol{z}_{k} = h(\boldsymbol{x}_{k}, \boldsymbol{v}_{k})$$

其中wk和vk是过程噪讯和量测噪讯，两者都假设是平均为0的多元正态分布噪声，其协方差矩阵 Qk为Rk。其协方差预测和新息（innovation）方程为

$$\boldsymbol{P}_{k|k-1} = {{{\boldsymbol{F}_{k-1}}}} {\boldsymbol{P}_{k-1|k-1}}{{{\boldsymbol{F}_{k-1}^T}}} {+} {\boldsymbol{L}_{k-1}} {\boldsymbol{Q}_{k-1}}{\boldsymbol{L}^{T}_{k-1}}$$

$$\boldsymbol{S}_{k} = {{\boldsymbol{H}_{k}}}{\boldsymbol{P}_{k|k-1}}{{\boldsymbol{H}_{k}^T}} {+} {\boldsymbol{M}_{k}} {\boldsymbol{R}_{k}} {\boldsymbol{M}_{k}^{T}}$$

其中矩阵$\boldsymbol{L}_{k-1}$和矩阵$\boldsymbol{M}_{k}$是雅可比矩阵：

$${{\boldsymbol{L}_{k-1}}} = \left . \frac{\partial f}{\partial \boldsymbol{w} } \right \vert _{\hat{\boldsymbol{x}}_{k-1|k-1},\boldsymbol{u}_{k-1}}$$

$${{\boldsymbol{M}_{k}}} = \left . \frac{\partial h}{\partial \boldsymbol{v} } \right \vert _{\hat{\boldsymbol{x}}_{k|k-1}}$$

预测的状态估测和量测残余是以过程的平均以及量测噪声项的平均来表示，两者都假设为0。不然，非加性噪声可以用加性噪声的扩展卡尔曼滤波器的作法来实现。

### 隐式扩展卡尔曼滤波器

有时，非线性系统的观测模型无法直接求解$\boldsymbol{z}_{k}$，只能用隐函数表示：

$$h(\boldsymbol{x}_{k}, \boldsymbol{z'}_{k}) = \boldsymbol{0}$$

其中$\boldsymbol{z}_{k} = \boldsymbol{z'}_{k} + \boldsymbol{v}_{k}$是有噪声的观测量。

可以在以下的调整之后，使用扩展卡尔曼滤波器：

$${{\boldsymbol{R}_{k}}} \leftarrow {{\boldsymbol{J}_{k}}} {{\boldsymbol{R}_{k}}} {{\boldsymbol{J}_{k}^{T}}}$$

$$\tilde{\boldsymbol{y}}_{k} \leftarrow -h(\hat{\boldsymbol{x}}_{k|k-1}, \boldsymbol{z}_{k})$$

其中：

$${{\boldsymbol{J}_{k}}} = \left . \frac{\partial h}{\partial \boldsymbol{z} } \right \vert _{\hat{\boldsymbol{x}}_{k|k-1}, \boldsymbol{z}_{k}}$$

此处原来的观测共变异数矩阵已经转换，以不同的方式定义新息（innovation）$\tilde{\boldsymbol{y}}_{k}$。雅可比矩阵${{\boldsymbol{H}_{k}}}$定义不变，但是是用隐观测模型$h(\boldsymbol{x}_{k}, \boldsymbol{z}_{k})$来决定。

## 修改版本及其他替代方案

### 迭代扩展卡尔曼滤波器

迭代扩展卡尔曼滤波器（iterated extended Kalman filter）改进了扩展卡尔曼滤波器的线性化，作法是重复的调整泰勒展开的中心点。这减少了线性化的误差，其代价是计算量的增加。

### 强健扩展卡尔曼滤波器

强健扩展卡尔曼滤波器（robust extended Kalman filter）在目前状态估测处对信号模型进行线性化，再用线性的卡尔曼滤波来预测下一个估测。此作法设法要产出区域最佳的滤波器，但不一定会收敛，因为底层黎卡提方程的解不保证正定。有一个改善性能的方式是伪代数Riccati技巧（faux algebraic Riccati technique）

会为在稳定性和最佳化之间权衡。此作法会维持熟悉的扩展卡尔曼滤波器结构，但是在增益设计时，选择一个伪代数Riccati方程的正定解，以达到稳定性。

另一个提升扩展卡尔曼滤波器性能的作法是导入强健控制中，H-infinity的结果。可以在设计Riccati方程时，加入正定项，以得到强健滤波器。新增的项有个纯量的参数，让设计者在均方误差性能和最大误差性能两者之间取舍。

### 不变扩展卡尔曼滤波器

不变扩展卡尔曼滤波器（invariant extended Kalman filter，IEKF）是修改版的扩展卡尔曼滤波器，是针对有对称（不变量）的非线性系统。此作法结合了EKF和近来引入对称保持滤波器的优点。不变扩展卡尔曼滤波器不是使用以线性输出误差为基础的线性修正项，此方法使用以不变量输出误差为基础的几何适应性校正项，同样地，增益矩阵也不是用线性状态误差来更新，而是以不变量状态误差来更新。主要的好处是增益和增益和协方差方程式可以在比平衡点大得多的轨迹集上收敛到常数值（扩展卡尔曼滤波器只能在平衡点上收敛），因此估测的收敛情形会比较好。

### 无迹卡尔曼滤波器

无迹卡尔曼滤波器（unscented Kalman filter，简称UKF）是一个有可能可以改善扩展卡尔曼滤波器的非线性卡尔曼滤波器。在无迹卡尔曼滤波器中，会用确定的取样点近似几率密度，取样点将底层的分布以正态分布来表示。这些点的非线性变换旨在估计后验几率，其矩可以从变换后的样本中导出。此转换称为无迹变换。在各方向的估计误差上，无迹卡尔曼滤波器比扩展卡尔曼滤波器更加强健，也更加准确。

"扩展卡尔曼滤波器（EKF）可能是非线性系统最广为使用的估测算法。不过，估测社群超过35年的经验，证实此方法很难实现、很难调校、只在一些在更新时间尺度上几乎线性的系统上是可靠的。很多的困难点就是来此方法所作的其线性化。"

2012年有一篇包括模拟结果的论文，指出一些已发表的UKF变体不比和二阶扩展卡尔曼滤波器（Second Order Extended Kalman Filter，SOEKF）准确二阶扩展卡尔曼滤波器也称为增广卡尔曼滤波器（augmented Kalman filter），比UKF早大约35年，最早是由Bass等人提出。要实现针对非线性状态转换的卡尔曼性滤波器，其困难点来自为了精度所需要的数值稳定性议题，而UKF也没有躲过这个问题，其中也有用到线性化，称为线性回归。UKF的稳定性问题是来自协方差矩阵的方根，而EKF和SOEKF的稳定性问题来自沿着轨迹的泰勒级数近似。

### 集合卡尔曼滤波器

集合卡尔曼滤波器（Ensemble Kalman filter）是由Evensen在1994年发明，比UKF要晚。集合卡尔曼滤波器和UKF相比较的优点是其集合成员可以比状态维度要少很多，因此可以用在非常高维的系统中，例如气象预报，其状态空间大小会到十亿以上。

### 模糊卡尔曼滤波器

近来有提出模糊卡尔曼滤波器（Fuzzy Kalman filter）以及一种表示可能性分布（possibility distributions）的方法，用可能性分布取代几率分布，从而获得真正的可能性滤波器（genuine possibilistic filter），可以用非对称的过程以及观测噪声，以及过程模型和观测型的较高不准确性。

## 外部链接

- Position estimation of a differential-wheel robot based on odometry and landmarks