扩展卡尔曼滤波程序示例(matlab)
2018-03-04 19:14:04
matlab写的一个扩展卡尔曼滤波程序，状态方程为线性，观测方程非线性，最后输出图片以便观察是否收敛，分享给大家参考。还有一个C++版本的。
收起
扩展卡尔曼滤波代码和数据
热门讨论 2017-10-26 22:40:06
基于扩展卡尔曼滤波的车辆追踪项目，C++实现，CTRV模型，激光雷达和雷达传感器融合，详情见博客http://blog.csdn.net/adamshan/article/details/78359048
收起
UKF

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

万次阅读 多人点赞 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

收起

展开全文

EKF 自适应EKF 信号处理

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加密...
收起

Windows 扩展 C 盘扩展卷按钮显示灰色怎么办

千次阅读 2020-02-14 12:25:44

压缩 C 盘 ... 会看到在 C 盘旁边生成一个未分配的盘此时再右键 C 盘，看到...扩展 C 盘选择磁盘，把未分配的盘放到已选，再下一步，即可点击完成，即可此时发现未分配的盘没有了，合并到 C 盘了 ...

压缩 C 盘

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

在这里插入图片描述

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

在这里插入图片描述

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

在这里插入图片描述

扩展 C 盘

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

在这里插入图片描述

点击完成，即可

在这里插入图片描述

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

在这里插入图片描述

收起

展开全文

windows

【软件安装】IDM安装并扩展到FireFox和Google Chrome

千次阅读 2020-03-08 10:34:12

按照这篇博客确实可以安装好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

收起

展开全文

idm

可扩展的艺术——现代企业的Web架构、流程及组织，完整扫描版
千次下载 热门讨论 2014-03-11 18:47:46
所谓扩展，并不只是为了设计用户过多时不会崩溃的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个扩展效果
热门讨论 2014-07-07 09:34:58
SuperSlide2.1的全部效果, 20个基础效果+72个扩展效果
收起
Python可以这样学（第七季：pillow扩展库图像编程）
千人学习 2017-06-12 08:38:38
董付国老师系列教材《Python程序设计（第2版）》（ISBN：9787302436515）、《Python可以这样学》（ISBN：9787302456469）配套视频，通过大量案例讲解Python 3.5.x和3.6.x扩展库pillow的用法。
收起
python工程师 python爬虫 python python教程 python安装
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语言中独有的新特性，利用它可以减少很多的样板代码，大大提高开发的效率；...
收起
Kotlin
补码/反码、零扩展和符号位扩展（Zero extension and Sign extension）
千次阅读 2019-11-13 22:02:35
正因如此，当把一个低精度的数据类型转成一个高精度的数据类型时，必然会涉及到如何扩展位数的问题。这里有两种解决方案：（1）补零扩展：填充一定位数的0。（2）补符号位扩展：填充一定位数的符号位（非负数填充0...
收起
C/C++
chrome快捷工具扩展
热门讨论 2012-05-31 00:52:54
chrome快捷工具扩展大镜查看网页上的任意图片，将其设为桌面背景，查看其原始大小
收起
大型网站架构核心要素之扩展性：可扩展架构
万次阅读 2020-05-26 15:30:40
简单的说就是在对现有系统影响最小的情况下，系统功能可持续扩展及提升的能力，讲扩展性之前，我先讲下扩展性和伸缩性的区别，因为这两个点经常有人会混淆；扩展性：指对现有系统影响最小的情况下，系统功能可持续...
收起
Chrome扩展安装包Postman 绿色版
万次下载 热门讨论 2014-12-10 15:39:52
收集到的可用的Postman的可用安装包，安装包内有使用说明。很多人无法在Chrome商店安装，下载的又无法使用的这个应该可以解决了。
收起
AxureRP谷歌浏览器扩展程序文件
2014-11-17 19:33:02
AxureRP谷歌浏览器扩展程序文件
收起
java调用kotlin的扩展方法、扩展属性、扩展函数
千次阅读 2020-05-13 11:36:22
kotlin的扩展方法，其实是以java的静态方法形式存在的，也就是说如果要用java调用kotlin的扩展方法，和调用静态函数一样调用扩展属性也是相同的道理举个例子，我们在某个kotlin文件（文件名为Utils.kt）里为...
收起
kotlin教程 kotlin技巧
【计算机组成原理】| 第四章存储器 | 字扩展和位扩展
千次阅读 多人点赞 2020-11-07 20:56:40
常见的扩容方式有：字扩展，位扩展，字位同时扩展主存与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，但其真实的差异是由于其合并策略不同...
收起
vue vue合并策略
计算机组成原理——存储器容量扩展（字扩展、位扩展、字位扩展）
千次阅读 2021-06-28 19:14:30
存储器容量扩展主存，主存储器，又称内存储器存储器芯片的容量是有限的，为了满足实际存储器的容量要求，需要对存储器进行扩展。主存扩展：将存储芯片连在一起组成足够的容量存储器容量扩展的主要方法有：位...
收起
字扩展和位扩展
千次阅读 多人点赞 2019-12-19 23:19:51
位扩展位扩展是指增加存储字长，例如，2片1K X4位的芯片，可以组成1K X 8位的存储器。满足下列条件时，采用位扩展的方式：只加长每个存储单元的字长，而不增加存储单元的数量芯片数=设计要求的存储器容量/选择...
收起
linux
存储器容量扩展——位扩展、字扩展
千次阅读 2020-06-19 11:18:48
存储器（二）——存储容量扩展前言：（基本概念）存储空间：CPU决定，操作系统存储器：用户需求决定（4G，8G）（8G的存储器1各芯片能否完成用户需求？如果不能完成，存储器由若干个芯片组成）存储芯片：芯片...
收起
芯片的字扩展，位扩展，字位扩展
千次阅读 2020-09-05 09:08:22
芯片的引脚构成：地址线(Address一般用A表示,从A0开始) 数据线(Data一般用D表示，从D0开始) 控制线（片选信号）其他信号(电源端Vcc，接地端GUD,也许还会有空端NC,用于固定芯片) 芯片的扩展一般在进行画芯片的扩展...
收起
单片机
通过docker自定义安装php7.x并且安装源码扩展、pecl扩展、及其他扩展
千次阅读 2019-07-11 19:41:29
用 pecl install 安装扩展，再用 docker-php-ext-enable 命令启用扩展 #3、其他扩展一些既不在 PHP 源码包，也不再 PECL 扩展仓库中的扩展，可以通过下载扩展程序源码，编译安装的方式安装 # 扩展版本号定义 #...
收起
php docker
Python 扩展使用 C/C++ 给 Python 写扩展模块的方法
万次阅读 2017-11-15 00:00:00
的 C 扩展开发惯例」，「阅读原文」查看交流实录「文末高能」编辑 | 嘉仔目录 1 简介 1.1 Python扩展模块的用途和优点 1.2 设计扩展模块的流程 2 setup.py脚本 3 函数接口、参数传递、简单返回值 3.1...
收起

