SLAM14讲学习记录-状态估计问题

经典SLAM模型

通常假设两个噪声项和满足高斯分布

,

我们希望通过带噪声的数据z和u推断位资x和地图y（以及它们的概率分布），这构成了一个状态估计问题

状态估计问题大致分为两类：

1. 增量方法，或称滤波器：尽关心当前时刻的状态估计，对之前的状态则不多考虑，具有马尔可夫性，即下个状态只和上个状态有关。
2. 批量方法：我们可以把0到k时刻所有的输入和观测数据都放在一起，可以在更大范围达到最优化。

批量方法：

定义机器人位姿和路标点坐标为：

，

用不带下标的u表示所有时刻的输入，z表示所有时刻的观测数据，已知输入数据u和观测数据z，求状态x,y的条件概率分布：

特别的，当不知道输入，只知道观测数据时，相当与估计，此问题也成为SfM，即如何从许多图像中重建出三维结构。

利用贝叶斯法则，有：

贝叶斯法则左侧称为后验概率，右侧成为似然，成为先验。

直接求后验分布是困难的，但求一个状态的最优估计，使得在该状态下后验概率最大化是可行的：

当没有先验时，可以求解最大似然估计：

似然：在现在的状态（位姿）下，可以产生怎样的观测数据。由于我们知道观测数据，则可以理解为：在什么样的状态（位姿）下，最可能产生现在观测到的数据。

最小二乘的引出：假设噪声项服从高斯分布，，则观测与输入数据的条件概率为：

假设各个时刻的输入与观测互相独立，则：

任意高维高斯分布，取其概率密度函数展开形式的负对数：

因对数函数是单调增的，所以对原函数取最大值相当与对负对数取最小值，上式第一项与x无关，可以省略。因此，只要最小化右侧的二次型项，就得到了状态的最大似然估计。

二次型成为马哈拉诺比斯距离（马氏距离），高斯分布协方差矩阵之逆称为信息矩阵

带入SLAM模型：

令：

，

得：

 这样就得到了一个最小二乘问题，它的解等价于状态的最大似然估计。