卡尔曼滤波系列——（二）扩展卡尔曼滤波

更新日志：2020.02.13：修改了第三节推导中的公式错误1 简介扩展卡尔曼滤波（Extended Kalman Filter，EKF）是标准卡尔曼滤波在非线性情形下的一种扩展形式，它是一种高效率的递归滤波器（自回归滤波器）。EKF的基本思想是利用泰勒级数展开将非线性系统线性化，然后采用卡尔曼滤波框架对信号进行滤波，因此它是一种次优滤波。2 算法介绍2.1 泰勒级数...

GHelpU

92627人浏览 · 2019-04-06 16:33:48

GHelpU · 2019-04-06 16:33:48 发布

更新日志：

2020.02.13：修改了第三节推导中的公式错误

2020.03.21：修改了2.1节中的部分表述和公式加粗，补充迹的求导公式

2021.04.14：修改公式显示错误

1 简介

扩展卡尔曼滤波（Extended Kalman Filter，EKF）是标准卡尔曼滤波在非线性情形下的一种扩展形式，它是一种高效率的递归滤波器（自回归滤波器）。

EKF的基本思想是利用泰勒级数展开将非线性系统线性化，然后采用卡尔曼滤波框架对信号进行滤波，因此它是一种次优滤波。

2 算法介绍

2.1 泰勒级数展开

泰勒级数展开是将一个在 $x=x_{0}$ 处具有 $n$ 阶导数的函数 $f(x)$ ，利用关于 $(x-x_{0})$ 的 $n$ 次多项式逼近函数值的方法。

若函数 $f(x)$ 在包含 $x_{0}$ 的某个闭区间 $[a,b]$ 上具有 $n$ 阶导数，且在开区间 $(a,b)$ 上具有 $(n+1)$ 阶导数，则对闭区间 $[a,b]$ 上的任意一点 $x$ ，都有：

$f(x)=\frac{f({{x}_{0}})}{0!}+\frac{f'({{x}_{0}})}{1!}(x-{{x}_{0}})+...+\frac{{{f}^{(n)}}({{x}_{0}})}{n!}{{(x-{{x}_{0}})}^{n}}+{{R}_{n}}(x)$

其中 ${{f}^{(n)}}({{x}_{0}})$ 表示函数 $f(x)$ 在 $x=x_{0}$ 处的 $n$ 阶导数，等式右边成为泰勒展开式，剩余的 ${{R}_{n}}(x)$ 是泰勒展开式的余项，是 $(x-x_{0})^{n}$ 的高阶无穷小。

（著名的欧拉公式 ${{e}^{ix}}=\cos x +i\sin x$ 就是利用 ${{e}^{ix}}$ ， $\cos x$ 和 $\sin x$ 的泰勒展开式得来的！）

当变量是多维向量时，一维的泰勒展开就需要做拓展，具体形式如下：

$f(\mathbf{x})=f({{\mathbf{x}}_{k}})+{{[\nabla f({{\mathbf{x}}_{k}})]}^{T}}(\mathbf{x}-{{\mathbf{x}}_{k}})+{{o}^{n}}$

其中， ${{[\nabla f({{\mathbf{x}}_{k}})]}^{T}}={{\mathbf{J}}({\bf x}_k)}$ 表示雅克比矩阵， ${{\mathbf{o}}^{n}}$ 表示高阶无穷小。

${[\nabla f({{\bf{x}}})]^T} = {{\bf{J}}({\bf x})} = \begin{bmatrix} \frac{\partial f({\bf x})_1}{\partial {\bf x}_1} & \hdots & \frac{\partial f({\bf x})_1}{\partial {\bf x}_n}\\ \vdots & \ddots & \vdots \\ \frac{\partial f({\bf x})_m}{\partial {\bf x}_1} & \hdots & \frac{\partial f({\bf x})_m}{\partial {\bf x}_n} \end{bmatrix}$

这里， $f({{\bf{x}}_k})$ 为 $m$ 维， ${{\bf{x}}_k}$ 状态向量为 $n$ 维， $\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_m}\partial {x_n}}} = \frac{{\partial f({{\bf{x}}_k})^T}}{{\partial {x_m}}}\frac{{\partial f({{\bf{x}}_k})}}{{\partial {x_n}}}$ 。

