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

一.贝叶斯滤波

1.概率学基础

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

2.贝叶斯滤波器

设运动和观测模型：
状态方程：${\mathbf{x}}_k=f({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{\omega }}_k)$

观测方程：${\mathbf{y}}_k=g({\mathbf{x}}_k,{\mathbf{n}}_k)$

由CK方程：
$$p(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})d{x}_{k-1}$$
$$p(x_k|v_{1:k},y_{0:k})=\mu p(y_k|x_k)p(x_k|v_{1:k},y_{0:k})$$
可以看到这种计算开销是非常大的，需要考虑之前所有状态来推导当前状态。
所以，假设系统马尔科夫性,k时刻的状态是由k-1时刻决定：
$$\begin{gathered} 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}) \end{gathered}$$
由此可以得到后验估计：
$$p(x_k|v_k,y_k)=\mu p(y_k|x_k)\int p(x_k|x_{k-1},v_k)p(x_{k-1}|v_{k-1},y_{k-1})d{x}_{k-1}$$
考虑系统的马尔科夫性，就可以从贝叶斯滤波中推导出扩展卡尔曼滤波。
 

二、卡尔曼滤波

1、条件高斯PDF

存在多元正态分布$(x,y)$，则联合高斯密度函数为：
$$p(x,y)=\mathbf{N}(\left[\begin{array}{c}{\mathbf{\mu }}_x\\ \\ {\mathbf{\mu }}_y\end{array}\right],\left[\begin{array}{cc}{\sum }_{xx}& {\sum }_{xy}\\ & \\ {\sum }_{yx}& {\sum }_{yy}\end{array}\right])$$
其中${\sum }_{yx}={\sum }_{xy}^T$。

由贝叶斯法则：
$$\begin{gathered} p(x,y)=p(x|y)\cdot p(y) \\ \frac{p(x,y)}{p(y)}=p(x|y) \end{gathered}$$
分解指数p(x,y)的部分：
$$\begin{array}{rl}& (\left[\begin{array}{c}\mathbf{x}\\ \\ \mathbf{y}\end{array}\right]-\left[\begin{array}{c}{\mathbf{\mu }}_x\\ \\ {\mathbf{\mu }}_y\end{array}\right]{)}^T{\left[\begin{array}{cc}{\sum }_{xx}& {\sum }_{xy}\\ & \\ {\sum }_{yx}& {\sum }_{yy}\end{array}\right]}^{-1}(\left[\begin{array}{c}\mathbf{x}\\ \\ \mathbf{y}\end{array}\right]-\left[\begin{array}{c}{\mathbf{\mu }}_x\\ \\ {\mathbf{\mu }}_y\end{array}\right])\\ & \\ & =(\mathbf{x}-{\mathbf{\mu }}_x-{\sum }_{xy}{\sum }_{yy}^{-1}(\mathbf{y}-{\mathbf{\mu }}_y){)}^T\cdot ({\sum }_{xx}-{\sum }_{xy}{\sum }_{yy}^{-1}{\sum }_{yx}{)}^{-1}\cdot (\mathbf{x}-{\mathbf{\mu }}_x-{\sum }_{xy}{\sum }_{yy}^{-1}(\mathbf{y}-{\mathbf{\mu }}_y))\\ & +(\mathbf{y}-{\mathbf{\mu }}_y{)}^T{\sum }_{yy}^{-1}(\mathbf{y}-{\mathbf{\mu }}_y)\end{array}$$
根据指数分解的结果以及$p(x,y)=p(x|y)\cdot p(y)$可以得到：
$$p(x|y)=\mathbf{N}({\mathbf{\mu }}_x+{\sum }_{xy}{\sum }_{yy}^{-1}(\mathbf{y}-{\mathbf{\mu }}_y),{\sum }_{xx}-{\sum }_{xy}{\sum }_{yy}^{-1}{\sum }_{yx})$$

2.广义卡尔曼步骤：

$$\begin{gathered} K_k={\sum }_{xy}{\sum }_{yy}^{-1} \\ {\hat{\mathbf{P}}}_{\mathbf{k}}={\sum }_{xx}-{\sum }_{xy}{\sum }_{yy}^{-1}{\sum }_{yx} \\ {\hat{\mathbf{x}}}_{\mathbf{k}}={\check{\mathbf{x}}}_{\mathbf{k}}+K_k({\mathbf{y}}_k-{\mathbf{\mu }}_y) \end{gathered}$$

针对线性的卡尔曼滤波

状态方程和观测方程：
状态方程：${\mathbf{x}}_k={\mathbf{A}}_k{\mathbf{x}}_{k-1}+{\mathbf{u}}_k+{\mathbf{\omega }}_k,{\mathbf{\omega }}_k\backsim \mathbf{N}(0,{\mathbf{Q}}_k)$

观测方程：${\mathbf{y}}_k={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{n}}_k,{\mathbf{n}}_k\backsim \mathbf{N}(0,{\mathbf{R}}_k)$
 
