多传感器融合定位九-基于滤波的融合方法Ⅰ其一

Reference:

1. 深蓝学院-多传感器融合
2. 多传感器融合定位理论基础

文章跳转：

1. 多传感器融合定位一-3D激光里程计其一：ICP
2. 多传感器融合定位二-3D激光里程计其二：NDT
3. 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
4. 多传感器融合定位四-3D激光里程计其四：点云线面特征提取
5. 多传感器融合定位五-点云地图构建及定位
6. 多传感器融合定位六-惯性导航原理及误差分析
7. 多传感器融合定位七-惯性导航解算及误差分析其一
8. 多传感器融合定位八-惯性导航解算及误差分析其二
9. 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
10. 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
11. 多传感器融合定位十一-基于滤波的融合方法Ⅱ
12. 多传感器融合定位十二-基于图优化的建图方法其一
13. 多传感器融合定位十三-基于图优化的建图方法其二
14. 多传感器融合定位十四-基于图优化的定位方法
15. 多传感器融合定位十五-多传感器时空标定(综述)

1. 滤波器的作用

滤波器的本质：结合预测与观测，得到最“精确”的后验值。
实际中，预测与观测均从传感器而来，因此滤波器的作用便是结合各传感器得到一个最好的融合结果。

1. 实际中预测往往从 IMU、编码器等传感器递推而来；
2. 观测往往从 GPS、雷达、相机等传感器而来；
3. 后验为融合后的结果，即定位模块的输出。

2. 概率基础知识

2.1 概率、概率密度

上图中，$p(x)$ 为 $x$ 在区间 $[a$, $b]$ 上的概率密度，它表示的是随机变量在区间的分布情况。
$\mathrm{Pr}$ 代表的是 $x$ 在区间 $[c,d]$ 上的概率，它是概率密度的积分，
$$\mathrm{Pr}(c\le x\le d)={\int }_c^{d}p(x)dx$$我们平时所说 “高斯分布”、“非高斯分布” 均是指它的概率密度。

2.2 联合概率密度

$x\in [a,b]$ 和 $y\in [r,s]$ 的联合概率密度函数可以表示为 $p(x,y)$，其积分表示 $x$ 和 $y$ 同时处在某个区间的概率，满足下式：
$${\int }_a^b{\int }_r^{s}p(x,y)dydx=1$$特别地，当 $x$ 和 $y$ 统计独立的时候，有：$$p(x,y)=p(x)p(y)$$

2.3 条件概率密度

$x$ 关于 $y$ 的条件概率密度函数可以表示为：
$$p(x\mid y)$$其含义是，在 $y\in [r,s]$ 的前提下，$x\in [a,b]$ 的概率分布，并且满足下式：
$$p(x)={\int }_r^{s}p(x\mid y)p(y)dy$$特别地，当 $x$ 和 $y$ 统计独立的时候，有：
$$p(x\mid y)=p(x)$$

2.4 贝叶斯公式

联合概率密度分解成条件概率密度($p(x\mid y)$)和边缘概率密度($p(y)$)的乘积(左边：$x,y$同时满足这个条件；右边：在 $y$ 满足条件的情况下，$x$ 也满足这个条件。这两者是等价的。)，即：
$$p(x,y)=p(x\mid y)p(y)=p(y\mid x)p(x)$$重新整理，即可得贝叶斯公式：
$$p(x\mid y)=\frac{p(y\mid x)p(x)}{p(y)}$$

2.5 贝叶斯推断

贝叶斯推断可以理解为贝叶斯公式的运用，它是指，如果已知先验概率密度函数 $p(x)$，以及传感器模型 $p(y\mid x)$，那么就可以根据贝叶斯公式推断出后验概率密度。
$$p(x\mid y)=\frac{p(y\mid x)p(x)}{\int p(y\mid x)p(x)\mathrm{d}x}$$实际中，贝叶斯推断有时也称为贝叶斯估计。

2.6 高斯概率密度函数

