一.贝叶斯滤波

1.概率学基础

a.贝叶斯公式
b.归一化积
参考：
机器人学中的概率估计----p11-p34；

2.贝叶斯滤波器

设运动和观测模型：
状态方程：xk=f(xk−1,vk,ωk)\bm x_k=f(\bm x_{k-1},\bm v_k,\bm \omega_k)xk=f(xk−1,vk,ωk)

观测方程：yk=g(xk,nk)\bm y_k=g(\bm x_k,\bm n_k)yk=g(xk,nk)

由CK方程：
p(xk∣v1:k,y0:k−1)=∫p(xk∣xk−1,v1:k,y0:k−1)p(xk−1∣v1:k,y0:k−1)dxk−1p(x_k|v_{1:k},y_{0:k-1})=\int p(x_k|x_{k-1},v_{1:k},y_{0:k-1})p(x_{k-1}|v_{1:k},y_{0:k-1})dx_{k-1} p(xk∣v1:k,y0:k−1)=∫p(xk∣xk−1,v1:k,y0:k−1)p(xk−1∣v1:k,y0:k−1)dxk−1
p(xk∣v1:k,y0:k)=μp(yk∣xk)p(xk∣v1:k,y0:k)p(x_k|v_{1:k},y_{0:k})= \mu \space p(y_k|x_k) p(x_k|v_{1:k},y_{0:k})\\ p(xk∣v1:k,y0:k)=μ p(yk∣xk)p(xk∣v1:k,y0:k)
可以看到这种计算开销是非常大的，需要考虑之前所有状态来推导当前状态。
所以，假设系统马尔科夫性,k时刻的状态是由k-1时刻决定：
p(xk∣xk−1,v1:k,y0:k−1)=p(xk∣xk−1,vk)p(xk−1∣v1:k,y0:k−1)=p(xk−1∣vk−1,yk−1)p(x_k|x_{k-1},v_{1:k},y_{0:k-1}) = p(x_k|x_{k-1},v_k)\\ p(x_{k-1}|v_{1:k},y_{0:k-1}) = p(x_{k-1}|v_{k-1},y_{k-1}) p(xk∣xk−1,v1:k,y0:k−1)=p(xk∣xk−1,vk)p(xk−1∣v1:k,y0:k−1)=p(xk−1∣vk−1,yk−1)
由此可以得到后验估计：
p(xk∣vk,yk)=μp(yk∣xk)∫p(xk∣xk−1,vk)p(xk−1∣vk−1,yk−1)dxk−1p(x_k|v_k,y_k)= \mu \space p(y_k|x_k)\int p(x_k|x_{k-1},v_k)p(x_{k-1}|v_{k-1},y_{k-1})dx_{k-1} p(xk∣vk,yk)=μ p(yk∣xk)∫p(xk∣xk−1,vk)p(xk−1∣vk−1,yk−1)dxk−1
考虑系统的马尔科夫性，就可以从贝叶斯滤波中推导出扩展卡尔曼滤波。

二、卡尔曼滤波

1、条件高斯PDF

存在多元正态分布(x,y)(x,y)(x,y)，则联合高斯密度函数为：
p(x,y)=N([μxμy],[∑xx∑xy∑yx∑yy])p(x,y)=\bm N(\begin{bmatrix} \bm \mu _x\\ \\\bm \mu _y \end{bmatrix},\begin{bmatrix} \bm {\sum} _{xx}& \bm {\sum} _{xy}\\ \\\bm {\sum} _{yx} & \bm {\sum} _{yy} \end{bmatrix}) p(x,y)=N(⎣⎡μxμy⎦⎤,⎣⎡∑xx∑yx∑xy∑yy⎦⎤)
其中∑yx=∑xyT\bm {\sum} _{yx} =\bm {\sum} _{xy}^T∑yx=∑xyT。

