以前记录过EKF，对于EKF这种增量方法仅关心当前时刻的状态估计，而对之前的状态则不多考虑。还有另一种方法，叫做批量方法，就是在更大的范围内达到最优化。就是把之前所有的数据都用来评估。显然这种做法不是实时的，所以在视觉slam中， 会用别的一些折中的方法，但是批量的方法更容易介绍背后的原理。

我们假设下面的两个公式为运动和观测模型。

假设噪声项  和  满足零均值高斯分布：

对噪声的假设很重要， 高斯分布可以最大可能的简化我们的公式推导和计算，带来很大的方便。

我们看下图的小车轨迹，实线代表真实轨迹， 虚线是进行批量计算后的估计轨迹，可以看到越后面，数据越多，估计的轨迹越准，状态也就越准。（运动状态估计就是获取准确的  和  ）

我们来简化一下问题， 我们现在假设物体运动了u，并且我们观测到了它的位置为z，我们希望用这两个数据来评估出物体在地图上的准确位置(x,y)。 很明显，我们用贝叶斯来描述：

前半部分是典型的贝叶斯，因为分母部分和x,y无关，所以我们不考虑，我们只考虑要优化的参数。所以就近似等于最右边的公式。

又由于，在现实中，我们一般是不会有先验概率的。比如我在做机器人运动轨迹估计的时候，很可能机器人在一个它完全不知道的地方运动，所以，我们可以直接忽略掉先验概率。简化后如下式：

这样，求最大概率的问题就转换为了最大似然值了。

有的童鞋估计还不太清楚什么叫做最大似然值，字面上理解就对了，就是最有可能的值是什么（上面的公式就是：在什么状态下，最有可能获取现在观测的数据(x,y)）。

接下来我们继续对这个公式进行改造。我们先不关注之前的联合概率P(x,y)。我们以观测数据为例：我们知道假设了观测数据的误差为  ，推导得知

 = 

回忆一个任意的高维高斯分布  ,其概率密度函数为：

高斯分布在负对数下有较好的形式，所以我们对其两边取负对数：

我们都知道对数的特性，所以  =  。求最大似然，就是求  得最小值。第一项与变量无关，忽略，1/2也可以忽略，不影响取最小值，得：

将  带入上式：

这里的  称为马氏距离，也叫作信息矩阵，即高斯分布协方差的逆。

有了上面的认知，我们可以来具体的推导一下  ，如果在存在K个观测数据点的情况下推导批量算法，由于运动和观测都是相互独立的，由条件概率公式得：

 ==>

 ==>

设运动和观测之间的误差为：

求得最终概率求解的问题就变成了一个最小化优化的问题了。

右边方程的最小值就是概率最大的时候，最终的目的就是在得到观测数据和移动数据之后，所估计的定位达到最大的准确度。

附上我的推导手稿，可以参考：