精华内容
下载资源
问答
  • matlab写的一个扩展卡尔曼滤波程序,状态方程为线性,观测方程非线性,最后输出图片以便观察是否收敛,分享给大家参考。还有一个C++版本的。
  • 扩展卡尔曼滤波代码和数据

    热门讨论 2017-10-26 22:40:06
    基于扩展卡尔曼滤波的车辆追踪项目,C++实现,CTRV模型,激光雷达和雷达传感器融合,详情见博客http://blog.csdn.net/adamshan/article/details/78359048
  • 卡尔曼滤波系列——(二)扩展卡尔曼滤波

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

    更新日志:

    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

    展开全文
  • pbfunc外部函数扩展(1.2.2.8) 2015-10-18

    热门讨论 2015-10-19 11:47:45
    pbfunc外部函数扩展是专给PowerBuilder各个版本可以使用的外部扩展库,部分功能其它开发工具也可以使用,主要功能如下: 1.以非图片方式在Datawindow中显示QR二维码 2.GBK和UTF-8编码相互转换 3.加密解密,RSA加密...
  • 压缩 C 盘 ... 会看到在 C 盘旁边生成一个未分配的盘 此时再右键 C 盘,看到...扩展 C 盘 选择磁盘,把未分配的盘放到已选,再下一步,即可 点击完成,即可 此时发现未分配的盘没有了,合并到 C 盘了 ...

    压缩 C 盘

    • 先压缩 C 盘,输入一个很小的空间量,比如 8,然后点击压缩按钮

    在这里插入图片描述

    • 会看到在 C 盘旁边生成一个未分配的盘

    在这里插入图片描述

    • 此时再右键 C 盘,看到扩展卷按钮可点击

    在这里插入图片描述

    扩展 C 盘

    • 选择磁盘,把未分配的盘放到已选,再下一步,即可

    在这里插入图片描述

    • 点击完成,即可

    在这里插入图片描述

    • 此时发现未分配的盘没有了,合并到 C 盘了

    在这里插入图片描述

    展开全文
  • 按照这篇博客确实可以安装好IDM,还可以自动在Google Chrome中装好扩展插件进行扩展。但是没有自动扩展到FireFox,所以我是手动自己添加的FireFox的扩展,步骤如下: (1)点击FireFox的右上角的三条横线处,点击...

    最近做毕设,需要用到一个国外的数据集LANL异常检测数据集。本来以为不过是下载个数据集,能有多麻烦,结果自己下载的时候差点没被整吐。当然这也跟我平时不怎么关注那些下载提速用的软件和插件有关。
    后来同学跟我推荐了IDM,下载的时候也耗费了一些神气。因为网上的相关博客很多,但是很多博客只是介绍IDM安装过程的其中一部分。所以尽管看了很多篇博客,还是遇到了一些问题,很多博客没有标清楚安装的全过程,所以漏掉一些步骤或者步骤错误就容易导致安装错误。
    最后找到了一篇步骤详细的安装博客,按照这篇博客的步骤操作,可以安装成功。
    按照这篇博客确实可以安装好IDM,还可以自动在Google Chrome中装好扩展插件进行扩展。但是没有自动扩展到FireFox,所以我是手动自己添加的FireFox的扩展,步骤如下:
    (1)点击FireFox的右上角的三条横线处,点击附加组件。
    (2)进入后,点击右上角的图标,选择从文件安装附加组件。
    在这里插入图片描述(3)进入我们安装IDM的路径下,一共有三个xpi。选择其中和FireFox版本匹配的xpi。53版本或以上选择3.xpi ,所以我选用第三个xpi。
    在这里插入图片描述
    (4)在添加xpi文件后,会跳出添加的窗口,点击添加。
    (5)添加成功后的界面是这样的。
    在这里插入图片描述(6)打开IDM软件,查看选项中的使用高级浏览器集成是否被勾选,以及下面的FireFox浏览器是否被勾选。正常按照博客的步骤来这些都是被勾选好了的。
    在这里插入图片描述
    参考的博客链接如下:
    https://www.jianshu.com/p/5cf695827ff6?utm_campaign=maleskine&utm_content=note&utm_medium=seo_notes&utm_source=recommendation

    展开全文
  • 所谓扩展,并不只是为了设计用户过多时不会崩溃的Web站点。它所要设计的,是在业务需求增长时,不会崩溃的公司。本书的作者们战斗在我们这个时代最成功的Internet公司的前沿,他们与我们分享的,并不只是如何在竞争...
  • 浏览器扩展插件:「油猴」使用详解 ( Tampermonkey )

    万次阅读 多人点赞 2019-09-03 18:11:47
    Tampermonkey 是一款免费的浏览器扩展和最为流行的用户脚本管理器,它适用于 Chrome, Microsoft Edge, Safari, Opera Next, 和 Firefox。 通过安装一系列脚本改变网站样式,添加强大功能等等,例如: ① CSDN浏览...

    最近发现一款浏览器神器插件:油猴 ,英文名:Tampermonkey

    一、介绍

    Tampermonkey 是一款免费的浏览器扩展和最为流行的用户脚本管理器,它适用于 Chrome, Microsoft Edge, Safari, Opera Next, 和 Firefox

    通过安装一系列脚本改变网站样式添加强大功能等等,例如:

    CSDN浏览文章去除G告、自动展开全文以及免登录
    百度搜索过滤广告,渲染样式
    去除某云下载文件需下载客户以及限速问题
    全网音乐一键下载
    各大视频网站VIP视频解析
    网站限制去除(复制,剪贴 => 百度文库、360doc文档)
    知乎网站免登录浏览
    百度贴吧免登录浏览(楼中楼)
    斗鱼、虎牙精简版,CPU加速

    二、安装

    ① 进入官网 Tampermonkey,选择对应的浏览器版本,直接点击【下载】,完成安装;

    ② 如果你使用的是Chrome,可以在 应用商店 搜索 Tampermonkey 并添加为扩展程序;

    添加成功:
    在这里插入图片描述
    三、添加脚本

    安装成功之后,可以点击菜单下的 获取新脚本 安装脚本,或者 添加新脚本 自己写脚本;
    在这里插入图片描述
    点击 获取新脚本
    在这里插入图片描述
    这里 给出了 脚本源地址 ,强烈推荐使用 :GreasyFork ,支持中文界面,中文检索,并且持续更新。
    在这里插入图片描述
    接下来,我们尝试安装一个脚本:去除百度多余广告,渲染样式
    在这里插入图片描述
    点击 安装此脚本
    在这里插入图片描述
    这里因为安装过,所以重新安装,反之点击安装即可:
    在这里插入图片描述
    安装脚本成功:
    在这里插入图片描述配置自定义功能:
    在这里插入图片描述
    我们对比一下安装之前和之后的效果:
    在这里插入图片描述
    在这里插入图片描述
    很明显,去除了多余的G告,改写侧边栏样式,删除下划线以及增加favicon图标

    如需关闭,点击按钮,刷新:
    在这里插入图片描述
    四、脚本推荐

    爱奇艺付费会员视频解析,点击左边VIP按钮:
    在这里插入图片描述
    解析成功:
    在这里插入图片描述
    某云直链下载:
    在这里插入图片描述
    音乐平台VIP试听音乐一键下载:
    在这里插入图片描述
    在这里插入图片描述

    • 购物党自动比价工具-领取淘宝内部券
      在这里插入图片描述

    • 网页解除限制(复制,剪贴)

      例如:百度文库 、360doc文档等等

    • 贴吧全能助手
      百度贴吧 tieba.baidu.com 看贴(包括楼中楼)无须登录,完全去除扰眼和各类广告模块,全面精简并美化各种贴吧页面,去除贴吧帖子里链接的跳转,按发帖时间排序,查看贴吧用户发言记录,贴子关键字屏蔽,移除会员彩名,直接在当前页面查看原图,可缩放,可多开,可拖拽

    • 虎牙精简斗鱼精简
      在这里插入图片描述
      在这里插入图片描述目前总结以上自认为不错的脚本,如有更多不错的脚本,欢迎下方留言推荐 ~

    展开全文
  • pbfunc外部函数扩展(1.2.2.17) 2016-09-30

    热门讨论 2016-09-30 16:41:16
    pbfunc外部函数扩展是专给PowerBuilder各个版本可以使用的外部扩展库,部分功能其它开发工具也可以使用,主要功能如下: 1.以非图片方式在Datawindow中显示QR二维码、以图片方式生成二维码 2.GBK和UTF-8编码相互...
  • Duilib ListUI扩展 源码

    千次下载 热门讨论 2014-03-23 22:56:45
    Duilib中自带的ListUI只支持文本功能,不适合项目应用,此代码用于扩展DUILIB中的ListUI
  • SuperSlide2.1的全部效果, 20个基础效果+72个扩展效果
  • 董付国老师系列教材《Python程序设计(第2版)》(ISBN:9787302436515)、《Python可以这样学》(ISBN:9787302456469)配套视频,通过大量案例讲解Python 3.5.x和3.6.x扩展库pillow的用法。
  • OpenGL开发包扩展包合集

    热门讨论 2014-09-23 16:24:07
    OpenGL开发包扩展包合集包含windows下opengl开发所需要的函数库,包括glut3.7 glew1.5 glew 1.11 glee5.4 glee在sourceforge上托管的相关代码和另外两个头文件glext.h和glxext.h
  • Kotlin系列之扩展函数

    万次阅读 2018-04-17 00:22:18
    简述: 今天带来的是Kotlin浅谈系列的第五弹,这讲主要是讲利用Kotlin中的扩展函数特性让我们的代码变得更加简单和整洁。扩展函数是Kotlin语言中独有的新特性,利用它可以减少很多的样板代码,大大提高开发的效率;...
  • 正因如此,当把一个低精度的数据类型转成一个高精度的数据类型时,必然会涉及到如何扩展位数的问题。这里有两种解决方案: (1)补零扩展:填充一定位数的0。 (2)补符号位扩展:填充一定位数的符号位(非负数填充0...
  • chrome快捷工具扩展

    热门讨论 2012-05-31 00:52:54
    chrome快捷工具扩展大镜查看网页上的任意图片,将其设为桌面背景,查看其原始大小
  • 简单的说就是在对现有系统影响最小的情况下,系统功能可持续扩展及提升的能力,讲扩展性之前,我先讲下扩展性和伸缩性的区别,因为这两个点经常有人会混淆; 扩展性:指对现有系统影响最小的情况下,系统功能可持续...
  • Chrome扩展安装包Postman 绿色版

    万次下载 热门讨论 2014-12-10 15:39:52
    收集到的可用的Postman的可用安装包,安装包内有使用说明。很多人无法在Chrome商店安装,下载的又无法使用的这个应该可以解决了。
  • AxureRP谷歌浏览器扩展程序文件
  • kotlin的扩展方法,其实是以java的静态方法形式存在的,也就是说如果要用java调用kotlin的扩展方法,和调用静态函数一样 调用扩展属性也是相同的道理 举个例子,我们在某个kotlin文件(文件名为Utils.kt)里为...
  • 常见的扩容方式有:字扩展,位扩展,字位同时扩展 主存与CPU的连接通过数据总线,地址总线,控制总线与CPU相连接。如下图所示: 其中: 地址线决定了CPU可寻址的最大内存空间。 控制总线(读写)指出总线周期的类型...
  • 扩展,位扩展和字位扩展

    万次阅读 多人点赞 2020-04-07 08:39:06
    扩展扩展指的是用多个存储器器件对字长进行扩充,指的是用多个存储器器件对字长进行扩充,如用2个16KX4位芯片组成16KX8位的存储器。 位扩展的连接方式是将多片存储器的地址、片选CS、读写控制端R/W相应并联,...
  • 软件系统的可扩展性设计

    万次阅读 2020-01-16 14:33:50
    系统的可扩展性一、可扩展性的设计1.可扩展性设计的优势2.可扩展性设计的目的3.可扩展性设计的两种方法二、可扩展性设计的形式1.分层架构2.消息队列3.远程调用4.开放平台三、企业级系统的平台化设计1.分层设计2.模块...
  • Vue 组件扩展

    千次阅读 2018-06-18 22:34:30
    最近,新项目架构搭建在扩展组件的场景中:图表使用了extends方式,而公共业务server和view之间使用了mixins方式。对于二者的选择,我们通常会解释为extends的优先级高于mixins,但其真实的差异是由于其合并策略不同...
  • 存储器容量扩展 主存,主存储器,又称内存储器 存储器芯片的容量是有限的,为了满足实际存储器的容量要求,需要对存储器进行扩展。 主存扩展:将存储芯片连在一起组成足够的容量 存储器容量扩展的主要方法有: 位...
  • 扩展和位扩展

    千次阅读 多人点赞 2019-12-19 23:19:51
    扩展扩展是指增加存储字长,例如,2片1K X4位的芯片,可以组成1K X 8位的存储器。满足下列条件时,采用位扩展的方式: 只加长每个存储单元的字长,而不增加存储单元的数量 芯片数=设计要求的存储器容量/选择...
  • 存储器(二)——存储容量扩展 前言:(基本概念) 存储空间:CPU决定,操作系统 存储器:用户需求决定(4G,8G)(8G的存储器1各芯片能否完成用户需求?如果不能完成,存储器由若干个芯片组成) 存储芯片:芯片...
  • 芯片的字扩展,位扩展,字位扩展

    千次阅读 2020-09-05 09:08:22
    芯片的引脚构成: 地址线(Address一般用A表示,从A0开始) 数据线(Data一般用D表示,从D0开始) 控制线(片选信号) 其他信号(电源端Vcc,接地端GUD,也许还会有空端NC,用于固定芯片) 芯片的扩展 一般在进行画芯片的扩展...
  • 用 pecl install 安装扩展,再用 docker-php-ext-enable 命令 启用扩展 #3、其他扩展 一些既不在 PHP 源码包,也不再 PECL 扩展仓库中的扩展,可以通过下载扩展程序源码,编译安装的方式安装 # 扩展版本号定义 #...
  • 的 C 扩展开发惯例」,「阅读原文」查看交流实录 「文末高能」 编辑 | 嘉仔 目录 1 简介 1.1 Python扩展模块的用途和优点 1.2 设计扩展模块的流程 2 setup.py脚本 3 函数接口、参数传递、简单返回值 3.1...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 3,779,503
精华内容 1,511,801
关键字:

扩展