扩展卡尔曼滤波器

在估计理论中，扩展卡尔曼滤波器(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}$

此处w_k和v_k是过程噪声和观测噪声，假设是平均为0的多元正态分布噪声，其协方差矩阵为Q_k和R_k。u_k是控制向量。

可以用函数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{aligned}{\dot {\mathbf {x} }}(t)&=f{\bigl (}\mathbf {x} (t),\mathbf {u} (t){\bigr )}\\\mathbf {z} (t)&=h{\bigl (}\mathbf {x} (t){\bigr )}(t)\end{aligned}}$

初始化

${\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{aligned}{\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{aligned}}$

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

离散时间量测

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

${\begin{aligned}{\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{aligned}}$

其中$\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{aligned}{\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{aligned}}$

其中

$\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})$

其中w_k和v_k是过程噪讯和量测噪讯，两者都假设是平均为0的多元正态分布噪声，其协方差矩阵 Q_k为R_k。其协方差预测和新息（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}}_{k-1}^{T}}$

${\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})$来决定。

相关条目

• 卡尔曼滤波
• 集合卡尔曼滤波器
• 快速卡尔曼滤波器
• 不变扩展卡尔曼滤波器
• 滚动时域估计
• 粒子滤波器
• 滤波问题
• 投影滤波器

延伸阅读

• Anderson, B.D.O.; Moore, J.B. Optimal Filtering. Englewood Cliffs, New Jersey: Prentice–Hall. 1979. 

• Gelb, A. Applied Optimal Estimation. MIT Press. 1974. 

• Jazwinski, Andrew H. Stochastic Processes and Filtering. Mathematics in Science and Engineering. New York: Academic Press. 1970: 376. ISBN 978-0-12-381550-7. 

• Maybeck, Peter S. Stochastic Models, Estimation, and Control. Mathematics in Science and Engineering 141–1. New York: Academic Press. 1979: 423. ISBN 978-0-12-480701-3. 

外部链接

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