一般来说，EKF在对非线性函数做泰勒展开时，只取到一阶导和二阶导，而由于二阶导的计算复杂性，更多的实际应用只取到一阶导，同样也能有较好的结果。取一阶导时，状态转移方程和观测方程就近似为线性方程，高斯分布的变量经过线性变换之后仍然是高斯分布，这样就能够延用标准卡尔曼滤波的框架。

2.1 EKF

标准卡尔曼滤波KF的状态转移方程和观测方程为

${{\mathbf{\theta }}_{k}}=\mathbf{A}{{\mathbf{\theta }}_{k-1}}+\mathbf{B}{{\mathbf{u}}_{k-1}}+{{\mathbf{s}}_{k}}$

${{\mathbf{z}}_{k}}=\mathbf{C}{{\mathbf{\theta }}_{k}}+{{\mathbf{v}}_{k}}$

扩展卡尔曼滤波EKF的状态转移方程和观测方程为

${{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}$ （1）

${{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}}$ （2）

利用泰勒展开式对（1）式在上一次的估计值 $\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle$ 处展开得

${{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}=f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )+{{\mathbf{F}}_{k-1}}\left( {{\mathbf{\theta }}_{k-1}}-\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle \right)+{{\mathbf{s}}_{k}}$ （3）

再利用泰勒展开式对（2）式在本轮的状态预测值 $\mathbf{\theta }_{k}^{'}$ 处展开得

${{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}}=h\left( \mathbf{\theta }_{{k}}^{\mathbf{'}} \right)+{{\mathbf{H}}_{k}}\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{{k}}^{\mathbf{'}} \right)+{{\mathbf{v}}_{k}}$ （4）

其中， ${\mathbf{F}}_{k-1}$ 和 ${\mathbf{H}}_{k}$ 分别表示函数 $f(\mathbf{\theta })$ 和 $h(\mathbf{\theta })$ 在 $\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle$ 和 $\mathbf{\theta }_{k}^{'}$ 处的雅克比矩阵。

（注：这里对泰勒展开式只保留到一阶导，二阶导数以上的都舍去，噪声假设均为加性高斯噪声）

基于以上的公式，给出EKF的预测（Predict）和更新（Update）两个步骤：

Propagation：

$\mathbf{\theta }_{k}^{'}=f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle)$

$\mathbf{\Sigma }_{k}^{'}=\mathbf{F}_{k-1}{{\mathbf{\Sigma }}_{k-1}}{{\mathbf{F}}_{k-1}^{T}}+\mathbf{Q}$

Update：

$\mathbf{S}_{k}^{'}={{\left( \mathbf{H_{k}\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}$

$\mathbf{K}_{k}^{'}=\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}\mathbf{S}_{k}^{'}$

$\left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+\mathbf{K}_{k}^{'}\left( {{\mathbf{z}}_{k}}-{h(\theta }_{k}^{'}) \right)$

${{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-\mathbf{K}_{k}^{'}\mathbf{H}_{k} \right)\mathbf{\Sigma }_{k}^{'}$

其中的雅克比矩阵 ${\mathbf{F}}_{k-1}$ 和 ${\mathbf{H}}_{k}$ 分别为

${{\mathbf{F}}_{k-1}}={{\left. \frac{\partial f}{\partial \mathbf{\theta }} \right|}_{\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle }}$ ， ${{\mathbf{H}}_{k}}={{\left. \frac{\partial h}{\partial \mathbf{\theta }} \right|}_{\mathbf{\theta }_{k}^{'}}}$

雅可比矩阵的计算，在MATLAB中可以利用对自变量加上一个eps（极小数），然后用因变量的变化量去除以eps即可得到雅可比矩阵的每一个元素值。

读者可能好奇？为什么扩展卡尔曼滤波EKF的传播和更新的形式会和标准卡尔曼滤波KF的形式一致呢？以下做一个简单的推导。

3 推导

先列出几个变量的表示、状态转移方程和观测方程：

真实值 ${{\mathbf{\theta }}_{k}}$ ，预测值 $\mathbf{\theta }_{k}^{'}$ ，估计值 $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle$ ，观测值 ${{\mathbf{z}}_{k}}$ ，观测值的预测 $\mathbf{\hat{z}}_{k}$ ，估计值与真实值之间的误差协方差矩阵 ${{\mathbf{\Sigma }}_{k}}$ ，求期望的符号 $\left\langle \cdot \right\rangle$ 。

${{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}$ ， ${{\mathbf{s}}_{k}}\sim \mathcal{N}(0,\mathbf{Q})$

${{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}}$ ， ${{\mathbf{v}}_{k}}\sim \mathcal{N}(0,\mathbf{R})$

引入反馈： $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-{{{\mathbf{\hat{z}}}}_{k}} \right)=\mathbf{\theta }_{k}^{'}+{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-h(\theta _{k}^{'} )\right)$ （5）

OK，可以开始推导了：

由公式（3）（4）得到以下两个等式，标为式（6）（7）

$f({{\mathbf{\theta }}_{k-1}})-f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )={{\mathbf{F}}_{k-1}}\left( {{\mathbf{\theta }}_{k-1}}-\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle \right)$

$h({{\mathbf{\theta }}_{k}})-h\left( \mathbf{\theta }_{{k}}^{\mathbf{'}} \right)={{\mathbf{H}}_{k}}\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{{k}}^{{'}} \right)$

计算估计值与真实值之间的误差协方差矩阵 ${{\mathbf{\Sigma }}_{k}}$ ，并把式子（4）（5）（7）代入，得到

${{\mathbf{\Sigma }}_{k}}=\left\langle {{\mathbf{e}}_{k}}\mathbf{e}_{k}^{T} \right\rangle =\left\langle \left( {{\mathbf{\theta }}_{k}}-\left\langle {{\mathbf{\theta }}_{k}} \right\rangle \right){{\left( {{\mathbf{\theta }}_{k}}-\left\langle {{\mathbf{\theta }}_{k}} \right\rangle \right)}^{T}} \right\rangle$

${{\mathbf{\Sigma }}_{k}}=\left\langle \left[ {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-h\left( \mathbf{\theta }_{k}^{'} \right) \right) \right]{{\left[ {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-h\left( \mathbf{\theta }_{k}^{'} \right) \right) \right]}^{T}} \right\rangle$

${{\mathbf{\Sigma }}_{k}}=\left\langle \left[ {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{{\mathbf{K}}_{k}}\left( h\left( {{\mathbf{\theta }}_{k}} \right)-h\left( \mathbf{\theta }_{k}^{'} \right)+{{\mathbf{v}}_{k}} \right) \right]{{\left[ {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{{\mathbf{K}}_{k}}\left( h\left( {{\mathbf{\theta }}_{k}} \right)-h\left( \mathbf{\theta }_{k}^{'} \right)+{{\mathbf{v}}_{k}} \right) \right]}^{T}} \right\rangle$

${{\mathbf{\Sigma }}_{k}}=\left\langle \left[ \left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)+{\mathbf{K}}_{k}{\mathbf{v}}_{k} \right] \left[ \left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)+{\mathbf{K}}_{k}{\mathbf{v}}_{k} \right]^T \right\rangle$

${{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)\left\langle \left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right){{\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)}^{T}} \right\rangle {{\left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)}^{T}}+{\mathbf{K}}_{k}{\mathbf{R}}{\mathbf{K}}_{k}^{T}$

