图结构SLAM的非线性优化

（一）SLAM概率问题定义

SLAM问题其实就是一个状态估计的问题，就是要根据一系列观测来推测状态量；一般情况下，我们把SLAM问题建立在概率论的框架下。

说白了，SLAM就是要是解决这样一类后验概率问题：

$$p(x|z)$$

其中$x$是系统当前状态量，$z$是与状态量相关的观测量；在求出这个条件概率之后，把观测值$Z$带入，就可以获得$p(x|z=Z)$，这也就是我们需要的分布。

1.1 概率分解

根据概率公式展开：

$$p(x|z=Z)=\frac{p(z=Z|x)p(x)}{p(z=Z)}$$

1.2 归一化因子

其中分母是常量，当作是归一化因子$\eta$即可，所以也可写为：

$$p(x|z=Z)=\eta p(z=Z|x)p(x)$$

1.3 似然

$p(z|x)$表示在$x$已知的情况下，$z$的概率分布；在求出$p(z|x)$之后，把$z=Z$带入，得到$p(z=Z|x)$，它也被称为似然项，它表示在$x$取不同值的情况下，$z=Z$的概率。

1.4 先验

$p(x)$也被称为先验，也就是在观测之前$x$服从什么分布。这可以通过其他信息源得到，比如GPS，惯导等。如果事前不知道任何先验分布信息，那么可以将其设为1，表示不相信先验信息，只根据系统使用的量测来估计。

1.5 最小二乘

$$x_{MAP}=\underset{x}{\mathrm{argmax}}p(z=Z|x)p(x)$$

因为每次观测相互独立，所以上式可以写为：

$$x_{MAP}=\underset{x}{\mathrm{argmax}}\prod _{i}p(z_i=Z_i|x)p(x)$$

取负对数得到：

$$x_{MAP}=\underset{x}{\mathrm{argmin}}\left[-\sum _{i}logp(z_i=Z_i|x)-logp(x)\right]$$
如果假设观测$p(z_i|x)$服从高斯分布$N(h_i(x)，{\sum }_i)$；先验$p(x)$服从$N(x_p，{\sum }_x$