SLAM卡尔曼滤波&&非线性优化

高斯分布（正态分布）是一个常见的连续概率分布。正态分布的数学期望值或期望值$\mu$等于位置参数，决定分布位置；方差${\sigma }^2$ 开平方或标准差$\sigma$ 等于尺度参数，决定分布范围。

随机变量$x$服从高斯分布$N$，其概率密度函数为：
$$p(x)=\frac{1}{\sigma \sqrt{2\pi }}exp(-\frac{(x-\mu {)}^2}{2{\sigma }^2})$$
它的高维形式为：
$$P(x)=\frac{1}{\sqrt{(2\pi {)}^{N}det(\Sigma )}}exp(-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu ))$$

设两个独立的高斯分布：
$$x\sim N({\mu }_x,{\Sigma }_{xx}),y\sim N({\mu }_y,{\Sigma }_{yy})$$
那么他们的和仍然是高斯分布：
$$x+y\sim N({\mu }_x+{\mu }_y,{\Sigma }_{xx}+{\Sigma }_{yy})$$
如果以常数$a$乘以$x$，那么$ax$满足：
$$ax\sim N({\mu }_{a}x,a^2{\Sigma }_{xx})$$
如果取$y=Ax$,那么$y$满足：
$$y\sim N({\mu }_{A}x,A^2{\Sigma }_{xx})$$

设两个高斯分布的乘积满足$p(xy)=N(\mu ,\Sigma )$,那么：
$$\begin{gathered} {\Sigma }^{-1}={\Sigma }_{xx}^{-1}+{\Sigma }_{yy}^{-1} \\ {\Sigma }_{\mu }={\Sigma }_{xx}^{-1}{\mu }_x+{\Sigma }_{yy}^{-1}{\mu }_y \end{gathered}$$

同时考虑到$x$和$y$，当他们不独立时，其复合分布为：
$$p(x,y)=N\left(\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right],\left[\begin{array}{cc}{\Sigma }_{xx}& {\Sigma }_{xy}\\ {\Sigma }_{yx}& {\Sigma }_{yy}\end{array}\right]\right)$$
其中${\Sigma }_{yx}$和${\Sigma }_{yx}$为两个变量的协方差，满足：
$$Cov(X,Y)=E[(X-E(X))(Y-E(Y))]=E[XY]-E[X]E[Y]$$
协方差为正，则说明这两个变量呈正相关，为零则不相关，为负则为负相关。
由条件分布（贝叶斯法则）展开式$p(x,y)=p(x|y)p(y)$可以推出，条件概率$p(x|y)$满足：
$$p(x|y)=N({\mu }_x+{\Sigma }_{xx}{\Sigma }_{yy}^{-1}(y-{\mu }_y),{\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx})$$