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

1. 最大似然估计

运动方程：
$x_k = f(x_{k-1}, u_k) + w_k$
它表示第 $k$ 个时刻的相机位置 $x_k$ ，是由上一个时刻的位置 $x_{k-1}$ ，经过 $u_k$ 的位置变化（IMU），以及一定的噪音 $w_k$ 决定的。

观测方程：
$z_{k, j} = h(y_j, x_k) + v_{k,j}$
它表示第 $k$ 个时刻对物体 $j$ 的观测数据，是由当时物体的坐标 $y_j$ 和相机的位置 $x_k$ ，以及这个物体一定的噪音 $v_{k, j}$ 决定的。

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

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

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

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

回到 Slam 问题里，我们想要根据已有的照片（也就是观测数据 $z_{k,j}$ ）和 IMU 数据( $u_k$ )，求解相机的位置 $x_k$ 和物体的位置 $y_j$ ，用概率表示为：
$P(x, y\ |\ z, u)$
特殊的，在纯视觉没有 IMU 时，可以去掉 $u$ ：
$P(x, y\ |\ z)$

根据贝叶斯法则，有：
$P(x, y\ |\ z, u) = \frac{P(z,u|x,y)P(x,y)}{P(z,u)}\propto \underbrace {P(z,u|x,y)}_{似然} · \underbrace {P(x,y)}_{先验}$

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

所以我们有， 最大后验估计（Maximize a Posterior）：
$x^*_{MAP} = argmax\ P(x,y\ |\ z, u) = argmax\ \underbrace {P(z,u|x,y)}_{似然} · \underbrace {P(x,y)}_{先验}$

实际上，我们不知道相机位姿 $x$ 和目标物体的位置 $y$ ，所以又没有了先验 $P(x, y)$ 。
所以我们只能求解最大似然估计（Maximize Likelihood Estimation）：
$x^*_{MLE} = argmax\ \underbrace {P(z,u|x,y)}_{似然}$

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

2. 最小二乘的引出

如何求解最大似然估计？
回顾观测模型：
$z_{k, j} = h(y_j, x_k) + v_{k,j}$
假设噪音 $v_{k,j}$ 满足均值是0的高斯分布，即 $v \sim N(0,~Q_{k, j})$ ，那么观测数据的似然概率是：
$P(z_{j, k}|x_k,y_j)~=~N(h(y_j, x_k), Q_{k, j})$

考虑任意高维高斯分布 $x \sim N(μ,Σ)$ ，它的概率密度函数展开后是：
$P(x) = \frac{1}{\sqrt{(2\pi)^Ndet(Σ)}}exp(-\frac{(x-μ)^T(x-μ)}{2Σ})$

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

我们要求 $P(x)$ 的最大值，可以先取负对数，改为求负对数形式的最小值：
$-ln(P(x)) = \underbrace {\frac{1}{2}ln((2\pi)^Ndet(Σ))}_{与x的计算无关}+\underbrace {\frac{1}{2}(x-μ)^TΣ^{-1}(x-μ)}_{只需求这一部分的最大值}$

所以只需要计算右侧的最小值，就得到了最大似然估计。
带入 Slam 的观测模型，相当于在求：
$\begin{align*} (x_k, y_j)^* &= argmax~N(h(y_j, x_k), Q_{k, j})\\ &= argmin~((z_{k,j} - h(x_k, y_j))^TQ^{-1}_{k, j}(z_{k, j} - h(x_k, y_j)))\\ \end{align*}$

上面是在求单次观测的最大似然估计，下面我们考虑批量的情况：
$P(z,μ|x,y)~=~\prod_kP(μ_k|x_{k-1}, x_k)\prod_{k,j}P(z_{k,j}|x_k,y_j)$

定义每次输入的误差：
$e_{u,k} ~=~ x_k - f(x_{k-1}, μ_k)$
每次观测的误差：
$e_{z,j,k} ~=~ z_{k,j} - h(x_k, y_j)$

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

$min J(x,y) = \underbrace {\sum_ke^T_{μ,k}R^{-1}_ke_{μ,k}}_{运动误差} + \underbrace {\sum_k\sum_je^T_{z,k,j}Q^{-1}_{k,j}e_{z,k,j}}_{观测误差}$

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