${{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right) \mathbf{\Sigma }_{k}^{'}{{\left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)}^{T}}+{\mathbf{K}}_{k}{\mathbf{R}}{\mathbf{K}}_{k}^{T}$

其中 $\mathbf{\Sigma }_{k}^{'}$ 表示真实值与与预测值之间的误差协方差矩阵。于是得到式（8）

${{\mathbf{\Sigma }}_{k}}=\mathbf{\Sigma }_{k}^{'}-{{\mathbf{K}}_{k}}\mathbf{H}_{k}{\mathbf{\Sigma } }_{k}^{'}-\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}_{k}^{T}}}\mathbf{K}_{k}^{T}+{{\mathbf{K}}_{k}}\left( \mathbf{H}_{k}\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}_{k}}^{T}}+\mathbf{R} \right)\mathbf{K}_{k}^{T}$

因为 ${{\mathbf{\Sigma }}_{k}}$ 的对角元即为真实值与估计值的误差的平方，矩阵的迹（用 $T[]$ 表示）即为总误差的平方和，即

$T\left[ {{\mathbf{\Sigma }}_{k}} \right]=T\left[ \mathbf{\Sigma }_{k}^{'} \right]+T\left[ {{\mathbf{K}}_{k}}\left( \mathbf{H}_{k}{\mathbf{\Sigma } }_{k}^{'}{{\mathbf{H}_{k}}^{T}}+\mathbf{R} \right)\mathbf{K}_{k}^{T} \right]-2T\left[ {{\mathbf{K}}_{k}}\mathbf{H}_{k}\mathbf{\Sigma }_{k}^{'} \right]$

利用以下矩阵迹的求导公式（其中 $\bf A$ 和 $\bf B$ 表示矩阵， $\bf a$ 表示列向量）：

$Tr(\mathbf{A}+\mathbf{B})=Tr(\mathbf{A})+Tr(\mathbf{B})$

$Tr(\mathbf{AB})=Tr(\mathbf{BA})$

$\mathbf{a}^{T} \mathbf{a}=Tr(\mathbf{a}\mathbf{a}^{T})$

$\frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{XBX}^{T})=\mathbf{XB}^{T}+\mathbf{XB}$

$\frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{AX}^{T})=\mathbf{A}$

