本文将以车辆三自由度模型为基础，利用扩展卡尔曼滤波，通过车辆的侧向加速度来估计横摆角速度、质心侧偏角、纵向速度等三个参数，通过一个实际的仿真案例来进行具体介绍扩展卡尔曼滤波的使用。
一般地，卡尔曼滤波会选择比较容易获取的参数，来估计不易测量的参数。
在这里，脚主把卡尔曼参数估计仿真分为四个步骤：
1)车辆模型搭建；
2)扩展卡尔曼滤波算法搭建；
3)模型整合及仿真工况设置；
4)仿真及结果分析。
车辆模型搭建
本例中，利用车辆三自由度模型(如下图)进行参数估计，需要知道车辆的输入信号(车轮转角、纵向加速度)和输出信号(侧向加速度)，所以需要自己搭建一个车辆模型来创造这些数据。即对车辆模型输入一个方向盘转角和纵向加车速，得到侧向加速度。
在实车上，这一步是可以忽略的，因为我们可以通过传感器直接测量卡尔曼滤波所需的信号。
脚主暂时选择比较简便的方法，借助carsim中的车辆模型来完成这项工作，仅需要设置好我们关注的车辆基本参数及信号接口即可。
质心到前后轴距离、沿Z轴转动惯量、质量设置位置如下图。
前后轮侧偏刚度设置位置如下图。
方向盘转角到车轮转角的传动比设置位置如下图。
输出接口选择输出横摆角速度、质心侧偏角、纵向车速、方向盘转角、纵向加速度、侧向加速度。前三个是待估计的参数，用于与仿真结果对比；后三个是车辆的输入输出信号，会作为卡尔曼滤波算法的输入。
一直用别人的车辆模型也不合适，后面脚主会自己动手搭建车辆模型，这样就可以避免联合仿真的麻烦，仿真可以全部在simulink中实现了。更重要的是自己搭建车辆模型更加能加深对车辆的理解，这个是商业软件所无法替代的。
扩展卡尔曼滤波算法搭建
扩展卡尔曼滤波算法就是把上文提到的5个核心公式表达出来。再次强调一下：需要使用非线性函数f、h来表示状态方程和输出方程；系统矩阵A、输出矩阵H需要用f、h函数求偏导后的雅克比矩阵表示。
脚主这里借助以前搭建的卡尔曼滤波算法，稍微改动一下，得到如下图的扩展卡尔曼滤波算法。
用5个function表达卡尔曼滤波算法其实比较繁琐，但是可以更好地表达5个公式之间的时序关系，便于初学者理解。
模型整合及仿真工况设置
把上述两部分内容组合起来就是整个基于扩展卡尔曼滤波的参数估计仿真模型，如下图。基本思路就是，carsim模型输出滤波算法所需的信号，然后进行参数估计，输出估计的结果，最后将估计结果与车辆实际信号对比，来验证算法的有效性。
为了验证算法，还需要在carsim中设置一下仿真工况。
1)初始车速为30km/h进行滑行。
2)方向盘转角按下图的曲线执行。
仿真及结果分析
运行模型，得到估计的横摆角速度、质心侧偏角、纵向车速，与车辆实际的状态对比如下图。
横摆角速度估计结果：
质心侧偏角估计结果：
纵向车速估计结果：
从图中可以看出，横摆角速度、质心侧偏角、纵向车速的估计值与实际值基本一致，算法可靠有效。
以上，介绍了扩展卡尔曼滤波算法进行参数估计的一个实例，仅供大家参考。

原始卡尔曼滤波算法（KF）、扩展卡尔曼滤波算法（EKF）以及无迹卡尔曼滤波算法（UKF）三者之间的区别？

原文：https://www.zhihu.com/question/22714163/answer/87629897

 

KF针对于线性高斯的情况，EKF针对于非线性高斯，其是将非线性部分进行一阶泰勒展开，因此忽 略了高阶项，误差较大，UKF是将UT变换与KF结合的产物，它的基础理念是接近一个非线性函数的 概率分布非接近其本身更简单。后两种卡尔曼是针对同一问题的不同思路的解决方案，其实UKF的 能力已经跳出了非线性高斯的范围，其也可以解决非高斯问题，只不过在这方面PF能做的更好，运 算量也更大。

 

1.KF是线性滤波器，不多解释。2.说到非线性卡尔曼滤波器，是由贝叶斯滤波理论得出统一的理论框架:非线性卡尔曼滤波器的状态估计可等效为多维向量积分的计算。积分形式可表示为非线性函数x高斯概率密度函数。但该积分的闭式解很难求解，所以得对高斯加权的多维非线性函数的积分进行近似。近似主要分两大类:非线性函数的近似和高斯概率密度函数的近似。

非线性函数的近似有基于泰勒级数展开的ekf，基于差分的DDF和多项式卡尔曼(比如 雪夫多项式等)。