由贝叶斯法则：
p(x,y)=p(x∣y)⋅p(y)p(x,y)p(y)=p(x∣y)p(x,y)=p(x|y)\cdot p(y)\\ \frac {p(x,y)} {p(y)} = {p(x|y)} p(x,y)=p(x∣y)⋅p(y)p(y)p(x,y)=p(x∣y)
分解指数p(x,y)的部分：
([xy]−[μxμy])T[∑xx∑xy∑yx∑yy]−1([xy]−[μxμy])=(x−μx−∑xy∑yy−1(y−μy))T⋅(∑xx−∑xy∑yy−1∑yx)−1⋅(x−μx−∑xy∑yy−1(y−μy))+(y−μy)T∑yy−1(y−μy)\small \begin{aligned} &(\begin{bmatrix} \bm x\\ \\\bm y \end{bmatrix}-\begin{bmatrix} \bm \mu _x\\ \\\bm \mu _y \end{bmatrix})^T\begin{bmatrix} \bm {\sum} _{xx}& \bm {\sum} _{xy}\\ \\\bm {\sum} _{yx} & \bm {\sum} _{yy} \end{bmatrix}^{-1}(\begin{bmatrix} \bm x\\ \\\bm y \end{bmatrix}-\begin{bmatrix} \bm \mu _x\\ \\\bm \mu _y \end{bmatrix})\\ ~\\&=(\bm x-\bm \mu _x-\bm {\sum} _{xy}\bm {\sum} _{yy}^{-1}(\bm y-\bm \mu_y))^T \cdot ( \bm {\sum} _{xx}- \bm {\sum} _{xy} \bm {\sum} _{yy}^{-1} \bm {\sum} _{yx})^{-1}\cdot (\bm x-\bm \mu _x-\bm {\sum} _{xy}\bm {\sum} _{yy}^{-1}(\bm y-\bm \mu_y)) \\ &+(\bm y -\bm \mu _y)^T \bm {\sum} _{yy}^{-1} (\bm y -\bm \mu _y) \end{aligned} ([xy]−[μxμy])T⎣⎡∑xx∑yx∑xy∑yy⎦⎤−1([xy]−[μxμy])=(x−μx−∑xy∑yy−1(y−μy))T⋅(∑xx−∑xy∑yy−1∑yx)−1⋅(x−μx−∑xy∑yy−1(y−μy))+(y−μy)T∑yy−1(y−μy)
根据指数分解的结果以及p(x,y)=p(x∣y)⋅p(y)p(x,y)=p(x|y)\cdot p(y)p(x,y)=p(x∣y)⋅p(y)可以得到：
p(x∣y)=N(μx+∑xy∑yy−1(y−μy),∑xx−∑xy∑yy−1∑yx)p(x|y)=\bm N(\bm \mu _x+ \bm {\tiny \sum} _{xy}\bm {\tiny \sum} _{yy}^{-1}(\bm y-\bm \mu_y), \bm {\tiny \sum} _{xx}- \bm {\tiny \sum} _{xy} \bm {\tiny \sum} _{yy}^{-1} \bm {\tiny \sum} _{yx}) p(x∣y)=N(μx+∑xy∑yy−1(y−μy),∑xx−∑xy∑yy−1∑yx)

2.广义卡尔曼步骤：

Kk=∑xy∑yy−1P^k=∑xx−∑xy∑yy−1∑yxx^k=xˇk+Kk(yk−μy)K_k=\bm {\tiny \sum} _{xy}\bm {\tiny \sum} _{yy}^{-1} \\ ~\\ \bm {\hat{P}_k}=\bm {\tiny \sum} _{xx}- \bm {\tiny \sum} _{xy} \bm {\tiny \sum} _{yy}^{-1} \bm {\tiny \sum} _{yx}\\ ~\\ \bm {\hat{x}_k}= \bm {\check{x}_k}+K_k(\bm y_k-\bm \mu_y) Kk=∑xy∑yy−1 P^k=∑xx−∑xy∑yy−1∑yx x^k=xˇk+Kk(yk−μy)

针对线性的卡尔曼滤波

状态方程和观测方程：
状态方程：xk=Akxk−1+uk+ωk,ωk∽N(0,Qk)\bm x_k=\bm A_k\bm x_{k-1}+\bm u_k+\bm \omega_k,\bm \omega_k\backsim \bm N(0,\bm Q_k)xk=Akxk−1+uk+ωk,ωk∽N(0,Qk)

