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

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/hzwwpgmwy/article/details/81607963

参考书：slam十四讲

着重理解如何将滤波的方法和优化的方法统一到贝叶斯的框架下的.

---

经典SLAM模型：

$$\left\{\begin{matrix} x_k=f(x_{x-1},u_k)+w_k \\ z_{k,j}=h(y_j,x_k)+v_{k,j} \end{matrix}\right.$$
其中，第一个是运动方程，第二个是观测方程. SLAM的问题其实就是根据运动输入 $u$ 和观测 $z$，求什么样的状态 $x$ 能使状态 $x$ 的条件概率分布最大：
$$x_{MAP}^*=arg\ maxP(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}^*=arg\ maxP(z|x)$$
最大似然估计可以理解为：“在什么样的状态下，最可能产生现在观测的数据.”
根据观测方程（ 注意，这个位置 $x_k$ 与KF中观测方程中 $\widetilde 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)=\frac1{\sqrt{(2\pi)^2det(\Sigma)}}exp\left(-\frac12 (x-u)^T\Sigma^{-1}(x-u)\right)$$
接着根据 最小化负对数的方式，对状态的 最大似然估计转换成：
$$x^*=arg\ min(z_{k,j}-h(y_j,x_k))^TQ_{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{equation} \begin{split} &x_k=A_kx_{k-1} + B_ku_k+w_k \\ &z_k=C_kx_k+v_k \end{split} \nonumber \end{equation}$$
KF状态转移方程如下：
$$x_k=A_kx_{k-1} + B_ku_k+w_k \\ $$
假设状态变量符合高斯分布时，我们将 只需维护状态变量的均值和协方差即可，均值为：
$$\widetilde x_k=A_k \hat x_{k-1} + B_ku_k$$
使用 协方差传递律，可得状态变量的协方差如下：
$$\widetilde{P}_k=A_k\hat{P}_{k-1}A_k^T+B_kM_kB_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_kx_k,\ R)·N(\widetilde x_k, \ \widetilde P_k)$$
令指数部分相等，即可推出 KF的三个更新方程，这样能看到 贝叶斯法则是如何应用到KF滤波中的了.

---

<完>
@leatherwang

---