更新日志：

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$ （单位弧度）；

采样时间点为 $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，笔者将在接下来的博文中为读者解读。

7 参考文献

[1] 林鸿. 基于EM算法的随机动态系统建模[J]. 福建师大学报(自然科学版), 2011, 27(6):33-37.

[2] https://www.cnblogs.com/gaoxiang12/p/5560360.html.

[3] https://max.book118.com/html/2017/0502/103920556.shtm.

原创性声明：本文属于作者原创性文章，小弟码字辛苦，转载还请注明出处。谢谢~

如果有哪些地方表述的不够得体和清晰，有存在的任何问题，亦或者程序存在任何考虑不周和漏洞，欢迎评论和指正，谢谢各路大佬。

需要代码和有需要相关技术支持的可咨询QQ：297461921

卡尔曼滤波系列——（二）扩展卡尔曼滤波相关推荐

卫星轨道的估计问题(Matlab)(二):扩展卡尔曼滤波(EKF)对新问题的尝试
前言在前面的问题中我们已经考虑到了用微分方程来描述卫星运动轨迹的方法: r¨=rθ˙2−GMr−2θ¨=−2r−1r˙θ˙\ddot r = r\dot \theta^2-GMr^{-2}\\\dd ...
卡尔曼滤波KF与扩展卡尔曼滤波EKF算法
最近学习了一些预测算法卡尔曼滤波具体推导可以查看B站的教程:贝叶斯滤波与卡尔曼滤波第一讲很有必要的绪论_哔哩哔哩_bilibili 建议从第一讲开始看看上述教程花不了多少时间,可能看一个电影的 ...
状态估计3（扩展卡尔曼滤波）
1 简述卡尔曼滤波适用于线性高斯系统,然而这是一个强假设:对于大部分机器人系统而言,非线性系统才是常态,如此卡尔曼滤波就不太适用了,那么该如何解决这个问题?这引出了扩展卡尔曼滤波. 2 扩展卡尔曼滤 ...
卡尔曼滤波系列——（六）卡尔曼平滑
更新日志: 2020.11.23:修改了2.4节中前向递推的公式书写错误目录 1 简介 2 原理 2.1 概率状态空间模型 2.2 贝叶斯平滑公式 2.3 两个引理 2.4 RTS平滑 3 推导 3 ...
扩展卡尔曼滤波新手教程（二）----中文版
扩展卡尔曼滤波新手教程(二) 说明:本文内容翻译自外文网站The Extended Kalman Filter: An Interactive Tutorial for Non-Experts,仅供学 ...
无人驾驶汽车系统入门（二）——高级运动模型和扩展卡尔曼滤波
前言:上一篇文章的最后我们提到卡尔曼滤波存在着一个非常大的局限性--它仅能对线性的处理模型和测量模型进行精确的估计,在非线性的场景中并不能达到最优的估计效果.所以之前为了保证我们的处理模型是线性的,我 ...
卡尔曼滤波原理(二)：扩展卡尔曼
1.理论部分上一篇介绍了线性卡尔曼滤波器,当系统为线性高斯模型时,滤波器能给出最优的估计,但是实际系统总是存在不同程度的非线性,如平方.三角关系.开方等.对于非线性系统,可以采用的一种方法是通过线性 ...
自动驾驶算法-滤波器系列（二）—— 卡尔曼滤波简介及其变种（EKF、UKF、PF）介绍
KF&EKF&UKF&PF 1. 基础知识概要协方差矩阵多维高斯分布状态空间表达式 2. 什么是卡尔曼滤波器 3. 五个重要的公式公式介绍公式推导过程 4. 卡尔曼滤 ...
卡尔曼滤波原理二：扩展卡尔曼
1.理论部分上一篇介绍了线性卡尔曼滤波器,当系统为线性高斯模型时,滤波器能给出最优的估计,但是实际系统总是存在不同程度的非线性,如平方.三角关系.开方等.对于非线性系统,可以采用的一种方法是通过线性 ...
卡尔曼滤波系列——（四）无损卡尔曼滤波
1 简介无损卡尔曼滤波又称无迹卡尔曼滤波(Unscented Kalman Filter,UKF),是无损变换(Unscented Transform,UT)与标准卡尔曼滤波体系的结合,通过UT变换 ...

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