观测方程：yk=Ckxk+nk,nk∽N(0,Rk)\bm y_k=\bm C_k\bm x_k+\bm n_k,\bm n_k\backsim \bm N(0,\bm R_k)yk=Ckxk+nk,nk∽N(0,Rk)

则：
Kk=Pˇk⋅CkT(CkPˇkCkT+Rk)−1P^k=(1−KkCk)Pkˇx^k=xˇk+Kk(yk−Ckxˇk)K_k=\bm {\check{P}_k}\cdot \bm C_k^T(\bm C_k\bm{\check{P}_k}\bm C_k^T+\bm R_k)^{-1}\\ ~\\ \bm {\hat{P}_k}=(1-K_k\bm C_k)\bm {\check{P_k}}\\ ~\\ \bm {\hat{x}_k}= \bm {\check{x}_k}+K_k(\bm y_k-\bm C_k\bm {\check{x}_k}) Kk=Pˇk⋅CkT(CkPˇkCkT+Rk)−1 P^k=(1−KkCk)Pkˇ x^k=xˇk+Kk(yk−Ckxˇk)

3.扩展卡尔曼滤波EKF

存在非线性的状态方程和观测方程：

状态方程：xk=f(xk−1,vk)+ωk,ωk∽N(0,Qk)\bm x_k=f(\bm x_{k-1},\bm v_k)+\bm \omega_k,\bm \omega_k\backsim \bm N(0,\bm Q_k)xk=f(xk−1,vk)+ωk,ωk∽N(0,Qk)

观测方程：yk=g(xk)+nk,nk∽N(0,Rk)\bm y_k=g(\bm x_k)+\bm n_k,\bm n_k\backsim \bm N(0,\bm R_k)yk=g(xk)+nk,nk∽N(0,Rk)

对运动和观测模型线性化：
f(xk−1,vk)≈xˇk+Fk−1(xk−1−x^k−1)=xˇk+∂f∂xk−1∣x^k−1,vk⋅(xk−1−x^k−1)g(xk)≈yˇk+Gk(xk−xˇk)=yˇk+∂g∂xk∣xˇk(xk−xˇk)\begin{aligned} f(x_{k-1 },v_k)&\approx \check{x}_k+\bm{F_{k-1}}(x_{k-1}-\hat{x}_{k-1})=\check{x}_k+\frac {\partial f } {\partial \bm x_{k-1}}\Bigg|_{\bm{\hat{x}_{k-1},v_k}}\cdot(x_{k-1}-\hat{x}_{k-1})\\ g(x_k)&\approx\check{y}_k+\bm {G_k}(x_k-\check x_k)=\check{y}_k+\frac {\partial g } {\partial \bm x_k} \Bigg|_{\bm{\check{x}_{k}}}(x_k-\check x_k) \end{aligned} f(xk−1,vk)g(xk)≈xˇk+Fk−1(xk−1−x^k−1)=xˇk+∂xk−1∂f∣∣∣∣∣x^k−1,vk⋅(xk−1−x^k−1)≈yˇk+Gk(xk−xˇk)=yˇk+∂xk∂g∣∣∣∣∣xˇk(xk−xˇk)

先验估计：
p(xk∣xk−1,vk)≈N(xˇk+Fk−1(xk−1−x^k−1),Qk)p(x_k|x_{k-1},v_k) \approx N(\check x_k+\bm{F_{k-1}}(x_{k-1}-\hat{x}_{k-1}),\bm Q_k) p(xk∣xk−1,vk)≈N(xˇk+Fk−1(xk−1−x^k−1),Qk)
似然估计：
p(yk∣xk)≈N(yˇk+Gk(xk−xˇk),Rk)p(y_k|x_k)\approx N(\check y_k+\bm{G_k}(x_k-\check x_k),\bm R_k) p(yk∣xk)≈N(yˇk+Gk(xk−xˇk),Rk)
代入贝叶斯优化中：