概率密度函数的近似主要有无迹变换（ukf），容积卡尔曼，中心差分卡尔曼，或是把协方差矩阵分解构成基于平方根的卡尔曼滤波器等。所以卡尔曼滤波器都是对后验密度做假设的次优估计。而粒子滤波，高斯混合滤波器，自适应网格点群滤波器等是不对后验概率密度做任何假设的最优估计。不过计算量挺大的。像基于ukf+srcdkf的位置状态估计，在进行时间更新的时候，就需要更新1700多个数据。要是采用粒子滤波计算量就更大……



图说卡尔曼滤波，一份通俗易懂的教程

原文：https://zhuanlan.zhihu.com/p/39912633

作者：Bzarg

编译：Bot

编者按：卡尔曼滤波（Kalman filter）是一种高效的自回归滤波器，它能在存在诸多不确定性情况的组合信息中估计动态系统的状态，是一种强大的、通用性极强的工具。它的提出者，鲁道夫.E.卡尔曼，在一次访问NASA埃姆斯研究中心时，发现这种方法能帮助解决阿波罗计划的轨道预测问题，后来NASA在阿波罗飞船的导航系统中确实也用到了这个滤波器。最终，飞船正确驶向月球，完成了人类历史上的第一次登月。



本文是国外博主Bzarg在2015年写的一篇图解。虽然是几年前的文章，但是动态定位、自动导航、时间序列模型、卫星导航——卡尔曼滤波的应用范围依然非常广。那么，作为软件工程师和机器学习工程师，你真的了解卡尔曼滤波吗？

什么是卡尔曼滤波？

对于这个滤波器，我们几乎可以下这么一个定论：只要是存在不确定信息的动态系统，卡尔曼滤波就可以对系统下一步要做什么做出有根据的推测。即便有噪声信息干扰，卡尔曼滤波通常也能很好的弄清楚究竟发生了什么，找出现象间不易察觉的相关性。

因此卡尔曼滤波非常适合不断变化的系统，它的优点还有内存占用较小（只需保留前一个状态）、速度快，是实时问题和嵌入式系统的理想选择。

如果你曾经Google过卡尔曼滤波的教程（如今有一点点改善），你会发现相关的算法教程非常可怕，而且也没具体说清楚是什么。事实上，卡尔曼滤波很简单，如果我们以正确的方式看它，理解是很水到渠成的事。

本文会用大量清晰、美观的图片和颜色来解释这个概念，读者只需具备概率论和矩阵的一般基础知识。

我们能用卡尔曼滤波做什么？

让我们举个例子：你造了一个可以在树林里四处溜达的小机器人，为了让它实现导航，机器人需要知道自己所处的位置。



也就是说，机器人有一个包含位置信息和速度信息的状态  ：



注意，在这个例子中，状态是位置和速度，放进其他问题里，它也可以是水箱里的液体体积、汽车引擎温度、触摸板上指尖的位置，或者其他任何数据。

我们的小机器人装有GPS传感器，定位精度10米。虽然一般来说这点精度够用了，但我们希望它的定位误差能再小点，毕竟树林里到处都是土坑和陡坡，如果机器人稍稍偏了那么几米，它就有可能滚落山坡。所以GPS提供的信息还不够充分。



我们也可以预测机器人是怎么移动的：它会把指令发送给控制轮子的马达，如果这一刻它始终朝一个方向前进，没有遇到任何障碍物，那么下一刻它可能会继续坚持这个路线。但是机器人对自己的状态不是全知的：它可能会逆风行驶，轮子打滑，滚落颠簸地形……所以车轮转动次数并不能完全代表实际行驶距离，基于这个距离的预测也不完美。

这个问题下，GPS为我们提供了一些关于状态的信息，但那是间接的、不准确的；我们的预测提供了关于机器人轨迹的信息，但那也是间接的、不准确的。

但以上就是我们能够获得的全部信息，在它们的基础上，我们是否能给出一个完整预测，让它的准确度比机器人搜集的单次预测汇总更高？用了卡尔曼滤波，这个问题可以迎刃而解。

卡尔曼滤波眼里的机器人问题

还是上面这个问题，我们有一个状态，它和速度、位置有关：



我们不知道它们的实际值是多少，但掌握着一些速度和位置的可能组合，其中某些组合的可能性更高：



卡尔曼滤波假设两个变量（在我们的例子里是位置和速度）都应该是随机的，而且符合高斯分布。每个变量都有一个均值  ，它是随机分布的中心；有一个方差  ，它衡量组合的不确定性。



在上图中，位置和速度是不相关的，这意味着我们不能从一个变量推测另一个变量。

那么如果位置和速度相关呢？如下图所示，机器人前往特定位置的可能性取决于它拥有的速度。



这不难理解，如果基于旧位置估计新位置，我们会产生这两个结论：如果速度很快，机器人可能移动得更远，所以得到的位置会更远；如果速度很慢，机器人就走不了那么远。

这种关系对目标跟踪来说非常重要，因为它提供了更多信息：一个可以衡量可能性的标准。这就是卡尔曼滤波的目标：从不确定信息中挤出尽可能多的信息！

