从LG系统开始

从LG系统，也即线性（Linear）高斯（Gaussian）系统状态估计开始，我们有两个不同的途径：

1、贝叶斯推断：我们从状态的先验概率密度函数开始，通过观测值、输入、运动方程、初始状态来计算后验概率密度函数

2、Map（最大后验估计）：用优化理论，在寻找最大后验估计

这两者在原理上是截然不同的，但是对于LG系统，最终会给出相同的结论，因为LG系统中，贝叶斯后验概率正好是一个高斯分布。我们用优化方法寻找的高斯分布的最大值（也叫模），也正是其均值。但是要注意到，当我们步入非线性非高斯领域后，一个分布的模和均值不再相等，从而两种方法会给出不同的答案

首先，我们有联合概率P（X,Z） = P（Z|X）P（X）

但我们想求的不是这个联合概率

而是P(X|Z),也就是在当前观测Z下的最优的状态X 这个叫后验概率 但不太好求

所以将其分解得到 

也就是将后验转化为求似然乘先验：

这个叫MAP 就是最大后验估计，但是似乎我们在非线性优化理论里，都认为我们没有先验，也即不知道机器人大概在什么位置，这时我们就做的是最大似然估计

所以我们定义的因子其实是正比与似然，而不是正比于似然和先验的乘积：

那么 我们现在定义因子

注意因子和概率之间的关系

所以因子为：

这里是最关键的一步，我们是怎么引入损失函数的呢

注意到，我们如此定义因子时，当fx也即残差为0的时候，因子为1，也即概率为1

从而我们将概率的问题，转化成用最小二乘来计算的问题

滤波、非线性优化（最小二乘）、因子图和后验概率、最大似然等

基础对比：

滤波考虑马尔科夫性，是实时的

位姿图需要考虑回环检测，最终矩阵可能不是稀疏矩阵。但好在不需要实时运行，cpu有空闲就算一些，有空闲就算一些，所以矩阵不稀疏也不是什么大问题。

BA优化，路标点数量远大于位姿数量，是一个箭头型矩阵，可以用舒尔补的方法转换成三角阵快速求解。在前端中，需要实时求解。

因子图优化，可以在问题的拓扑结构上进行一些顶层抽象和简化，而不是直接硬解，相当于多了一步简化过程。如果只有路标和位姿之间的因子，和BA优化完全一样。不过因子图是个大筐，什么约束都能加，IMU，轮速计，GPS。边缘化掉节点之后还能保留稍微复杂一些的分布，信息损失较少。还有isam，可以在顶层结构上分析出新的测量值进来之后哪些节点需要优化，节省资源。

但不管是位姿图还是BA还是因子图，都是非线性优化，在当前估计点处求导展开用GN或者LM求解

滤波方法：

要么有运动方程（比如有测量运动的装置），这时肯定求的是后验概率，也就是用运动方程计算先验，然后先验加上测量值更新为后验

要么没有运动方程，这时要么假设不动，要么假设匀速，实际上也就有了运动方程，和之前一样

最小二乘方法：

BA：我们意识到，BA实际是只有观测方程，也即没有运动方程的SLAM问题，这时我们定义损失为zij-h（x,p），没有运动方程的项，我们可以认为这时实际上是最大似然估计

但在更一般的slam问题上，我们发现，最小二乘的引出一开始是对观测方程引出的，而且也对应着求最大似然估计，也即：

我们可以看到，一开始都是针对观测方程去推理的，最后得到6.10，也是一个很自然的对状态的最大似然估计

但到了6.11和6.12 我们不仅仅定义了来自测量方程的误差，还定义了来自运动方程的误差，并且都加入整体目标函数之中，这里有了运动方程了，所以，这里肯定是对状态的最大后验估计，我们为什么这么肯定，是因为：

来自，机器人学中的状态估计：

可以看到，这里是求的是最大后验估计MAP。也即argmax（P(y|x)P(x|v)）

注意，这里用y来表示观测，用v来表示控制也即我们常用的u

注意这里最后定义的损失，和我们刚才十四讲里的那个损失其实是相同的

所以我们可以很自信的认为：十四讲里一开始是用观测方程去推理的，最后得到6.10，如果我们的损失函数里只有这一部分，那显然是一个MIE估计

但到了6.11和6.12 十四讲还定义了来自运动方程的误差，并且都加入整体目标函数之中，这里有了运动方程，所以最终定义的一定是一个MAP估计。

实际上我们认为，十四讲在这里其实有一些指代不明，我们继续看机器人中的状态估计就有：

在机器人的状态估计中，把控制u和测量y放在一起，才组成z，可以认为z是提升形式的观测

从而就有3.14的损失函数形式，这个损失函数形式和14讲里的6.10的形式是一样的，但意义完全不同，这里3.14是一个MAP而6.10是MLE。

我们更推荐机器人的状态估计里的这种命名方式

我们回过头看VINS的目标函数：

这里的误差有，来自imu的观测z和状态x的误差，来自视觉的观测zl和状态x的误差，以及rp-HP代表的边缘化约束

那VINS是属于一个怎样的估计呢，我们可以武断的认为，MAP和MIE估计的区别在于是否有运动方程，或者是否有测量运动的传感器，这里我们注意到，imu的残差实际上是在两帧之间的，所以我们可以认为VINS是一个MAP

同理，在我们的手写BA里，我们的误差是在测量的图像点和状态之间的，所以我们认为手写BA是一个MLE