通过高斯融合：
p(xk∣xˇ0,v1:k,y0:k)=N(xˇk+Kk(yk−yˇk),(1−KkGk)(Fk−1Pk−1Fk−1T+Qk))p(x_k|\check x_0,v_{1:k},y_{0:k})=N(\check x_k+\bm {K_k}(y_k-\check y_k),(1-\bm {K_kG_k})(\bm {F_{k-1}}\bm {P_{k-1}}\bm {F^T_{k-1}}+\bm Q_k) ) p(xk∣xˇ0,v1:k,y0:k)=N(xˇk+Kk(yk−yˇk),(1−KkGk)(Fk−1Pk−1Fk−1T+Qk))
步骤：
Kk=Pˇk⋅GkT(GkPˇkGkT+Rk)−1P^k=(1−KkGk)Pkˇx^k=xˇk+Kk(yk−g(xˇk))\begin{aligned} K_k&=\bm {\check{P}_k}\cdot \bm G_k^T(\bm G_k\bm{\check{P}_k}\bm G_k^T+\bm R_k)^{-1}\\ ~\\ \bm {\hat{P}_k}&=(1-K_k\bm G_k)\bm {\check{P_k}}\\ ~\\ \bm {\hat{x}_k}&= \bm {\check{x}_k}+K_k(\bm y_k-\bm g(\bm {\check{x}_k})) \end{aligned} Kk P^k x^k=Pˇk⋅GkT(GkPˇkGkT+Rk)−1=(1−KkGk)Pkˇ=xˇk+Kk(yk−g(xˇk))

三、粒子滤波

粒子滤波算法

1.先从先验和运动噪声的联合密度函数中抽取M个样本：
[x^k−1,mωk,m][ \hat x_{k-1,m}\quad \omega_{k,m} ] [x^k−1,mωk,m]

2.将每个先验粒子和噪声代入到非线性运动模型中：
xˇk,m=f(x^k−1,m,vk,ωk,m)\check x_{k,m}=f(\hat x_{k-1,m},v_k,\omega_{k,m}) xˇk,m=f(x^k−1,m,vk,ωk,m)
3. 结合观测值yky_kyk对后验概率进行校正：
根据误差或者收敛程度∣∣g(x^k−1,m,0)−yk∣∣||g(\hat x_{k-1,m},0)-y_k||∣∣g(x^k−1,m,0)−yk∣∣对每个粒子重新赋予权重 ωk,m\omega_{k,m}ωk,m:
ωk,m=p(xˇk,m∣vk,yk)p(xˇk,m∣vk,yk−1)=μp(yk∣xˇk,m)\omega_{k,m} = \frac {p(\check x_{k,m}|v_k,y_k)} {p(\check x_{k,m}|v_k,y_{k-1})}=\mu p(y_k|\check x_{k,m}) ωk,m=p(xˇk,m∣vk,yk−1)p(xˇk,m∣vk,yk)=μp(yk∣xˇk,m)
这里可以假设p(yk∣xˇk,m)=p(yk∣yˇk,m)p(y_k|\check x_{k,m}) = p(y_k|\check y_{k,m})p(yk∣xˇk,m)=p(yk∣yˇk,m)，则满足高斯分布的概率密度函数：
q(yk∣xˇk,m)=1(2π)n2(det⁡∣nk∣)12exp⁡(−12(yk−yˇk,m)Tnk−1(yk−yˇk,m))q(y_k|\check x_{k,m})=\frac {1} {(2\pi)^{\frac n 2}\bm ({\det} |\bm n_k|)^{\frac 1 2}}\exp(-\frac 1 2(y_k-\check y_{k,m})^T{\bm n_k^{-1}(y_k-\check y_{k,m}) }) q(yk∣xˇk,m)=(2π)2n(det∣nk∣)211exp(−21(yk−yˇk,m)Tnk−1(yk−yˇk,m))
这里yˇk,m\check y_{k,m}yˇk,m越接近yky_{k}yk权值越大。

