卡尔曼滤波算法简介

1.卡尔曼滤波器--最优状态估计
2.非线性系统
- 2.1扩展卡尔曼滤波器EKF
- 2.2 无迹卡尔曼滤波器UKF
- 2.3 粒子滤波器
3.总结

参考自B站MATLAB中国及《视觉SLAM十四讲》第9章9.1

1.卡尔曼滤波器–最优状态估计

假如有一个自动驾驶汽车比赛，要求参赛汽车根据GPS测量定位分别在100种地形上行驶1公里，在每种地形上都尽量停靠在1公里终点处。计算100次的平均最终位置，取位置方差最小且平均位置最接近1公里的队伍获胜。

GPS的定位是比较粗糙的，误差较大，为了赢得比赛，期望获得尽量准确的位置估计。一个汽车系统可以简化成如下形式，输入是油门，输出是位置。

上图所示的汽车系统有比较多的状态量，其模型可表示为：

只保留速度作为输入，输出是位置后的简化模型为：

上式中：μk\mu_kμk是速度，xkx_kxk是预测量，yky_kyk是测量量。
理想情况下测量值等于预测值，因此yk=xky_k=x_kyk=xk,因此C=1C=1C=1。为了让汽车停靠位置尽量靠近终点，因此必须得到尽可能准确的yyy。

实际场景还需考虑噪声，对于测量量yky_kyk，使用GPS测量时难免会有测量误差，记为vk∼N(0,R),R=σv2v_k\sim N(0,R),R=\sigma _v^2vk∼N(0,R),R=σv2。同样对于预测变量xkx_kxk，因xkx_kxk与速度有关，速度有可能受风速等的影响，必须考虑这个过程噪声，记为ωk∼N(0,Q),Q=σω2\omega_k \sim N(0, Q),Q=\sigma _ \omega^2ωk∼N(0,Q),Q=σω2。虽然不能准确的确定噪声的具体值，但可以知道噪声值的概率分布服从高斯分布。

上述介绍的是实际的汽车动力学模型，这个模型我们是不知道的，只能根据假设的模型来预测计算。

这里如何根据带有噪声的测量量和有噪声的预测量来尽可能准确的估计汽车的真实位置，正是kalman滤波可以做的事情。

如上图，k−1k-1k−1时刻的估计位置x^k−1\hat{x}_{k-1}x^k−1方差较小，预测位置的不确定性较小，到了第kkk时刻，预测值x^k\hat{x}_kx^k的不确定性变大，测量值yky_kyk的不确定性相对较小，将预测量与测量量概率分布相乘，得到新的高斯分布，取其均值作为汽车在kkk的位置是比较准确的。

预测量与测量量概率密度函数的乘法与如下所示的离散Kalman滤波方程有关，
x^k=Ax^k−1+Bμk+Kk(yk−C(Ax^k−1+Bμk))\hat{x}_k = A\hat{x}_{k-1} +B\mu_k+K_k(y_k-C(A\hat{x}_{k-1} +B\mu_k)) x^k=Ax^k−1+Bμk+Kk(yk−C(Ax^k−1+Bμk))
卡尔曼滤波器是为随机系统设计的状态观测器。
上述方程的第一部分表示为x^k−\hat{x}_k^-x^k−，取决于k−1k-1k−1时刻的输出x^k\hat{x}_kx^k和kkk时刻的输入μk\mu_kμk,如下图：

x^k−\hat{x}_k^-x^k−部分取决于先前时刻的位置估计和当前的速度，因此也称为先验估计。借助x^k−\hat{x}_k^-x^k−,可将x^k\hat{x}_kx^k表示为：
x^k=x^k−+Kk(yk−Cx^k−)\hat{x}_k = \hat{x}_k^- +K_k(y_k-C\hat{x}_k^-) x^k=x^k−+Kk(yk−Cx^k−)

从上式可以看到第二部分使用的是测量量，KalmanKalmanKalman滤波方程由预测值和基于测量量的更新值组成，KkK_kKk称为Kalman 增益系数，x^k\hat{x}_kx^k称为后验估计。如下图：

题外话：关于先验估计与后验估计，可以根据字面意思理解。先验估计顾名思义就是借助先前的经验来估计，譬如1枚硬币，在扔出去之前根据经验就可以估计出来出现正面的概率是1/2。后验估计，就是要通过测量试验后才能进行的估计，譬如已知一个箱子里装了红黑两种颜色的球，但是不知两种球的比率，学过概率后都知道可以从箱子中取样，样本中红黑球的比率可当作箱子中红黑球的比率，这种通过试验来进行的估计就是后验估计。

回过来说汽车的例子，汽车位置估计的卡尔曼方程的先验估计和更新部分可表示为

还记得Q和R分别是过程噪声和测量噪声的方差，P表示的是预测值的协方差，对于单变量系统，其对应的是方差。卡尔曼增益KKK用来调整预测值和测量值在状态估计时所在的比例，当预测值较准确时，希望x^k−\hat{x}_k^-x^k−占比大些，当测量时准确时，希望yk−Cx^k−y_k-C\hat{x}_k^-yk−Cx^k−所占的比例大些。

观察卡尔曼增益KKK的表达式，当测量误差趋近于0时，RRR趋近0，可以得到估计值等于测量值，测量值的概率密度函数将变成1个脉冲值，其对应的位置就是最优估计。

再考虑当PPP趋近于0时，此时卡尔曼增益也等于0，预测值完全取决于先验估计，与测量值无关。

卡尔曼滤波进行状态估计，并不需要所有的先验信息，只需k−1k-1k−1时刻的预测估计，不断进行更新即可。