一维情况下，高斯概率密度函数表示为：
$$p\left(x\mid \mu ,{\sigma }^2\right)=\frac{1}{\sqrt{2\pi {\sigma }^2}}\exp \left(-\frac{1}{2}\frac{(x-\mu {)}^2}{{\sigma }^2}\right)$$其中 $\mu$ 为均值，${\sigma }^2$ 为方差。
多维情况下，高斯概率密度函数表示为：
$$p(\mathbf{x}\mid \mathbf{\mu },\mathbf{\Sigma })=\frac{1}{\sqrt{(2\pi {)}^N\det \mathbf{\Sigma }}}\exp \left(-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })\right)$$其中均值为 $\mathbf{\mu }$，方差为 $\mathbf{\Sigma }$。
一般把高斯分布写成 $\mathbf{x}\sim \mathcal{N}(\mathbf{\mu },\mathbf{\Sigma })$。

2.7 联合高斯概率密度函数

若有高斯分布：
$$\begin{array}{rl}& p(\mathbf{x})=\mathcal{N}\left({\mathbf{\mu }}_x,{\mathbf{\Sigma }}_{xx}\right)\\ & p(\mathbf{y})=\mathcal{N}\left({\mathbf{\mu }}_y,{\mathbf{\Sigma }}_{yy}\right)\end{array}$$则它们的联合概率密度函数可以表示为：
$$p(\mathbf{x},\mathbf{y})=\mathcal{N}\left(\left[\begin{array}{l}{\mathbf{\mu }}_x\\ {\mathbf{\mu }}_y\end{array}\right],\left[\begin{array}{ll}{\mathbf{\Sigma }}_{xx}& {\mathbf{\Sigma }}_{xy}\\ {\mathbf{\Sigma }}_{yx}& {\mathbf{\Sigma }}_{yy}\end{array}\right]\right)$$由于联合概率密度满足下式：
$$p(\mathbf{x},\mathbf{y})=p(\mathbf{x}\mid \mathbf{y})p(\mathbf{y})$$该式在高斯分布的前提下可以重新分解。
由于高斯分布中指数项包含方差的求逆(即 $\Sigma$ 求逆，这个是不太好展开的)，而此处联合概率的方差是一个高维矩阵，对它求逆的简洁办法是运用舒尔补(对于求逆来说，是个非常有利的工具)。

舒尔补的主要目的是把矩阵分解成上三角矩阵、对角阵、 下三角矩阵乘积的形式，方便运算，即(舒尔补 的具体含义：从下面一个矩阵拆成三个)：

