Slam笔记-状态估计与最小二乘的引出

1. 最大似然估计

运动方程：

它表示 第 个时刻的相机位置 ， 是由上一个时刻的位置 ，经过 的位置变化（IMU），以及一定的噪音 决定的。

观测方程：

它表示 第 个时刻 对物体 的观测数据，是由当时物体的坐标 和 相机的位置 ，以及这个物体一定的噪音 决定的。

关于先验分布、后验分布和似然估计：
在概率中，表示结果，表示原因，表示给定结果，原因是的概率。

后验概率
给定结果，求某个原因的概率。例如小明从家到公司5公里，他可以选择走路或者做地铁上班。如果上班用了1个小时，那我们推测小明上班最有可能是步行。如果用了10分钟，那我们推测小明更可能是坐地铁。 即 是后验概率。

先验概率
事先就已知的概率。例如已知一个盒子里有一红一白两个球，那么任取一个球颜色是白色的概率一定是50%。即 是先验概率。

似然概率
给定条件/原因，得到某个结果的概率，又称条件概率。例如给出投一次骰子得到6的概率是1/6, 得到的 投两次骰子都是6的概率是1/36。即 。

回到 Slam 问题里，我们想要根据已有的照片（也就是观测数据 ）和 IMU 数据()，求解相机的位置 和物体的位置 ，用概率表示为：

特殊的，在纯视觉没有 IMU 时，可以去掉 ：

根据贝叶斯法则，有：

表示成比例的。
可以知道，这种给定结果（观测数据 , ），求原因（相机位姿 和物体位置 ）属于后验概率。
直接求解「后验概率的分布」是很困难的，所以需要转化为求「一个」状态的最优估计，使得在该状态下后验概率最大化。

所以我们有， 最大后验估计（Maximize a Posterior）：

实际上，我们不知道 相机位姿 和 目标物体的位置 ，所以又没有了先验 。
所以我们只能求解最大似然估计（Maximize Likelihood Estimation）：

最大似然估计的直观意义：在什么样的状态下，最可能产生现在观测到的数据。

---

2. 最小二乘的引出

如何求解最大似然估计？
回顾观测模型：

假设噪音 满足均值是0的高斯分布，即 ，那么观测数据的似然概率是：

考虑任意高维高斯分布 ，它的概率密度函数展开后是：

是求多维方差Σ的行列式(determinant)。

我们要求 的最大值，可以先取负对数，改为求负对数形式的最小值：

所以只需要计算右侧的最小值，就得到了最大似然估计。
带入 Slam 的观测模型，相当于在求：

上面是在求单次观测的最大似然估计，下面我们考虑批量的情况：

定义每次输入的误差：

每次观测的误差：

那么，最小化所有时刻的误差，等价于求最大似然估计。

这样就得到了一个最小二乘问题。