SLAM中基于滤波与优化的方法的联系

参考书：slam十四讲、SLAM中的EKF，UKF，PF原理简介

基于滤波的状态估计思路是，用前一个时刻的值来估计下一个时刻，而优化则是把所有状态看成变量，把运动方程和观测方程看成变量间的约束，构造误差函数，然后最小化这个误差的二次型。这里，着重理解如何将滤波的方法和优化的方法统一到贝叶斯的框架下的.

---

经典SLAM模型：
$$\{\begin{array}{c}{x}_k=f(x_{x-1},u_k)+w_k\\ z_{k,j}=h(y_j,x_k)+v_{k,j}\end{array}$$
其中，第一个是运动方程，第二个是观测方程. SLAM的问题其实就是根据运动输入 $u$ 和观测 $z$，求什么样的状态 $x$ 能使状态 $x$ 的条件概率分布最大：
$$x_{MAP}^{\ast }=argmaxP(x|z,u)$$

---

非线性优化

在只考虑观测方程时，也就是没有测量运动输入时（注意：这里不是说整个系统没有运动输入，而是观测方程中不包含运动输入这一项，也就是说运动输入只出现在运动方程中），根据贝叶斯法则，有：
$$P(x|z)=\frac{P(z|x)P(x)}{P(z)}\propto P(z|x)P(x)$$
其中，$P(x|z)$ 是后验概率，右侧的 $P(z|x)$ 叫做似然，$P(x)$ 叫做先验.
我们的目的是寻找 $x$ 使得 $P(x|z)$ 最大，即最大后验概率，它等于似然与先验的乘积，因为我们在观测方程中不知道机器人位姿在哪里（注意，这是因为观测方程没有运动输入啊，运动输入属于运动方程的范畴，注意跟KF的区别），所以就没有先验喽~，然后上面的最大后验概率问题就转换成了最大似然问题，即：
$$x_{MLE}^{\ast }=argmaxP(z|x)$$
最大似然估计可以理解为：“在什么样的状态下，最可能产生现在观测的数据.”
根据观测方程（注意，这个位置 $x_k$ 与KF中观测方程中 ${\tilde{x}}_k$ 是不一样的，KF里是指状态变量的先验估计，这里没有先验的说法，而就是指优化变量本身.）
$$z_{k,j}=h(y_j,x_k)+v_{k,j}$$
得到观测数据的条件概率为：
$$P(z_{j,k}|x_k,y_j)=N(h(y_j,x_k),Q_{k,j})$$
多维高斯分布概率密度函数为：
$$P(x)=\frac{1}{\sqrt{(2\pi {)}^{2}det(\Sigma )}}exp\left(-\frac{1}{2}(x-u{)}^T{\Sigma }^{-1}(x-u)\right)$$
接着根据最小化负对数的方式，对状态的最大似然估计转换成：
$$x^{\ast }=argmin(z_{k,j}-h(y_j,x_k){)}^T{Q}_{k,j}^{-1}(z_{k,j}-h(y_j,x_k))$$
该式等价于最小化噪声项（即误差）的平方（$\Sigma$ 范数意义下），这里的误差也叫做残差，如下：
$$e_{y,j,k}=z_{k,j}-h(y_j,x_k)$$

---

卡尔曼滤波

$$\begin{array}{c}\begin{array}{rl}& x_k=A_k{x}_{k-1}+B_k{u}_k+w_k\\ & z_k=C_k{x}_k+v_k\end{array}\end{array}$$
KF状态转移方程如下：
$$x_k=A_k{x}_{k-1}+B_k{u}_k+w_k$$
假设状态变量符合高斯分布时，我们将只需维护状态变量的均值和协方差即可，均值为：
$${\tilde{x}}_k=A_k{\hat{x}}_{k-1}+B_k{u}_k$$
使用协方差传递律，可得状态变量的协方差如下：
$${\tilde{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^T+B_k{M}_k{B}_k^T$$
根据刚才推导的，后验概率：
$$P(x_k|z_k)=P(z_k|x_k)P(x_k)$$
为了推导KF公式，这里将结果设为 $x_k\sim N({\hat{x}}_k,{\hat{P}}_k)$，然后根据后验概率公式得到：
$$N({\hat{x}}_k,{\hat{P}}_k)=N(C_k{x}_k,R)\cdot N({\tilde{x}}_k,{\tilde{P}}_k)$$
令指数部分相等，即可推出 KF的三个更新方程，这样能看到贝叶斯法则是如何应用到KF滤波中的了.

---

<完>
@leatherwang

---