4.重要性重采样
对权重大的粒子保留，重新生成粒子群。
参考：
https://blog.csdn.net/qq_29796781/article/details/80259339?utm_source=app&app_version=5.0.1&code=app_1562916241&uLinkId=usr1mkqgl919blen
5.期望估计：
首先对权值进行归一化：
ωˉk,m=ωk,m∑i=1mωk,i\bar \omega_{k,m}=\frac {\omega_{k,m}} { \sum\limits_{i=1}^m \omega_{k,i}} ωˉk,m=i=1∑mωk,iωk,m
则估计值为：
x^k=∑i=1mωˉk,m⋅xˇk,m\hat x_k=\sum_{i=1}^m \bar \omega_{k,m}\cdot \check x_{k,m} x^k=i=1∑mωˉk,m⋅xˇk,m
return 2；

SLAM--卡尔曼滤波、粒子滤波相关推荐

卡尔曼滤波/粒子滤波融合定位模拟器
最近稍闲,稍微整理了一下以前的部分代码,虽然写得不够好,但是对于新手也许也有一定的分享价值.具体算法细节我就暂时不讲了,网上太多了. 所以分享这个我用JAVA写的一个模拟器.模拟定位和惯导(或者pdr ...
机器人学习--网友资料系列激光SLAM建图、粒子滤波定位和位姿图优化
一.移动机器人自主导航的前提是在未知环境中先构建地图 (目前市内很多用的2D激光雷达,构建栅格地图,相当于立体空间中的某个水平面高度的切面) 一般用的是2D 激光SLAM算法构建概率栅格占用地图: ...
粒子滤波在图像跟踪领域的实践
文章目录前言粒子滤波与卡尔曼滤波的介绍卡尔曼滤波粒子滤波粒子滤波与卡尔曼滤波跟踪对比实验小结跋前言原文首发于微信公众号「3D视觉工坊」. 前两天在GitChat上总结了自己当 ...
【SLAM基础入门】贝叶斯滤波、卡尔曼滤波、粒子滤波笔记（1）
贝叶斯滤波.卡尔曼滤波.粒子滤波 (https://www.bilibili.com/video/BV1HT4y1577g?spm_id_from=333.999.header_right.histo ...
【SLAM基础入门】贝叶斯滤波、卡尔曼滤波、粒子滤波笔记（2）
基于B站老王的贝叶斯滤波.卡尔曼滤波.粒子滤波 Bilibili 文章目录第三部分:随机过程的贝叶斯滤波BF 第四部分:卡尔曼滤波KF 第三部分:随机过程的贝叶斯滤波BF 随机过程包含一系列随机变量 ...
rbpf粒子滤波slam matlab程序_学习笔记（优达学城）- 车辆定位之粒子滤波器（整合版）...
1.代码传送门首先,一如既往的,打开传送门! Fred159/CarND-Kidnapped-Vehicle-Projectgithub.com 代码, 很重要,但更重要的是从代码的行与行之间探索 ...
机器人学习--粒子滤波SLAM/MCL定位参考资料+学习经验
学习材料1: <概率机器人学> 谷歌无人驾驶之父 Sebastian Thrun等人著作. 注释: 可能是本人智商有限,或者是移动机器人学领域的基础知识了解不多. 刚刚看这本书的时候,尤其 ...
湖南工程学院计算机与通讯学院肖鹏,基于区间分析无迹粒子滤波的移动机器人SLAM方法...
[摘要] In particle filter simultaneous localization and mapping method ( SLAM ) for mobile robots, lin ...
卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波原理
所有滤波问题其实都是求感兴趣的状态的后验概率分布,只是由于针对特定条件的不同,可通过求解递推贝叶斯公式获得后验概率的解析解(KF.EKF.UKF),也可通过大数统计平均求期望的方法来获得后验概率(PF ...
对无迹卡尔曼滤波(UKF)和粒子滤波(PF)的理解
对无迹卡尔曼滤波(UKF)的总结: EKF通过在工作点附近进行一阶泰勒展开来线性化非线性的运动方程和观测方程.这样的线性近似在高度非线性的情况下存在较大误差.而UKF换了一个线性化的思路.1)选取2n ...

SLAM--卡尔曼滤波、粒子滤波

目录