$\frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{XA})=\mathbf{A}^{T}$

要让估计值更接近于真实值，就要使上面的迹尽可能的小，因此要取得合适的卡尔曼增益 ${{\mathbf{K}}_{k}}$ ，使得迹得到最小，言外之意就是使得迹对 ${{\mathbf{K}}_{k}}$ 的偏导为0，即

$\frac{dT\left[ {{\mathbf{\Sigma }}_{k}} \right]}{d{{\mathbf{K}}_{k}}}=2{{\mathbf{K}}_{k}}\left( \mathbf{H}_{k}{\mathbf{\Sigma }}_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)-2{{\left( \mathbf{H}_{k}{\mathbf{\Sigma }}_{k}^{'} \right)}^{T}}=0$

这样就能算出合适的卡尔曼增益了，即

${{\mathbf{K}}_{k}}=\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}{{\left( \mathbf{H}_{k}{\mathbf\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}$

代回式（8）得到

${{\mathbf{\Sigma }}_{k}}=\mathbf{\Sigma }_{k}^{'}-\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}{{\left( \mathbf{H}_{k}{\mathbf\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}\mathbf{H}_{k}{\mathbf\Sigma }_{k}^{'}=\left( \mathbf{I}-{{\mathbf{K}}_{k}}\mathbf{H}_{k} \right)\mathbf{\Sigma }_{k}^{'}$

接下来就差真实值与预测值之间的协方差矩阵 $\mathbf{\Sigma }_{k}^{'}$ 的求值公式了

$\mathbf{\Sigma }_{_{k}}^{'}=\left\langle \mathbf{e}_{k}^{'}\mathbf{e}{{_{k}^{'}}^{T}} \right\rangle =\left\langle \left( {{\theta }_{k}}-\theta _{k}^{'} \right){{\left( {{\theta }_{k}}-\theta _{k}^{'} \right)}^{T}} \right\rangle$

将以下两个等式代入到上式，

${{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}$ ， $\mathbf{\theta }_{k}^{'}=f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )$

可以得到

$\mathbf{\Sigma }_{_{k}}^{'}=\left\langle \left[f({{\mathbf{\theta }}_{k-1}})-f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )+{{\mathbf{s}}_{k}} \right]{{\left[ f({{\mathbf{\theta }}_{k-1}})-f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )+{{\mathbf{s}}_{k}} \right]}^{T}} \right\rangle$

有 ${{\mathbf{\theta }}_{k}}$ 、 $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle$ 与观测噪声 ${{\mathbf{s}}_{k}}$ 是独立的，求期望等于零;； $\left\langle {{\mathbf{s}}_{k}}{{\mathbf{s}}_{k}}^{T} \right\rangle$ 表示观测噪声的协方差矩阵，用 ${\mathbf{Q}}$ 表示。于是得到

$\mathbf{\Sigma }_{_{k}}^{'}=\mathbf{F}_{k-1}\left\langle \left( {{\theta }_{k-1}}-\left\langle {{\theta }_{k-1}} \right\rangle \right){{\left( {{\theta }_{k-1}}-\left\langle {{\theta }_{k-1}} \right\rangle \right)}^{T}} \right\rangle {{\mathbf{F}}_{k-1}^{T}}+\left\langle {{\mathbf{s}}_{k}}\mathbf{s}_{k}^{T} \right\rangle \\ =\mathbf{F}_{k-1}{{\mathbf{\Sigma }}_{k-1}}{{\mathbf{F}}_{k-1}^{T}}+\mathbf{Q}$

其中的协方差矩阵的转置矩阵就是它本身。OK，大功告成，以上就完成了全部公式的推导了。

4 实际应用

现在我们假设在海上有一艘正在行驶的船只，其相对于传感器的横纵坐标为 $(x;y;v_{x};v_{y})$ 为隐藏状态，无法直接获得，而传感器可以测量得到船只相对于传感器的距离和角度 $(r;\theta)$ ，传感器采样的时间间隔为 $\Delta t$ ，则：

状态向量 $(x;y;v_{x};v_{y})$ ，观测向量 $(r;\theta)$

状态转移方程和观测方程为：

$\left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{x_{k}}} \\ {{v}_{y_{k}}} \\ \end{matrix} \right]=f(\left[ \begin{matrix} {{x}_{k-1}} \\ {{y}_{k-1}} \\ {{v}_{{{x}_{k-1}}}} \\ {{v}_{{{y}_{k-1}}}} \\ \end{matrix} \right])+{{\mathbf{s}}_{k}}=\left[ \begin{matrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]\left[ \begin{matrix} {{x}_{k-1}} \\ {{y}_{k-1}} \\ {{v}_{{{x}_{k-1}}}} \\ {{v}_{{{y}_{k-1}}}} \\ \end{matrix} \right]+{{\mathbf{s}}_{k}}$

$\left[ \begin{matrix} {{r}_{k}} \\ {{\theta }_{k}} \\ \end{matrix} \right]=h(\left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{xk}} \\ {{v}_{yk}} \\ \end{matrix} \right])+{{\mathbf{v}}_{k}}=\left[ \begin{matrix} \sqrt{x_{k}^{2}+x_{y}^{2}} \\ \arctan \frac{{{y}_{k}}}{{{x}_{k}}} \\ \end{matrix} \right]+{{\mathbf{v}}_{k}}$

那么雅克比矩阵为

${{\mathbf{F}}_{k-1}}=\left[ \begin{matrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]$

${{H}_{k}}=\left[ \begin{matrix} \frac{\partial {{r}_{k}}}{\partial {{x}_{k}}} & \frac{\partial {{r}_{k}}}{\partial {{y}_{k}}} & \frac{\partial {{r}_{k}}}{\partial {{v}_{{{x}_{k}}}}} & \frac{\partial {{r}_{k}}}{\partial {{v}_{{{y}_{k}}}}} \\ \frac{\partial {{\theta }_{k}}}{\partial {{x}_{k}}} & \frac{\partial {{\theta }_{k}}}{\partial {{y}_{k}}} & \frac{\partial {{\theta }_{k}}}{\partial {{v}_{{{x}_{k}}}}} & \frac{\partial {{\theta }_{k}}}{\partial {{v}_{{{y}_{k}}}}} \\ \end{matrix} \right]$

这里给定距离传感器的噪声均值为 $0$ ，方差为 $10$ ；角度传感器的噪声均值为0，方差为 $0.001$ （单位弧度）；

采样时间点为 $\small 100$ 个；

船只的初始状态为 $(1000;1500;5;-3)$ ，四个状态量的噪声的方差分别为 $(2;2;0.2;0.2)$ 。仿真结果如下：

从仿真结果可以看出，EKF在这种情形下的滤波效果还是不错的，但是在实际应用中，对于船只运动的状态转移噪声的均值 $\mathbf q$ 和协方差矩阵 $\mathbf Q$ ，以及传感器的观测噪声的均值 $\mathbf r$ 和协方差矩阵 $\mathbf R$ ，往往都是未知的，有很多情况都只有观测值而已，这样的情形下，就有必要利用观测值对噪声的统计量参数做出适当的估计（学习）。

5 参数估计（参数学习）

利用EM算法和极大后验概率估计（MAP），对未知的噪声参数做出估计，再利用估计出的参数去递推卡尔曼滤波的解。本文对EM算法在卡尔曼滤波框架中的推导暂时先不给出，之后可能会补充，这里就先给出一种Adaptive-EKF算法的公式。

${{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}$ ， ${{\mathbf{s}}_{k}}\sim \mathcal{N}(\mathbf{q},\mathbf{Q})$

${{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}}$ ， ${{\mathbf{v}}_{k}}\sim \mathcal{N}(\mathbf{r},\mathbf{R})$

${{\mathbf{\varepsilon }}_{k}}={{\mathbf{z}}_{k}}-h(\mathbf{\theta }_{k}^{'})-{{\mathbf{r}}_{k}}$

（1）E-Step

Propagation：

$\mathbf{\theta }_{k}^{'}=f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle)$

$\mathbf{\Sigma }_{k}^{'}=\mathbf{F}_{k-1}{{\mathbf{\Sigma }}_{k-1}}{{\mathbf{F}}_{k-1}^{T}}+\mathbf{Q}$

Update：

$\mathbf{S}_{k}^{'}={{\left( \mathbf{H_{k}\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}$

$\mathbf{K}_{k}^{'}=\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}\mathbf{S}_{k}^{'}$

$\left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+\mathbf{K}_{k}^{'}\left( {{\mathbf{z}}_{k}}-{h(\theta }_{k}^{'}) \right)$

${{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-\mathbf{K}_{k}^{'}\mathbf{H}_{k} \right)\mathbf{\Sigma }_{k}^{'}$

（2）M-Step

${{\mathbf{\hat{q}}}_{k}}=\left( 1-\frac{1}{k} \right){{\mathbf{\hat{q}}}_{k\text{-}1}}+\frac{1}{k}\left[ \left\langle {{\theta }_{k}} \right\rangle -f\left( \left\langle {{\theta }_{k-1}} \right\rangle \right) \right]$

${{\mathbf{\hat{Q}}}_{k}}=\left( 1-\frac{1}{k} \right){{\mathbf{\hat{Q}}}_{k\text{-}1}}+\frac{1}{k}\left[ {{\mathbf{K}}_{k}}{{\mathbf{\varepsilon }}_{k}}\mathbf{\varepsilon }_{k}^{T}\mathbf{K}_{k}^{T}+{{\mathbf{\Sigma }}_{k}}-{{\mathbf{F}}_{k-1}}{{\mathbf{\Sigma }}_{k-1}}\mathbf{F}_{k-1}^{T} \right]$

${{\mathbf{\hat{r}}}_{k}}=\left( 1-\frac{1}{k} \right){{\mathbf{\hat{r}}}_{k\text{-}1}}+\frac{1}{k}\left[ {{\mathbf{z}}_{k}}-h\left( \left\langle \theta _{k}^{'} \right\rangle \right) \right]$

${{\mathbf{\hat{R}}}_{k}}=\left( 1-\frac{1}{k} \right){{\mathbf{\hat{R}}}_{k\text{-}1}}+\frac{1}{k}\left[ {{\mathbf{\varepsilon }}_{k}}\mathbf{\varepsilon }_{k}^{T}-{{\mathbf{H}}_{k}}\mathbf{\Sigma }_{k}^{'}\mathbf{H}_{k}^{T} \right]$

利用以上的Adaptive-EKF算法对船只的运动做滤波跟踪，得到的效果如下图所示：

相比于没有做参数估计的EKF滤波，可以看出，Adaptive-EKF在估计误差上要优于EKF滤波，而且，它并不需要指定状态转移噪声和观测噪声的参数，将更有利于在实际中的应用。

6 总结

EKF滤波通过泰勒展开公式，把非线性方程在局部线性化，使得高斯分布的变量在经过线性变换后仍然为高斯分布，这使得能继续把标准卡尔曼滤波KF的框架拿过来用，总体来说，EKF在函数的非线性不是很剧烈的情形下，能够具有很不错的滤波效果。但是EKF也有它的不足之处：其一，它必须求解非线性函数的Jacobi矩阵，对于模型复杂的系统，它比较复杂而且容易出错；其二，引入了线性化误差，对非线性强的系统，容易导致滤波结果下降。基于以上原因，为了提高滤波精度和效率，以满足特殊问题的需要，就必须寻找新的逼近方法，于是便有了粒子滤波PF和无迹卡尔曼滤波UKF，笔者将在接下来的博文中为读者解读。