为了捕获这种相关性，我们用的是协方差矩阵。简而言之，矩阵的每个值是第  个变量和第  个变量之间的相关程度（由于矩阵是对称的，  和  的位置可以随便交换）。我们用  表示协方差矩阵，在这个例子中，就是  。



用矩阵描述问题

为了把以上关于状态的信息建模为高斯分布（图中色块），我们还需要  时的两个信息：最佳估计  （均值，也就是  ），协方差矩阵  。（虽然还是用了位置和速度两个变量，但只要和问题相关，卡尔曼滤波可以包含任意数量的变量）



接下来，我们要通过查看当前状态（k-1时）来预测下一个状态（k时）。这里我们查看的状态不是真值，但预测函数无视真假，可以给出新分布：



我们可以用矩阵  表示这个预测步骤：



它从原始预测中取每一点，并将其移动到新的预测位置。如果原始预测是正确的，系统就会移动到新位置。

这是怎么做到的？为什么我们可以用矩阵来预测机器人下一刻的位置和速度？下面是个简单公式：



换成矩阵形式：



这是一个预测矩阵，它能给出机器人的下一个状态，但目前我们还不知道协方差矩阵的更新方法。这也是我们要引出下面这个等式的原因：如果我们将分布中的每个点乘以矩阵A，那么它的协方差矩阵会发生什么变化



把这个式子和上面的最佳估计  结合，可得：



外部影响

但是，除了速度和位置，外因也会对系统造成影响。比如模拟火车运动，除了列车自驾系统，列车操作员可能会手动调速。在我们的机器人示例中，导航软件也可以发出停止指令。对于这些信息，我们把它作为一个向量  ，纳入预测系统作为修正。

假设油门设置和控制命令是已知的，我们知道火车的预期加速度  。根据运动学基本定理，我们可得：



把它转成矩阵形式：



 是控制矩阵，  是控制向量。如果外部环境异常简单，我们可以忽略这部分内容，但是如果添加了外部影响后，模型的准确率还是上不去，这又是为什么呢？

外部不确定性

当一个国家只按照自己的步子发展时，它会自生自灭。当一个国家开始依赖外部力量发展时，只要这些外部力量是已知的，我们也能预测它的存亡。

但是，如果存在我们不知道的力量呢？当我们监控无人机时，它可能会受到风的影响；当我们跟踪轮式机器人时，它的轮胎可能会打滑，或者粗糙地面会降低它的移速。这些因素是难以掌握的，如果出现其中的任意一种情况，预测结果就难以保障。

这要求我们在每个预测步骤后再加上一些新的不确定性，来模拟和“世界”相关的所有不确定性：



如上图所示，加上外部不确定性后，  的每个预测状态都可能会移动到另一点，也就是蓝色的高斯分布会移动到紫色高斯分布的位置，并且具有协方差  。换句话说，我们把这些不确定影响视为协方差  的噪声。



这个紫色的高斯分布拥有和原分布相同的均值，但协方差不同。



我们在原式上加入  ：



简而言之，这里：

 是基于  和  校正后得到的预测。

 是基于  和  得到的预测。

现在，有了这些概念介绍，我们可以把传感器数据输入其中。

通过测量来细化估计值

我们可能有好几个传感器，它们一起提供有关系统状态的信息。传感器的作用不是我们关心的重点，它可以读取位置，可以读取速度，重点是，它能告诉我们关于状态的间接信息——它是状态下产生的一组读数。



请注意，读数的规模和状态的规模不一定相同，所以我们把传感器读数矩阵设为  。



把这些分布转换为一般形式：



卡尔曼滤波的一大优点是擅长处理传感器噪声。换句话说，由于种种因素，传感器记录的信息其实是不准的，一个状态事实上可以产生多种读数。



我们将这种不确定性（即传感器噪声）的协方差设为  ，读数的分布均值设为  。

现在我们得到了两块高斯分布，一块围绕预测的均值，另一块围绕传感器读数。



如果要生成靠谱预测，模型必须调和这两个信息。也就是说，对于任何可能的读数  ，这两种方法预测的状态都有可能是准的，也都有可能是不准的。重点是我们怎么找到这两个准确率。

最简单的方法是两者相乘：



两块高斯分布相乘后，我们可以得到它们的重叠部分，这也是会出现最佳估计的区域。换个角度看，它看起来也符合高斯分布：



事实证明，当你把两个高斯分布和它们各自的均值和协方差矩阵相乘时，你会得到一个拥有独立均值和协方差矩阵的新高斯分布。最后剩下的问题就不难解决了：我们必须有一个公式来从旧的参数中获取这些新参数！

结合高斯

让我们从一维看起，设方差为  ，均值为  ，一个标准一维高斯钟形曲线方程如下所示：



那么两条高斯曲线相乘呢？





把这个式子按照一维方程进行扩展，可得：



如果有些太复杂，我们用k简化一下：



 

以上是一维的内容，如果是多维空间，把这个式子转成矩阵格式：



这个矩阵  就是我们说的卡尔曼增益，easy！

把它们结合在一起