$$\begin{array}{rl}& \left[\begin{array}{ll}\mathbf{A}& \mathbf{B}\\ \mathbf{C}& \mathbf{D}\end{array}\right]\\ =& \left[\begin{array}{cc}\mathbf{I}& {\mathbf{BD}}^{-1}\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{cc}\Delta \mathbf{D}& 0\\ 0& \mathbf{D}\end{array}\right]\left[\begin{array}{cc}\mathbf{I}& 0\\ {\mathbf{D}}^{-1}\mathbf{C}& \mathbf{I}\end{array}\right]\end{array}$$其中 $\Delta \mathrm{D}=\mathbf{A}-{\mathbf{BD}}^{-1}\mathbf{C}$ 称为矩阵 $\mathrm{D}$ 关于原矩阵的舒尔补。
此时有(好处就在于下面求逆的时候，公式中 对角阵的逆、上三角及下三角的逆 都是比较好求的，这样上面方差矩阵的逆就更好求出来了)：
$$\begin{array}{rl}& {\left[\begin{array}{ll}\mathbf{A}& \mathbf{B}\\ \mathbf{C}& \mathbf{D}\end{array}\right]}^{-1}=\\ & \left[\begin{array}{cc}\mathrm{I}& 0\\ -{\mathbf{D}}^{-1}\mathbf{C}& \mathrm{I}\end{array}\right]\left[\begin{array}{cc}\Delta {\mathbf{D}}^{-1}& 0\\ 0& {\mathbf{D}}^{-1}\end{array}\right]\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{BD}}^{-1}\\ 0& \mathbf{I}\end{array}\right]\end{array}$$利用舒尔补，联合分布的方差矩阵可以写为：
$$\left[\begin{array}{cc}{\mathbf{\Sigma }}_{xx}& {\mathbf{\Sigma }}_{xy}\\ {\mathbf{\Sigma }}_{yx}& {\mathbf{\Sigma }}_{yy}\end{array}\right]=\left[\begin{array}{cc}1& {\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{\mathbf{\Sigma }}_{xx}-{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}{\mathbf{\Sigma }}_{yx}& 0\\ 0& {\mathbf{\Sigma }}_{yy}\end{array}\right]\left[\begin{array}{cc}1& 0\\ {\mathbf{\Sigma }}_{yy}^{-1}{\mathbf{\Sigma }}_{yx}& 1\end{array}\right]$$它的逆矩阵为：
$${\left[\begin{array}{cc}{\mathbf{\Sigma }}_{xx}& {\mathbf{\Sigma }}_{xy}\\ {\mathbf{\Sigma }}_{yx}& {\mathbf{\Sigma }}_{yy}\end{array}\right]}^{-1}=\left[\begin{array}{cc}1& 0\\ -{\mathbf{\Sigma }}_{yy}^{-1}{\mathbf{\Sigma }}_{yx}& 1\end{array}\right]\left[\begin{array}{cc}{\left({\mathbf{\Sigma }}_{xx}-{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}{\mathbf{\Sigma }}_{yx}\right)}^{-1}& 0\\ 0& {\mathbf{\Sigma }}_{yy}^{-1}\end{array}\right]\left[\begin{array}{cc}1& -{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}\\ 0& 1\end{array}\right]$$联合分布 $p(\mathbf{x},\mathbf{y})$ 仍为高斯分布，
$$p(\mathbf{x},\mathbf{y})=\mathcal{N}\left(\left[\begin{array}{l}{\mathbf{\mu }}_x\\ {\mathbf{\mu }}_y\end{array}\right],\left[\begin{array}{cc}{\mathbf{\Sigma }}_{xx}& {\mathbf{\Sigma }}_{xy}\\ {\mathbf{\Sigma }}_{yx}& {\mathbf{\Sigma }}_{yy}\end{array}\right]\right)$$它的指数部分的二次项包含如下内容(注意说的是指数部分)
$$\begin{array}{rl}& {\left(\left[\begin{array}{l}\mathbf{x}\\ \mathbf{y}\end{array}\right]-\left[\begin{array}{l}{\mathbf{\mu }}_x\\ {\mathbf{\mu }}_y\end{array}\right]\right)}^{\mathrm{T}}{\left[\begin{array}{cc}{\mathbf{\Sigma }}_{xx}& {\mathbf{\Sigma }}_{xy}\\ {\mathbf{\Sigma }}_{yx}& {\mathbf{\Sigma }}_{yy}\end{array}\right]}^{-1}\left(\left[\begin{array}{l}\mathbf{x}\\ \mathbf{y}\end{array}\right]-\left[\begin{array}{l}{\mathbf{\mu }}_x\\ {\mathbf{\mu }}_y\end{array}\right]\right)\\ =& {\left(\left[\begin{array}{l}\mathbf{x}\\ \mathbf{y}\end{array}\right]-\left[\begin{array}{l}{\mathbf{\mu }}_x\\ {\mathbf{\mu }}_y\end{array}\right]\right)}^{\mathrm{T}}\left[\begin{array}{cc}1& 0\\ -{\mathbf{\Sigma }}_{yy}^{-1}{\mathbf{\Sigma }}_{yx}& 1\end{array}\right]\left[\begin{array}{cc}{\left({\mathbf{\Sigma }}_{xx}-{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}{\mathbf{\Sigma }}_{yx}\right)}^{-1}& 0\\ 0& {\mathbf{\Sigma }}_{yy}^{-1}\end{array}\right]\left[\begin{array}{cc}1& -{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}\\ 0& 1\end{array}\right]\left(\left[\begin{array}{l}\mathbf{x}\\ \mathbf{y}\end{array}\right]-\left[\begin{array}{l}{\mathbf{\mu }}_x\\ {\mathbf{\mu }}_y\end{array}\right]\right)\\ =& {\left(\mathbf{x}-{\mathbf{\mu }}_x-{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}\left(\mathbf{y}-{\mathbf{\mu }}_y\right)\right)}^{\mathrm{T}}{\left({\mathbf{\Sigma }}_{xx}-{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}{\mathbf{\Sigma }}_{yx}\right)}^{-1}\left(\mathbf{x}-{\mathbf{\mu }}_x-{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}\left(\mathbf{y}-{\mathbf{\mu }}_y\right)\right)\\ & +{\left(\mathbf{y}-{\mathbf{\mu }}_y\right)}^{\mathrm{T}}{\mathbf{\Sigma }}_{yy}^{-1}\left(\mathbf{y}-{\mathbf{\mu }}_y\right)\end{array}$$最后得到两个二次项的和，由于同底数幂相乘后，底数不变，指数相加，且 $p(\mathbf{y})=\mathcal{N}\left({\mathbf{\mu }}_y,{\mathbf{\Sigma }}_{yy}\right)$(这时就可以求 $p(\mathbf{x}\mid \mathbf{y})=\frac{p(\mathbf{x},\mathbf{y})}{p(y)}$，正好消去指数项 ${\left(\mathbf{y}-{\mathbf{\mu }}_y\right)}^{\mathrm{T}}{\mathbf{\Sigma }}_{yy}^{-1}\left(\mathbf{y}-{\mathbf{\mu }}_y\right)$)
因此有 $p(\mathbf{x}\mid \mathbf{y})=\mathcal{N}\left({\mathbf{\mu }}_x+{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}\left(\mathbf{y}-{\mathbf{\mu }}_y\right),{\mathbf{\Sigma }}_{xx}-{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}{\mathbf{\Sigma }}_{yx}\right)$

2.8 高斯随机变量的线性分布

在上面的例子中，若已知 $\mathbf{x}$ 和 $\mathbf{y}$ 之间有如下关系
$$\mathbf{y}=\mathbf{G}\mathbf{x}+\mathbf{n}$$其中 $G$ 是一个常量矩阵， $n=\mathcal{N}(0,R)$ 为零均值白噪声， 在实际中指的是观测噪声。则 $x$ 和 $y$ 的均值和方差之间必然存在联系，其联系可通过以下推导获得。
均值($E[\mathbf{n}]$ 为零均值白噪声，所以 $E[\mathbf{n}]=0$)：
$$\begin{array}{rl}{\mathbf{\mu }}_y& =E[\mathbf{y}]\\ & =E[\mathbf{G}\mathbf{x}+\mathbf{n}]\\ & =\mathbf{G}E[\mathbf{x}]+E[\mathbf{n}]\\ & =\mathbf{G}{\mathbf{\mu }}_x\end{array}$$方差：
$$\begin{array}{rl}{\mathbf{\Sigma }}_{yy}& =\mathbf{\Sigma }(\mathbf{G}\mathbf{x})+\mathbf{\Sigma }(\mathbf{n})\\ & =E\left[\left(\mathbf{G}\mathbf{x}-{\mathbf{\mu }}_y\right){\left(\mathbf{G}\mathbf{x}-{\mathbf{\mu }}_y\right)}^{\mathrm{T}}\right]+\mathbf{R}\\ & =\mathbf{G}E\left[\left(\mathbf{x}-{\mathbf{\mu }}_x\right){\left(\mathbf{x}-{\mathbf{\mu }}_x\right)}^{\mathrm{T}}\right]{\mathbf{G}}^{\mathrm{T}}+\mathbf{R}\\ & =\mathbf{G}{\mathbf{\Sigma }}_{xx}{\mathbf{G}}^{\mathrm{T}}+\mathbf{R}\end{array}$$方差的交叉项：
$$\begin{array}{rl}{\mathbf{\Sigma }}_{xy}& =E\left[\left(\mathbf{x}-{\mathbf{\mu }}_x\right){\left(\mathbf{y}-{\mathbf{\mu }}_y\right)}^{\mathrm{T}}\right]\\ & =E\left[\left(\mathbf{x}-{\mathbf{\mu }}_x\right){\left(\mathbf{G}\mathbf{x}-\mathbf{G}{\mathbf{\mu }}_x+\mathbf{n}\right)}^{\mathrm{T}}\right]\\ & =E\left[\left(\mathbf{x}-{\mathbf{\mu }}_x\right){\left(\mathbf{G}\mathbf{x}-\mathbf{G}{\mathbf{\mu }}_x\right)}^{\mathrm{T}}+\left(\mathbf{x}-{\mathbf{\mu }}_x\right){\mathbf{n}}^{\mathrm{T}}\right]\\ & ={\mathbf{\Sigma }}_{xx}{\mathbf{G}}^{\mathrm{T}}+E\left[\left(\mathbf{x}-{\mathbf{\mu }}_x\right){\mathbf{n}}^{\mathrm{T}}\right]\\ & ={\mathbf{\Sigma }}_{xx}{\mathbf{G}}^{\mathrm{T}}\end{array}$$同理可得 ${\mathbf{\Sigma }}_{yx}={\mathbf{\Sigma }}_{xy}^{\mathrm{T}}=\mathbf{G}{\mathbf{\Sigma }}_{xx}$(互为转置)