精华内容
下载资源
问答
  • 卡尔曼滤波系列——(二)扩展卡尔曼滤波

    万次阅读 多人点赞 2019-04-06 16:33:48
    扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线性情形下的一种扩展形式,它是一种高效率的递归滤波器(自回归滤波器)。 EKF的基本思想是利用泰勒级数展开将非线性系统线性化,然后采用...

    更新日志:

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

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

    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}})+\frac{1}{2!}{{(\mathbf{x}-{{\mathbf{x}}_{k}})}^{T}}H({{\mathbf{x}}_{k}})(\mathbf{x}-{{\mathbf{x}}_{k}})+{{o}^{n}}

    其中,{{[\nabla f({{\mathbf{x}}_{k}})]}^{T}}={{\mathbf{J}}_{F}}表示雅克比矩阵,\mathbf{H}({{\mathbf{x}}_{k}})表示海塞矩阵,{{\mathbf{o}}^{n}}表示高阶无穷小。

    {[\nabla f({{\bf{x}}_k})]^T} = {{\bf{J}}_F} = \left[ {\begin{array}{*{20}{c}} {\frac{{\partial f{{({{\bf{x}}_k})}_1}}}{{\partial {x_1}}}}&{\frac{{\partial f{{({{\bf{x}}_k})}_1}}}{{\partial {x_2}}}}& \cdots &{\frac{{\partial f{{({{\bf{x}}_k})}_1}}}{{\partial {x_n}}}}\\ {\frac{{\partial f{{({{\bf{x}}_k})}_2}}}{{\partial {x_1}}}}&{\frac{{\partial f{{({{\bf{x}}_k})}_2}}}{{\partial {x_2}}}}& \cdots &{\frac{{\partial f{{({{\bf{x}}_k})}_2}}}{{\partial {x_n}}}}\\ \vdots & \vdots & \ddots & \vdots \\ {\frac{{\partial f{{({{\bf{x}}_k})}_m}}}{{\partial {x_1}}}}&{\frac{{\partial f{{({{\bf{x}}_k})}_m}}}{{\partial {x_2}}}}& \cdots &{\frac{{\partial f{{({{\bf{x}}_k})}_m}}}{{\partial {x_n}}}} \end{array}} \right]

    {\bf{H}}({{\bf{x}}_k}) = \left[ {\begin{array}{*{20}{c}} {\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial x_1^2}}}&{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_1}\partial {x_2}}}}& \cdots &{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_1}\partial {x_n}}}}\\ {\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_2}\partial {x_1}}}}&{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial x_2^2}}}& \cdots &{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_2}\partial {x_n}}}}\\ \vdots & \vdots & \ddots & \vdots \\ {\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_n}\partial {x_1}}}}&{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_n}\partial {x_2}}}}& \cdots &{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial x_n^2}}} \end{array}} \right]

    这里,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]

    利用以下矩阵迹的求导公式(其中\mathbf{A}\mathbf{B}表示矩阵,\mathbf{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{G}}_{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

    展开全文
  • 扩展卡尔曼滤波

    2020-10-27 17:08:46
    这里必须保证Xk-1,Xk,Xk+1都是正态分布,否则的话我们就没法递推了,如果Xk不是正态分布的话就没有办法调用我们扩展卡尔曼滤波的算法,自然就会引起误差,因为本质上f和h是非线性函数,所以说如果是这个真实的后验...

    在这里插入图片描述
    在这里插入图片描述
    这里必须保证Xk-1,Xk,Xk+1都是正态分布,否则的话我们就没法递推了,如果Xk不是正态分布的话就没有办法调用我们扩展卡尔曼滤波的算法,自然就会引起误差,因为本质上f和h是非线性函数,所以说如果是这个真实的后验概率呢他一定不是这个正态分布,因为他是非线性函数,但是我们滤波的结果呢却是正态分布,所以说他就是一种误差,误差就在这体现,所以说扩展卡尔曼滤波是一种近似,这种近似是有误差的,误差来源就在这
    在这里插入图片描述
    在这里插入图片描述
    预测步和更新步泰勒展开的展开点不同,
    预测步是在Xk-1plus展开
    更新步是在Xkminus展开
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    扩展卡尔曼滤波的算法和卡尔曼滤波的算法很像,最大的不一样就是他要求一个雅克比矩阵,第二个不一样就是算法的2 7步,这里是在这里插入图片描述
    卡尔曼是Xkminus=A*Xk-1plus,也就是说在这里他并没有线性化,还有第7步在这里插入图片描述

    卡尔曼是Xkplus=C*Xkminus,下面来算一个C帮助大家理解雅克比矩阵
    在这里插入图片描述
    以上是预测
    以下是观测
    在这里插入图片描述
    这样呢ekf的理论就讲完了,接下来是代码

    %EKF代码
    %纸上得来终觉浅,绝知此事要躬行%x(k)=sin(3x(k-1))
    %y(k)=x(k)'2
    %注意似然概率是多峰分布,具有强烈的非线性,当y=4时,不知道x=2还是-2
    %%%生成真实信号与观测
    t=0.01:0.01:1;
    n=length(t);
    X=zeros(1,n);
    y=zeros(1,n);
    x(1)=0.1;
    y(1)=0.1^2;
    for i=2:n
    x(i)=sin(3
    x(i-1));
    y(i)=x(i)^2+normrnd(0,0.1);
    end
    plot(t,x,‘r’,t,y,‘b’,‘LineWidth’,4)
    在这里插入图片描述
    红色代表真实的信号,蓝色代表观测

    %EKF代码
    %纸上得来终觉浅,绝知此事要躬行%x(k)=sin(3x(k-1))
    %y(k)=x(k)'2
    %注意似然概率是多峰分布,具有强烈的非线性,当y=4时,不知道x=2还是-2
    %%%生成真实信号与观测
    t=0.01:0.01:1;
    n=length(t);
    X=zeros(1,n);
    y=zeros(1,n);
    x(1)=0.1;
    y(1)=0.1^2;
    for i=2:n
    x(i)=sin(3
    x(i-1));
    y(i)=x(i)^2+normrnd(0,0.7);
    end
    %EKF
    Xplus=zeros(1,n);%设置初值
    Pplus=0.1;
    Xplus(1)=0.1;
    Q=0.1;
    R=1;
    for i=2:n
    %预测步
    A=3cos(3Xplus(i-1));
    Xminus=sin(3Xplus(i-1));
    Pminus=A
    PplusA’+Q;
    %更新步
    C=2
    Xminus;
    K=PminusCinv(CPminusC’+R);
    Xplus(i)=Xminus+K*(y(i)-Xminus^2);
    Pplus=(eye(1)-K*C)*Pminus;
    end

    plot(t,x,‘r’,t,Xplus,‘b’,‘LineWidth’,4)
    在这里插入图片描述
    结果是比较差的,但也算符合我们的预期,我们知道ekf对强烈的非线性问题处理的不太好,他的观测噪声比较大,所以效果不好,我们看一下能不能抢救一下,
    Q=0.0001;以后长这样在这里插入图片描述

    结果还是比较差,比刚才那个稍微好一些,而且有一种很强烈的多峰分布的一个感觉,是因为ekf直接把函数线性化了,这样的话对强非线性问题算的就不是特别的好,如果对一些比较弱的非线性问题他有可能会得到一个比较好的结果,像我们这个例子他的观测噪声比较大,他的似然概率又是一个多峰分布,这样的话就会很差,如果遇到一些强烈的非线性问题的话,扩展卡尔曼滤波需要花很多精力去调Q和R才能得到一个理想的结果,而且一般来说是比较难调的,这就是ekf自然的缺陷,他的优点就在于快,计算速度非常快,和kf差不多,需要的内存也比较小,世上第一个可以进行实时计算的slam程序就是用ekf写出来的,不过呢现在slam都在用最线性优化,第九讲提到了slam和相关的算法,ekf也是人类发明出来的第一个处理非线性问题的kf,对一些实时性要求过高并且非线性程度比较弱的场景中ekf也是经常被用到的,ekf到此为止,最后只剩一个ukf(无迹卡尔曼滤波)。

    展开全文
  • 扩展卡尔曼滤波_无迹卡尔曼滤波_扩展信息滤波_l粒子滤波算法.rar
  • matlab卡尔曼滤波和扩展卡尔曼滤波(KF&EKF)例程,可供学习参考使用,谢谢! 希望能帮到你。。。。。。。。。。。
  • 惯性导航扩展卡尔曼滤波(MATLAB)
  • 扩展卡尔曼滤波算法的matlab实例,将扩展卡尔曼滤波简介明了的用代码表示,有助于读者了解该算法,对算法有深一步的认识
  • 扩展卡尔曼滤波SOC算法Simulink模型,里面包括按时积分的SOC计算,包含噪音的SOC计算,和扩展卡尔曼滤波的SOC计算,输出三者的比较曲线,可以用来参考学习。
  • 卡尔曼滤波理论及应用-基于扩展卡尔曼滤波的汽车质心侧偏角估计.pdf 卡尔曼滤波理论及应用 Unnamed QQ Screenshot20121023091849.png 卡尔曼滤波与维纳滤波(哈工大)....
  • 扩展卡尔曼滤波与粒子滤波性能对比_薛长虎.pdf
  • 针对修正迭代扩展卡尔曼滤波一次迭代后系统状态和估计状态不与测量噪声之间保持相互独立的问题,将测量噪声增广到状态变量中,提出了基于状态增广的修正迭代扩展卡尔曼滤波。然后将该滤波方法应用到基于雷达测量的再...
  • 扩展卡尔曼滤波代码

    2016-03-05 16:02:53
    扩展卡尔曼滤波代码,详细的matlab代码
  • 基于扩展卡尔曼滤波的车辆追踪项目,C++实现,CTRV模型,激光雷达和雷达传感器融合,详情见博客http://blog.csdn.net/adamshan/article/details/78359048
  • 可用的扩展卡尔曼滤波程序示例(m)
  • 基于扩展卡尔曼滤波的电池soc估计simulink模型,将模型计算得到的电池soc与扩展卡尔曼滤波得到的电池soc进行比较。
  • 采用扩展卡尔曼滤波算法建立由动态负荷和静态负荷组成的综合负荷数学模型,并列出了其转子运动方程、状态方程和输出方程,其中动态负荷由等值的异步电机表示,静态负荷由恒定导纳并联组成。通过动模试验,取得给定负荷在...
  • 扩展卡尔曼滤波算法的matlab程序
  • 前段时间给帮助同学做了个毕业设计,基于matlab的扩展卡尔曼滤波,上传上来供大家学习参考,直接打开就可以正常运行。
  • 本资源提供了一个自动驾驶汽车程序启动扩展卡尔曼滤波项目C++代码。所谓扩展卡尔曼滤波器,就是适用于非线性系统的卡尔曼滤波器,可以更广泛的应用在项目中。
  • 扩展卡尔曼滤波
  • PMSM 扩展卡尔曼滤波状态估计
  • 为了解决无线室内定位系统实时跟踪位置坐标误差较大问题,提出一种基于扩展卡尔曼滤波(EKF)算法的室内定位方法。系统采用基于WiFi信号指纹定位,然后利用扩展卡尔曼滤波对估算的位置进行滤波,以改善WiFi指纹定位方法的...
  • 针对人体行为事件,研究了多传感器数据采集模型和扩展卡尔曼滤波优化算法的应用。系统将加速度传感器分量数据映射为三维加速度空间,并与压力传感器数据结合建立四维实时数据采集空间。基于系统的模型特征,提出了非...
  • 基于EKF扩展卡尔曼滤波车身状态估计 汽车稳定性控制系统需要的状态信息一部分可通过车载传感器直接测量,另一部分不能直接测量。为了实现车辆动力学控制系统中的这种闭环状态反馈,受某些测量技术以及成本等因素的...
  • 摘 要: 提出一种基于扩展卡尔曼滤波的 GPS 信号跟踪方法,通过扩展卡尔曼滤波器,得到基于相 干积分支路的滤波模型,有效地削弱常规 GPS 跟踪环路中的跟踪误差,增强接收机的抗干扰性能,提升 其在信号较弱位置下...
  • 针对现有的迭代扩展卡尔曼滤波(EIEKF)跟踪时估计精度较低这一不足,提出了一种改进扩展卡尔曼滤波(NIEKF)新算法。该算法将迭代滤波理论引入到扩展卡尔曼滤波方法中,重复利用观测信息,采用经典的非线性非高斯...
  • 零基础读懂“扩展卡尔曼滤波”——上篇 https://www.jianshu.com/p/4ccac1da3f3f 零基础读懂“扩展卡尔曼滤波”——中篇 https://www.jianshu.com/p/f78082643ea2 零基础读懂“扩展卡尔曼滤波”——中篇 ...

    看到一篇博客写的很漂亮,记录下来,希望更多人看到这样好的讲解.

     

    零基础读懂“扩展卡尔曼滤波”——上篇

    https://www.jianshu.com/p/4ccac1da3f3f

    零基础读懂“扩展卡尔曼滤波”——中篇

    https://www.jianshu.com/p/f78082643ea2

    零基础读懂“扩展卡尔曼滤波”——中篇

    https://www.jianshu.com/u/9598cbc80577

     

    演示:

    https://junshengfu.github.io/

    展开全文
  • lp__matlab 学习笔记之基于扩展卡尔曼滤波的锂电池SOC估计

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 781
精华内容 312
关键字:

扩展卡尔曼滤波