截至目前，我们有用矩阵  预测的分布，有用传感器读数  预测的分布。把它们代入上节的矩阵等式中：



相应的，卡尔曼增益就是：



考虑到  里还包含着一个  ，我们再精简一下上式：



最后，  是我们的最佳估计值，我们可以把它继续放进去做另一轮预测：



希望这篇文章能对你有用！

基于扩展卡尔曼滤波算法的室内定位跟踪系统
凌海波，周先存
【摘
要】
摘要：为了解决无线室内定位系统实时跟踪位置坐标误差较大问题，
提出一种基于扩展卡尔曼滤波
(EKF)
算法的室内定位方法。系统采用基于
WiFi
信号指纹定位，然后利用扩展卡尔曼滤波对估算的位置进行滤波，以改善
WiFi
指纹定位方法的精度，达到对目标实时跟踪。仿真和实验结果表明，该算法有
效地改善了系统定位精度，能较好地满足室内定位的需求。
【期刊名称】
河北工程大学学报(自然科学版)
【年
(
卷
),
期】
2016(033)002
【总页数】
5
【关键词】
无线局域网；室内定位；扩展卡尔曼滤波；实时跟踪
无线室内定位技术已经被用于很多基于位置的应用，随着智能手机的普及，其
应用越来越广泛。现有最新理论成果仍然面临很多挑战，
WiFi
信号会受到多径
效应和环境动态变化的影响，在无线局域网络中对目标进行监测并跟踪，由于
在室内复杂环境中，目标的状态会随着时间的变化而变化，并且传感器获取的
数据受到室内环境的影响，此时直接用定位算法计算的位置和实际的位置会有
很大的误差，从而影响了室内定位系统的精度
[1-3]
。跟踪定位系统应以最小的
计算复杂度获得较高的定位精度，文献
[4-6]
提出了利用卡尔曼滤波、扩展卡尔
曼滤波、粒子滤波等算法实现跟踪，其中扩展卡尔曼滤波算法收敛速度快，在
实时跟踪定位中有着很广泛的应用。本文采用扩展卡尔曼滤波算法随定位算法
估算的位置坐标进行滤波处理，进一步提高室内定位的精度。
1
基于
WiFi
信号指纹定位算法描述

文章目录
1 前提介绍
2 通过泰勒展式进行线性化
3 扩展卡尔曼滤波算法（EKF）
4 扩展卡尔曼滤波实例
5 扩展卡尔曼滤波（EKF）公式推导
5.1 预测公式推导
5.2 测量更新公式推导
6 扩展卡尔曼滤波的优缺点
7 参考文献

1 前提介绍

扩展卡尔曼滤波顾名思义是卡尔曼滤波的扩展形式（卡尔曼滤波教程），但是在哪些方面进行扩展呢？这就要从卡尔曼滤波的三个前提条件入手：

状态转移函数必须是线性函数:$x_t = A_t x_{t-1} + B_t u_t + \epsilon_t$
测量函数必须是线性函数:$z_t = C_t x_t + \delta_t$
初始的置信度函数服从正态分布:$b(x_0) \sim N(\mu_0, \Sigma_0)$


扩展卡尔曼滤波对第$1,2$个前提条件进行扩展：

状态转移函数不必是线性函数$x_t = g(u_t, x_{t-1}) + \epsilon_t$
测量函数必须不必是线性函数:$z_t = h(x_t) + \delta_t$


若$X$服从正态分布$X \sim N(x;\mu, \sigma^2)$,那么变换后的变量$Y$服从怎样的分布？


线性变换$Y=aX+b$，那么变量$Y$也服从正态分布$Y \sim N(y;a\mu+b, a^2\sigma^2)$







非线性变换$Y = g(X)$,那么变量$Y$将不再服从正态分布，且解析形式很难求得





扩展卡尔曼滤波与卡尔曼滤波状态转移函数和测量函数对比：

用函数$g$代替矩阵$A_t,B_t$
用函数$h$代替矩阵$C_t$


对于任意函数$g,h$将会导致置信度$bel(x)$不再服从正态分布，那么置信度更新将不再存在封闭解，换句话说贝叶斯滤波将不能进行
那么为了克服上述问题，只能退而求其次，采用将上述两个非线性函数$g,h$进行线性化

2 通过泰勒展式进行线性化

泰勒展开式与线性化：