则：
$$\begin{gathered} K_k={\check{\mathbf{P}}}_{\mathbf{k}}\cdot {\mathbf{C}}_k^T({\mathbf{C}}_k{\check{\mathbf{P}}}_{\mathbf{k}}{\mathbf{C}}_k^T+{\mathbf{R}}_k{)}^{-1} \\ {\hat{\mathbf{P}}}_{\mathbf{k}}=(1-K_k{\mathbf{C}}_k)\check{{\mathbf{P}}_{\mathbf{k}}} \\ {\hat{\mathbf{x}}}_{\mathbf{k}}={\check{\mathbf{x}}}_{\mathbf{k}}+K_k({\mathbf{y}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_{\mathbf{k}}) \end{gathered}$$

3.扩展卡尔曼滤波EKF

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

状态方程：${\mathbf{x}}_k=f({\mathbf{x}}_{k-1},{\mathbf{v}}_k)+{\mathbf{\omega }}_k,{\mathbf{\omega }}_k\backsim \mathbf{N}(0,{\mathbf{Q}}_k)$

观测方程：${\mathbf{y}}_k=g({\mathbf{x}}_k)+{\mathbf{n}}_k,{\mathbf{n}}_k\backsim \mathbf{N}(0,{\mathbf{R}}_k)$

对运动和观测模型线性化：
$$\begin{array}{rl}f(x_{k-1},v_k)& \approx {\check{x}}_k+{\mathbf{F}}_{\mathbf{k}-1}(x_{k-1}-{\hat{x}}_{k-1})={\check{x}}_k+\frac{\partial f}{\partial {\mathbf{x}}_{k-1}}{|}_{{\hat{\mathbf{x}}}_{\mathbf{k}-1},{\mathbf{v}}_{\mathbf{k}}}\cdot (x_{k-1}-{\hat{x}}_{k-1})\\ g(x_k)& \approx {\check{y}}_k+{\mathbf{G}}_{\mathbf{k}}(x_k-{\check{x}}_k)={\check{y}}_k+\frac{\partial g}{\partial {\mathbf{x}}_k}{|}_{{\check{\mathbf{x}}}_{\mathbf{k}}}(x_k-{\check{x}}_k)\end{array}$$

先验估计：
$$p(x_k|x_{k-1},v_k)\approx N({\check{x}}_k+{\mathbf{F}}_{\mathbf{k}-1}(x_{k-1}-{\hat{x}}_{k-1}),{\mathbf{Q}}_k)$$
似然估计：
$$p(y_k|x_k)\approx N({\check{y}}_k+{\mathbf{G}}_{\mathbf{k}}(x_k-{\check{x}}_k),{\mathbf{R}}_k)$$
代入贝叶斯优化中：

通过高斯融合：
$$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})=N({\check{x}}_k+{\mathbf{K}}_{\mathbf{k}}(y_k-{\check{y}}_k),(1-{\mathbf{K}}_{\mathbf{k}}{\mathbf{G}}_{\mathbf{k}})({\mathbf{F}}_{\mathbf{k}-1}{\mathbf{P}}_{\mathbf{k}-1}{\mathbf{F}}_{\mathbf{k}-1}^{\mathbf{T}}+{\mathbf{Q}}_k))$$
步骤：
$$\begin{array}{rl}{K}_k& ={\check{\mathbf{P}}}_{\mathbf{k}}\cdot {\mathbf{G}}_k^T({\mathbf{G}}_k{\check{\mathbf{P}}}_{\mathbf{k}}{\mathbf{G}}_k^T+{\mathbf{R}}_k{)}^{-1}\\ & \\ {\hat{\mathbf{P}}}_{\mathbf{k}}& =(1-K_k{\mathbf{G}}_k)\check{{\mathbf{P}}_{\mathbf{k}}}\\ & \\ {\hat{\mathbf{x}}}_{\mathbf{k}}& ={\check{\mathbf{x}}}_{\mathbf{k}}+K_k({\mathbf{y}}_k-\mathbf{g}({\check{\mathbf{x}}}_{\mathbf{k}}))\end{array}$$

 

三、粒子滤波

粒子滤波算法

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

2.将每个先验粒子和噪声代入到非线性运动模型中：
$${\check{x}}_{k,m}=f({\hat{x}}_{k-1,m},v_k,{\omega }_{k,m})$$
3. 结合观测值$y_k$对后验概率进行校正：
根据误差或者收敛程度$||g({\hat{x}}_{k-1,m},0)-y_k||$对每个粒子重新赋予权重 ${\omega }_{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})$$
这里可以假设$p(y_k|{\check{x}}_{k,m})=p(y_k|{\check{y}}_{k,m})$，则满足高斯分布的概率密度函数：
$$q(y_k|{\check{x}}_{k,m})=\frac{1}{(2\pi {)}^{\frac{n}{2}}(\det |{\mathbf{n}}_k|{)}^{\frac{1}{2}}}\exp (-\frac{1}{2}(y_k-{\check{y}}_{k,m}{)}^T{\mathbf{n}}_k^{-1}(y_k-{\check{y}}_{k,m}))$$
这里${\check{y}}_{k,m}$越接近$y_k$权值越大。

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