状态估计问题

1.批量状态估计与最大后验估计

经典SLAM模型由一个运动方程和一个观测方程构成：
$\begin{cases} x_k=f(x_{k-1},u_k)+w_k \\ z_{k,j}=h(y_i,x_k)+v_{k,j} \end{cases}$
其中 $x_k$是相机的位姿，可以用 $T_k \in SE(3)$描述，观测方程即针孔相机模型。假设在 $x_k$处对路标 $y_j$进行了 一次观测，对应到图像上的像素位置 $z_{k,j}$，那么观测方程可以表示成
$sz_{k,j}=K(R_ky_j+t_k)$
其中 $K$为相机内参， $s$为像素点的距离，也是 $(R_k y_j + t_k)$的第三个分量。
 考虑数据受噪声影响后发生的改变。在运动和观测方程中，我们通常假设两个噪声项 $w_k,v_{k,j}$满足零均值的高斯分布，像这样：
$W_k \sim \mathcal{N}(0,R_v), v_k \sim \mathcal{N}(0, Q_{k,j}).$
其中 $\mathcal{N}$表示高斯分布， $0$表示零均值， $R_k,Q_{k,j}$为协方差矩阵。在噪声影响下，我们希望通过带噪声的数据 $z$和 $u$推断位姿 $x$和地图 $y$（以及它们的概率分布），这构成了一个状态估计问题。
 从 1 到 N 的所有时刻，假设有 M 个路标点。定义所有时刻的机器人位姿和路标点坐标为
$x=\{x_1,...,x_N\}, y=\{y_1,...,y_N\}$
同样用不带下标的 $u$表示所有时刻的输入， $z$表示所有时刻的观测数据。求机器人的状态估计，就是在 $u$和 $z$已知条件下，求状态 $x$和 $y$的条件概率分布： $P(x,y|z,u)$
 利用贝叶斯法则，有
$P(x,y|z,u)={\frac{P(z,u|x,y)P(x,y)}{P(z,u)}} \Rightarrow \underbrace {P(z,u|x,y)}_{似然}\underbrace {P(x,y)}_{先验}$
贝叶斯法则左侧称为后验概率，右侧的 $P(z|x)$称为似然，另一部分 $P(x)$称为先验。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化，则是可行的:
$(x,y)^*_{MAP}=argmaxP(x,y|z,u)=argmaxP(z,u|x,y)P(x,y)$
请注意贝叶斯法则的分母部分与待估计的状态 $x,y$无关，因而可以忽略。求解最大后验概率等价于最大似然和先验的乘积。由于我们不知道机器人位姿或路标大概在什么地方，因此就没有了先验。那么，可以求解最大似然估计:
$(x,y)^*_{MLE}=argmaxP(z,u|x,y)$
 最大似然估计可以理解成：“在什么样的状态下，最可能产生现在观测到的数据”。

2.最小二乘的引出

 对于某一次观测:
$z_{k,j}=h(y_j,x_k)+v_{k,j}$
由于我们假设了噪声项 $v_k \sim \mathcal{N}(0,Q_{k,j})$，所以观测数据的条件概率为
$P(z_{j,k} | x_k,y_j) = \mathcal{N}(h(y_j,x_k), Q_{k,j})$
 单次观测的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然。任意高维高斯分布 $x \sim N(\mu,\sum)$，它的概率密度函数展开形式为
$P(x)={\frac{1}{\sqrt{(2\pi)^Ndet(\sum)}}}exp(-\frac{1}{2}(x-\mu)^T{\sum}^{-1}(x-\mu))$
对其取负对数，得
$-ln(P(x))=\frac{1}{2}ln((2\pi)^Ndet(\sum))+\frac{1}{2}(x-\mu)^T{\sum}^{-1}(x-\mu)$
因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化。在最小化上式的 $x$时，第一项与 $x$无关，可以略去。代入SLAM的观测模型:
$(x_k.y_j)^*=argmax \mathcal{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)))$
该式等于最小化噪声项的一个二次型。这个二次型成为马哈拉诺比斯距离，又叫马氏距离。它也可以看成由 $Q^{-1}_{k,j}$加权之后的欧氏距离，在这里 $Q^{-1}_{k,j}$也叫做信息矩阵，即高斯分布协方差矩阵之逆。
 定义各次输入和观测数据与模型之间的误差:
$e_{u,k}=x_k-f(x_{k-1},u_k) \\ e_{z,j,k}=z_{k,j}-h(x_k,y_j)$
最小化所有时刻估计值与真实值之间的马氏距离，等价于求最大似然估计。负对数允许我们把乘积变成求和:
$minJ(x,y)=\sum_ke^T_{u,k}R^{-1}_ke_{u,k}+\sum_k\sum_je^T_{z,k,j}Q^{-1}_{k,j}e_{z,j,k}$
这样就得到了最小二乘问题，它的解等价于状态的最大似然估计。