$\text{在}x=a\text{出泰勒展开形式：}\\
f(x) = f(a) + \frac{f'(a)}{1!}(x-a) + \frac{f^{(2)}(a)}{2!}(x-a)^2 + \cdots + \frac{f^{(n)}(a)}{n!}(x-a)^n + \cdots \\
\downarrow \text{仅保留线性部分}\\
f(x) \approx f(a) + \frac{f'(a)}{1!}(x-a)$
如果$X$服从正态分布$X \sim N(x; \mu, \sigma^2)$，$g(x)$是非线性函数，那么为了线性化$g(x)$，应该在什么位置对$g(x)$进行一阶泰勒展开呢？

答案是应该在$X$的均值出进行展开$(x=\mu)$
这是因为$x$等于均值$\mu$的概率最大
变换如下图：

右下角为$X$服从正态分布$X \sim N(x; \mu, \sigma^2)$，$x$等于均值$\mu$的概率最大(峰值)
右上角为对$x$进行非线性变换$g(x)$,虚线是在$x=\mu$处的一阶泰勒展开式，用来近似实线部分
左上角虚线为通过线性化$g(x)$，求得的$g(x)$的概率分布；实线为实际的概率分布，可见误差在允许的范围内







线性化状态转移函数$x_t = g(u_t, x_{t-1}) + \epsilon_t$

对于高斯分布，后验概率$\mu_{t-1}$是状态变量$x_{t-1}$最可能的状态
所以对$x_t = g(u_t, x_{t-1}) + \epsilon_t$在$\mu_{t-1}$处展开(即$a=\mu_{t-1}$)

$g(u_t, x_{t-1}) \approx g(u_t, \mu_{t-1}) + g'(u_t, \mu_{t-1})(x_{t-1} - \mu_{t-1})\\
\downarrow \text{定义：}G_t := g'(u_t, \mu_{t-1})~~~\text{雅克比矩阵}\\
g(u_t, x_{t-1}) \approx g(u_t, \mu_{t-1}) + G_t(x_{t-1} - \mu_{t-1})\\
\downarrow \\
x_t \approx g(u_t, \mu_{t-1}) + G_t(x_{t-1} - \mu_{t-1}) + \epsilon_t\\
\downarrow \\
x_t \sim N(x_t; g(u_t, \mu_{t-1}) + G_t(x_{t-1} - \mu_{t-1}), R_t)\\
\downarrow \\
\begin{aligned}
&p(x_t|u_t,x_{t-1})\\
&= det(2\pi R_t)^{-\frac{1}{2}}\exp\left \{  -\frac{1}{2} (x_t-g(u_t, \mu_{t-1}) - G_t(x_{t-1} - \mu_{t-1}))^T  R_t^{-1}(x_t-g(u_t, \mu_{t-1}) - G_t(x_{t-1} - \mu_{t-1}))\right \}
\end{aligned}\\
\text{雅克比矩阵：} G_t = g'(u_t, \mu_{t-1}) = 
\begin{bmatrix}
\frac{\partial g_1}{\partial x_1}&  \frac{\partial g_1}{\partial x_2}&  \cdots& \frac{\partial g_1}{\partial x_n} \\ 
\frac{\partial g_2}{\partial x_1}&  \frac{\partial g_2}{\partial x_2}&  \cdots& \frac{\partial g_2}{\partial x_n} \\ 
\vdots & \vdots  & \vdots  & \vdots \\ 
\frac{\partial g_n}{\partial x_1}&  \frac{\partial g_n}{\partial x_2}&  \cdots& \frac{\partial g_n}{\partial x_n} \\ 
\end{bmatrix}_{x_{t-1}=\mu_{t-1}}$


线性化测量函数:$z_t = h(x_t) + \delta_t$

对于高斯分布，$\bar{\mu}_{t}$是状态变量$x_{t}$最可能的状态
所以对$z_t = h(x_t) + \delta_t$在$\bar{\mu}_{t}$处展开(即$a=\bar{\mu}_{t}$)

$h(x_t) \approx h(\bar{\mu}_{t}) + h'(\bar{\mu}_{t})(x_t - \bar{\mu}_{t})\\
\downarrow \text{定义：}H_t := h'(\bar{\mu}_{t})=\frac{\partial h(x_t)}{\partial x_t}|_{x_t = \bar{\mu}_{t}}~~~\text{雅克比矩阵}\\
\downarrow \\
z_t \approx h(\bar{\mu}_{t}) + H_t(x_t - \bar{\mu}_{t}) + \delta_t\\
\downarrow \\
z_t \sim N(z_t; h(\bar{\mu}_{t}) + H_t(x_t - \bar{\mu}_{t}), Q_t)\\
\downarrow \\
\begin{aligned}
&p(z_t|x_t)\\
&= det(2\pi Q_t)^{-\frac{1}{2}}\exp\left \{  -\frac{1}{2} (z_t-h(\bar{\mu}_{t}) - H_t(x_t - \bar{\mu}_{t}))^T  Q_t^{-1}(z_t-h(\bar{\mu}_{t}) - H_t(x_t - \bar{\mu}_{t}))\right \}
\end{aligned}\\
\text{雅克比矩阵：} H_t = h'(\bar{\mu}_{t})= 
\begin{bmatrix}
\frac{\partial h_1}{\partial x_1}&  \frac{\partial h_1}{\partial x_2}&  \cdots& \frac{\partial h_1}{\partial x_n} \\ 
\frac{\partial h_2}{\partial x_1}&  \frac{\partial h_2}{\partial x_2}&  \cdots& \frac{\partial h_2}{\partial x_n} \\ 
\vdots & \vdots  & \vdots  & \vdots \\ 
\frac{\partial h_n}{\partial x_1}&  \frac{\partial h_n}{\partial x_2}&  \cdots& \frac{\partial h_n}{\partial x_n} \\ 
\end{bmatrix}_{x_{t}=\bar{\mu}_{t}}$



3 扩展卡尔曼滤波算法（EKF）

通过上一部分对状态转移函数和观测函数的线性化，使得这两个函数用两个线性函数表示，使得置信度$bel(x_t)$还服从正态分布
这里先给出扩展卡尔曼滤波算法，并与卡尔曼滤波进行对比，具体公式推导见下面


扩展卡尔曼滤波算法







卡尔曼滤波算法





将扩展卡尔曼滤波与卡尔曼滤波进行一下对比：

$A_t\mu_{t-1} + B_tu_t \Rightarrow g(u_t,\mu_{t-1})$
$C_t\bar{\mu_t} \Rightarrow h(\bar{\mu_t} )$
雅可比矩阵$G_t$代替$A_t,B_t$
雅可比矩阵$H_t$代替$C_t$



4 扩展卡尔曼滤波实例

这里采用滤波算法 “扩展卡尔曼滤波（EFK）实例” 讲解的例子
实例：雷达监测空中抛物轨迹

从空中位置$(x(0)=0,y(0)=500)$水平抛射出一个物体（初始水平速度为$v_x(0)=50$，初始竖直速度为$v_y(0)=0$）
物体受重力$g=9.8$和阻尼力（与速度的平方成正比）的影响
水平和竖直阻尼系数分别为$k_x=0.01,k_y=0.05$，不确定度为零均值白噪声$\delta a_x \sim N(0, 0.09),\delta a_y \sim N(0, 0.09)$
在坐标原点处有一雷达，可测得距离$r$,角度$\alpha$, 不确定度为零均值白噪声$\delta r \sim N(0, 64),\delta \alpha \sim N(0, 0.01)$






状态变量: 物体横向位置$x(k)$，物体横向速度$v_x(k)$，物体纵向位置$y(k)$，物体纵向速度$v_y(k)$

$X(k) = [x(k), v_x(k), y(k), v_y(k)]$


状态方程：

$\begin{aligned}
x(k+1) &= x(k) + v_x(k) \cdot T\\
v_x(k+1) &= v_x(k) - (k_x \cdot v_x^2(k) + \delta a_x)\cdot T\\
y(k+1) &= y(k) + v_y(k) \cdot T \\
v_y(k+1) &= v_y(k) + (k_y \cdot v_y^2(k) - g + \delta a_y)\cdot T
\end{aligned}$


观测方程：

$\begin{aligned}
r(k) &= \sqrt{x(k)^2 + y(k)^2 } + \delta r\\
\alpha(k) &= \arctan{\frac{x(k)}{y(k)}} + \delta \alpha\\
\end{aligned}$


$\text{雅克比矩阵：} G_t$

$G_t = 
\begin{bmatrix}
1&  T&  0& 0 \\ 
0&  1 - 2k_x  v_xT&  0& 0\\ 
0&  0&  1& T \\ 
0&  0&  0& 1 + 2k_y  v_yT\\ 
\end{bmatrix}_{x_{t-1}=\mu_{t-1}}$


$\text{雅克比矩阵：} H_t$

$H_t = 
\begin{bmatrix}
\frac{x}{r}&  0&  \frac{y}{r}& 0 \\ 
\frac{1/y}{1+(x/y)^2}&  0&  \frac{-x/y^2}{1+(x/y)^2}& 0 \\ 
\end{bmatrix}_{x_t = \bar{\mu}_{t}}$


仿真时间总为$t=15$，采样周期$T=0.1$, 滤波效果如图：




matlab 代码

clear;
close all;
clc
%% 初值设定
x_0 = 0;                    %初始x位置
y_0 = 500;                  %初始y位置
v_x_0 = 50;                 %初始x速度
v_y_0 = 0;                  %初始y速度
g = 9.8;                    % 重力加速度
k_x = 0.01;                 % 阻尼系数
k_y = 0.05;                 % 阻尼系数
sigma_a_x = 0.09;           % 状态转移不确定度方差
sigma_a_y = 0.09;           % 状态转移不确定度方差
sigma_r = 64;               % 测量不确定度方差
sigma_alpha = 0.01;         % 测量不确定度方差
t = 15;                     % 仿真时间
T = 0.1;                    % 采样周期 
len = fix(t/T);             % 仿真步数
%% 真实轨迹
X = zeros(len, 4);
X(1,:) = [x_0, v_x_0, y_0, v_y_0];
for k = 2 : len
    x_k = X(k-1,1);
    v_x_k = X(k-1,2);
    y_k = X(k-1,3);
    v_y_k = X(k-1,4);
    x_k = x_k + v_x_k * T;
    v_x_k = v_x_k - (k_x*v_x_k^2 + sqrt(sigma_a_x)*randn(1,1)) * T;
    y_k = y_k + v_y_k * T;
    v_y_k = v_y_k + (k_y*v_y_k^2 - g + sqrt(sigma_a_y)*randn(1,1)) * T;   
    X(k,:) = [x_k, v_x_k, y_k, v_y_k];
end
X_temp = X;
%% 雷达测量
Z = zeros(len, 2);
for k = 1 : len
    x_k = X(k,1);
    y_k = X(k,3);
    r = sqrt(x_k^2 + y_k^2) + sqrt(sigma_r)*randn(1,1);
    alpha = atan(x_k / y_k) * 180 / pi + sqrt(sigma_alpha)*randn(1,1);
    Z(k,:) = [r, alpha];
end
%% EKF 扩展卡尔曼滤波
R_k = diag([0; sigma_a_x; 0; sigma_a_y]);	% 状态转移误差的协方差矩阵
Q_k = diag([sigma_r, sigma_alpha]);         % 测量函数误差的协方差矩阵
sigma_k = 10 * eye(4);                      % 最优状态协方差矩阵
sigma_bar_k= 10 * eye(4);                   % 预测状态协方差矩阵
mu_k = [0, 40, 400, 0]';                    % 最优状态均值矩阵
mu_bar_k = zeros(4,1);                      % 预测的状态均值矩阵
z_k = zeros(4,1);                           % 观测矩阵
X_est = zeros(len,4);                       %EKF后的状态存储
for k = 1 : len
    % 1 状态预测
    x1 = mu_k(1) + mu_k(2)*T;
    v_x1 = mu_k(2) - (k_x*mu_k(2)^2)*T;
    y1 = mu_k(3) + mu_k(4)*T;
    v_y1 = mu_k(4) + (k_y*mu_k(4)^2 - g)*T;
    mu_bar_k = [x1; v_x1; y1; v_y1];            % 预测的均值   
    G_k = zeros(4,4);                       % 状态雅可比矩阵
    G_k(1,1) = 1; G_k(1,2) = T;
    G_k(2,2) = 1 - 2*k_x*v_x1*T;
    G_k(3,3) = 1; G_k(3,4) = T;
    G_k(4,4) = 1 + 2*k_y*v_y1*T;    
    sigma_bar_k = G_k * sigma_k * G_k' + R_k;
    % 2 观测更新
    r = sqrt(x1*x1+y1*y1);
    alpha = atan(x1/y1)*180/pi;
    z_k = [r, alpha]';                      % h(\bar{\mu}_t) 
    H_k = zeros(2,4);                       % 状态雅可比矩阵
    x = mu_k(1); y = mu_k(3); 
    H_k(1,1) = x / r;   H_k(1,3) = y / r;
    H_k(2,1) = (1/y)/(1 + (x/y)^2);   H_k(2,3) = (-x/(y^2))/(1 + (x/y)^2);
    K_k = sigma_bar_k * H_k' * (H_k * sigma_bar_k * H_k' + Q_k)^(-1);
    mu_k = mu_bar_k + K_k * (Z(k,:)' - z_k);
    sigma_k = (eye(4) - K_k * H_k) * sigma_bar_k;
    % 3 存储EKF后的值
    X_est(k,:) = mu_k;
end
%% 绘图 
figure, hold on, grid on;
plot(X(:,1),X(:,3),'-g');                                       %真实位置
plot(Z(:,1).*sin(Z(:,2)*pi/180), Z(:,1).*cos(Z(:,2)*pi/180),'-b');   %观测位置
plot(X_est(:,1),X_est(:,3), 'r');                               %最优位置
xlabel('X'); 
ylabel('Y'); 
title('EKF simulation');
legend('real', 'measurement', 'ekf estimated');
axis([-5,230,290,530]);


5 扩展卡尔曼滤波（EKF）公式推导

这里的公式推导用卡尔曼滤波公式进行类比，详细过程可以结合卡尔曼滤波公式的推导过程

$\text{Step 1: 预测}\left\{\begin{matrix}
\bar{\mu}_t = g(u_t, \mu_{t-1})\\ 
\bar{\Sigma}_t = G_t\Sigma_{t-1} G_t^T+R_t
\end{matrix}\right.\\
\text{Step 2: 测量更新}\left\{\begin{matrix}
K_t =  \bar{\Sigma}_t H_t^T(H_t \bar{\Sigma}_t H_t^T + Q_t)^{-1} \\ 
\mu_t = \bar{\mu}_t + K_t(z_t - h(\bar{\mu}_{t}))\\
\Sigma_t =  (I - K_tH_t)\bar{\Sigma}_t 
\end{matrix}\right.$

5.1 预测公式推导

预测公式：

$\bar{bel}(x_t) = \int p(x_t|x_{t-1},u_t)bel(x_{t-1})dx_{t-1}\\
\text{卡尔曼滤波：}p(x_t|x_{t-1},u_t) \sim N(x_t; A_t x_{t-1} + B_t u_t, R_t)\\
bel(x_{t-1}) \sim N(x_{t-1}; u_{t-1}, \Sigma_{t-1})\\
\text{扩展卡尔曼滤波：}p(x_t|x_{t-1},u_t) \sim N(x_t; g(u_t, \mu_{t-1}) + G_t(x_{t-1} - \mu_{t-1}), R_t)\\
bel(x_{t-1}) \sim N(x_{t-1}; u_{t-1}, \Sigma_{t-1})$
不同之处对比：$A_t x_{t-1} + B_t u_t \Leftrightarrow  g(u_t, \mu_{t-1}) + G_t(x_{t-1} - \mu_{t-1})$

$A_t \Leftrightarrow G_t$
$B_t u_t \Leftrightarrow  g(u_t, \mu_{t-1}) - G_t \mu_{t-1}$


那么扩展卡尔曼滤波预测步的公式可以根据卡尔曼滤波公式进行类比

$\text{Step 1: 预测}\left\{\begin{matrix}
\bar{\mu}_t = A_t\mu_{t-1}+B_t u_t \Rightarrow G_t\mu_{t-1}+g(u_t, \mu_{t-1}) - G_t \mu_{t-1}= g(u_t, \mu_{t-1})\\ 
\bar{\Sigma}_t = A_t\Sigma_{t-1} A_t^T+R_t \Rightarrow  G_t\Sigma_{t-1} G_t^T+R_t \end{matrix}\right.\\
\downarrow\\
\text{Step 1: 预测}\left\{\begin{matrix}
\bar{\mu}_t = g(u_t, \mu_{t-1})\\ 
\bar{\Sigma}_t = G_t\Sigma_{t-1} G_t^T+R_t
\end{matrix}\right.$

5.2 测量更新公式推导

测量更新公式：

$bel(x_t) = \eta p(z_t|x_t)\bar{bel}(x_t)~~~(\eta:\text{用于归一化})\\
\text{卡尔曼滤波：} p(z_t|x_t) \sim N(z_t;C_tx_t,Q_t)\\
\bar{bel}(x_t) \sim N(x_t;\bar{\mu}_t,\bar{\Sigma}_t)\\
\text{扩展卡尔曼滤波：}p(x_t|x_{t-1},u_t) \sim N(z_t; h(\bar{\mu}_{t}) + H_t(x_t - \bar{\mu}_{t}), Q_t)\\
\bar{bel}(x_t) \sim N(x_t;\bar{\mu}_t,\bar{\Sigma}_t)$
不同之处对比：$C_tx_t \Leftrightarrow  h(\bar{\mu}_{t}) + H_t(x_t - \bar{\mu}_{t})$

$C_t \Leftrightarrow H_t$
$C_t \bar{\mu}_{t}\Leftrightarrow  h(\bar{\mu}_{t}) + H_t( \bar{\mu}_{t} - \bar{\mu}_{t}) = h(\bar{\mu}_{t})$


那么扩展卡尔曼滤波预测步的公式可以根据卡尔曼滤波公式进行类比

$\text{Step 2: 测量更新}\left\{\begin{matrix}
K_t =\bar{\Sigma}_t C_t^T(C_t \bar{\Sigma}_t C_t^T + Q_t)^{-1} \Rightarrow \bar{\Sigma}_t H_t^T(H_t \bar{\Sigma}_t H_t^T + Q_t)^{-1} \\ 
\mu_t =\bar{\mu}_t + K_t(z_t - C_t\bar{\mu}_t) \Rightarrow \bar{\mu}_t + K_t(z_t - h(\bar{\mu}_{t}))\\
\Sigma_t =(I - K_tC_t)\bar{\Sigma}_t \Rightarrow  (I - K_tH_t)\bar{\Sigma}_t 
\end{matrix}\right.\\
\downarrow\\
\text{Step 2: 测量更新}\left\{\begin{matrix}
K_t =  \bar{\Sigma}_t H_t^T(H_t \bar{\Sigma}_t H_t^T + Q_t)^{-1} \\ 
\mu_t = \bar{\mu}_t + K_t(z_t - h(\bar{\mu}_{t}))\\
\Sigma_t =  (I - K_tH_t)\bar{\Sigma}_t 
\end{matrix}\right.$

6 扩展卡尔曼滤波的优缺点

优点：计算简单效率高
缺点：对状态转移函数和测量函数的线性近似，使得结果存在误差


原函数不确定度（方差）越大，近似效果越差，见下图：

右下角$p(x)$的分布均值相同，方差不同
右上角变换函数$y=g(x)$相同
左上角$p(y)$的分布，下图误差小于上图，原因在于下图的方差较小










原函数局部非线性越强，近似效果越差，见下图：

右下角$p(x)$的分布均值不同，方差相同
右上角变换函数$y=g(x)$相同
左上角$p(y)$的分布，下图误差小于上图，原因在于下图变换函数的局部非线性越弱








总结：状态变量的不确定度（方差）对EKF影响很大，尽量保持一个小的不确定度

7 参考文献

Thrun, & Sebastian. (2005). Probabilistic robotics.
滤波算法 “扩展卡尔曼滤波（EFK）实例” 讲解