ch-6.1状态估计问题

目的：如何在有噪声的数据中进行准确的状态估计

经典SLAM模型：

其中位姿变量xk可以由Tk 或表达，二者是等价的，即观测方程可表示为：

 这里K 为相机内参，s 为像素点的距离。现在，考虑数据受噪声的影响后，会发生什么改变。在运动和观测方程中，我们通常假设两个噪声项满足零均值的高斯分布：

 

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

---

问题描述：已知运动和观测，求轨迹和地图

利用贝叶斯法则：

将最大后验概率转换为max（似然*先验），后验概率最大化（Maximize a Posterior，MAP）：

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

但是有时候我们没有运动信息，只有一组观测，所以没有先验那么，可以求解x 的最大似然估计（Maximize Likelihood Estimation, MLE）：

 

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

---

观测误差：为了求最大似然估计

 某次观测：

 其噪声项：

 所以观测数据的条件概率为：

 观测数据的条件概率是一个高斯分布。为了计算使它最大化的xk; yj，我们往往使用最小化负对数的方式，来求一个高斯分布的最大似然。

对于一个任意多元高斯分布：

它的概率密度函数展开形式为:

取它的负对数，则变为：

代入SLAM观测模型：

这个二次型可以看成信息矩阵加权后的二范数考虑噪声加权的误差二次型。

---

求解最小二乘问题：

对于经典SLAM问题，假设各个时刻的输入和观测是独立的，可以分解成多个概率的乘积：

定于运动和观测误差：

将xy作为优化变量，将输入误差和观测误差之和作为目标函数，求当误差最小时的xy：

这个就是最小二乘问题，其解就等价于x的最大似然估计。

---

补充高斯分布定义： 

对应：

 对应：

---