从前面描述中可以看到测量值是基于GPSGPSGPS的，增加更多的传感器，如IMUIMUIMU等可提高测量值的准确度，得到更准确的位置估计。观察卡尔曼方程，增加传感器时，只需要增加yyy,KKK,CCC的维度即可。因此卡尔曼滤波器也被称为传感器融合算法。

2.非线性系统

第一部分介绍的简化后汽车模型的卡尔曼方程对应的是一个线性系统，实际中，更常见的是非线性系统，如汽车模型考虑摩擦，则预测值与速度的关系将变成非线性的。对应的测量函数也有可能与预测值之间是非线性的。
{xk=Axk−1+Bμk+ωkyk=Cxk+vk=>{xk=f(xk−1,μk)+ωkyk=g(xk)+vk\left\{\begin{matrix} x_k = Ax_{k-1}+B\mu_k+\omega_k \\ y_k = Cx_k + v_k \end{matrix}\right.=>\left\{\begin{matrix} x_k = f(x_{k-1}, \mu_k)+\omega_k \\ y_k=g(x_k) + v_k \end{matrix}\right. {xk=Axk−1+Bμk+ωkyk=Cxk+vk=>{xk=f(xk−1,μk)+ωkyk=g(xk)+vk

在线性系统中，服从高斯分布的预测值和测量值经过线性运算后得到的估计值依然服从高斯分布:

而对于非线性系统得到的估计值将不再服从高斯分布,这将导致卡尔曼滤波器可能不收敛。

2.1扩展卡尔曼滤波器EKF

这里引出用于非线性系统的扩展卡尔曼滤波器(Extened Kalman Filter,EKF)，EKF将非线性函数在kkk时刻估算状态的平均值附近进行线性化，

使用EKF时有如下缺点：

1）导数复杂时，雅可比矩阵的解析解不易求得
2）使用数值解时，雅可比计算量较大
3）EKF仅适用于可微系统模型，系统模型不可微分时无法求雅可比矩阵，不能使用EKF
4）对于高度非线性系统，EKF的结果不是最优的
5）对于系统某些不能线性近似的状态，EKF表现较差

2.2 无迹卡尔曼滤波器UKF

由于EKF中使用了非线性函数的线性近似，存在诸多问题，而无迹卡尔曼滤波器(Unscented Kalman Filter，UKF) 通过近似预测量和测量量的概率分布而非近似非线性函数来进行状态估计。无迹卡尔曼滤波器在原概率分布上选择一组最小的采样点，使采样点的均值和方差与原概率分布相同，然后将每个采样点代入非线性函数进行计算得到输出点的均值和方差。根据输出点来计算高斯分布。

2.3 粒子滤波器

粒子滤波器(Particle Filter,PF),粒子滤波器近似任意分布，而不像UKF仅局限于高斯分布，为了估计任意的概率分布，粒子滤波器所需的采样点远远大于UKF。

3.总结

REF

1.https://www.bilibili.com/video/BV1V5411V72J?p=6&spm_id_from=pageDriver
2.https://book.douban.com/subject/27028215/

(一)卡尔曼滤波算法简介相关推荐

卡尔曼滤波算法及c语言代码,卡尔曼滤波算法及C语言代码
卡尔曼滤波算法及C语言代码卡尔曼滤波简介及其算法实现代码卡尔曼滤波算法实现代码(C,C++分别实现) 卡尔曼滤波器简介近来发现有些问题很多人都很感兴趣.所以在这里希望能尽自己能力跟大家讨论一些力 ...
卡尔曼滤波系列（五）——奇异值鲁棒的卡尔曼滤波算法
目录 1 简介 2 原理 3 实验 4 参考文献 1 简介传统的卡尔曼滤波算法假定了噪声服从高斯分布,而实际应用场景中,由于传感器受到各种因素的影响,可能存在部分远偏离预期值的观测结果,称为奇异值. ...
卡尔曼滤波算法——基本原理及举例（python实现radar数据滤波）
卡尔曼滤波算法--基本原理及举例(python实现radar数据滤波) 一.基本原理 1.1 预测 1.2 更新 1.2.1 写法一 1.2.2 写法二二.举例 2.1 数据说明 2.2 代码 2. ...
解读基于多传感器融合的卡尔曼滤波算法
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达本文转自|3D视觉工坊卡尔曼滤波器是传感器融合工程师用于自动驾驶 ...
【转】卡尔曼滤波算法详细推导（相当值得一看）
转载自卡尔曼滤波算法详细推导这一篇对预备知识的介绍还是很好的,过程与原理讲解也很到位,应该是目前看到中文里最好的讲解了. 一.预备知识 1.协方差矩阵是一个维列向量,是的期望 ...
数据结构与算法：算法简介
数据结构与算法:算法简介雪柯大工生物信息提笔为写给奋进之人已关注你说呢 . shenwei356 等 70 人赞同了该文章引用自算法图解,作者[美] Aditya Bhargava 译袁国 ...
hash算法_一致性hash算法简介
一致性hash算法有什么用?我们为什么需要一致性hash算法?这两个问题的答案可以看这篇文章分布式系统路由算法简介. 了解了一致性hash算法出现的背景,我们来看看什么是一致性hash算法.一致性h ...
Minimax 和 Alpha-beta 剪枝算法简介，及以此实现的井字棋游戏（Tic-tac-toe）
前段时间用 React 写了个2048 游戏来练练手,准备用来回顾下 React 相关的各种技术,以及试验一下新技术.在写这个2048的过程中,我考虑是否可以在其中加入一个 AI 算法来自动进行游戏, ...
推荐系统算法_机器学习和推荐系统（二）推荐算法简介
推荐算法简介一. 基于人口统计学的推荐算法二.基于内容的推荐算法三. 基于协同过滤的推荐算法协同过滤(Collaborative Filtering , CF) 基于近邻的系统过滤基于用户( ...

(一)卡尔曼滤波算法简介