扩展卡尔曼滤波程序示例(matlab)

扩展卡尔曼滤波代码和数据

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

1 简介

2 算法介绍

2.1 泰勒级数展开

2.1 EKF

3 推导

4 实际应用

5 参数估计（参数学习）

6 总结

7 参考文献

pbfunc外部函数扩展(1.2.2.8) 2015-10-18

Windows 扩展 C 盘扩展卷按钮显示灰色怎么办

压缩 C 盘

扩展 C 盘

【软件安装】IDM安装并扩展到FireFox和Google Chrome

可扩展的艺术——现代企业的Web架构、流程及组织，完整扫描版

浏览器扩展插件：「油猴」使用详解 ( Tampermonkey )

pbfunc外部函数扩展(1.2.2.17) 2016-09-30

Duilib ListUI扩展 源码

SuperSlide2.1 20个基础效果+72个扩展效果

Python可以这样学（第七季：pillow扩展库图像编程）

OpenGL开发包扩展包合集

Kotlin系列之扩展函数

补码/反码、零扩展和符号位扩展（Zero extension and Sign extension）

chrome快捷工具扩展

大型网站架构核心要素之扩展性：可扩展架构

Chrome扩展安装包Postman 绿色版

AxureRP谷歌浏览器扩展程序文件

java调用kotlin的扩展方法、扩展属性、扩展函数

【计算机组成原理】| 第四章 存储器 | 字扩展和位扩展

字扩展，位扩展和字位扩展

软件系统的可扩展性设计

Vue 组件扩展

计算机组成原理——存储器容量扩展（字扩展、位扩展、字位扩展）

字扩展和位扩展

存储器容量扩展——位扩展、字扩展

芯片的字扩展，位扩展，字位扩展

通过docker自定义安装php7.x并且安装源码扩展、pecl扩展、及其他扩展

Python 扩展使用 C/C++ 给 Python 写扩展模块的方法

扩展

Duilib ListUI扩展源码

【计算机组成原理】| 第四章存储器 | 字扩展和位扩展