卡尔曼滤波系列——(二)扩展卡尔曼滤波

信号处理 专栏收录该内容
10 篇文章 36 订阅

更新日志:

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,笔者将在接下来的博文中为读者解读。

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

评论 74 您还未登录,请先 登录 后发表或查看评论
©️2022 CSDN 皮肤主题:技术黑板 设计师:CSDN官方博客 返回首页

打赏作者

GHelpU

感恩每一份鼓励

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值