SLAM最大后验估计求解

问题定义

$运动方程：x_k=A_{k-1}{x}_{k-1}+v_k+w_k,k=1...K$
$观测方程：y_k=C_k{x}_k+n_k,k=0...K$
其中$k$为时间下标，最大值为$K$。各变量的含义如下
系统状态：$x_kϵR^n$
初始状态：$x_0ϵR^n\sim N({\check{x}}_0,{\check{P}}_0)$
输⼊：$v_kϵR^n$
过程噪声：$w_kϵR^n\sim N(0,Q_k)$
测量： $y_kϵR^m$
测量噪声：$n_kϵR^m\sim N(0,R_k)$

状态估计问题是指，在$k$个(一个或多个)时间点上，基于初始的状态信息${\check{x}}_0$，一系列观测数据$y_{0:K,meas}$，一系列输入$v_{1:K}$，以及系统的运动和观测模型，来计算系统的真实状态的估计值${\hat{x}}_k$。

最大后验估计(Maximum A Posteriori, MAP)

MAP的目标就是求解以下问题：
$$\hat{x}=\underset{x}{\mathrm{argmax}}p(x|v,y)$$
由公式：
$p(x,y,v)=p(y|x,v)p(x,v)=p(x|v,y)p(v|y)$
$p(x,v)=p(x|v)p(v)$
$p(v,y)=p(y|v)p(v)$
可得出：
$$\hat{x}=\underset{x}{\mathrm{argmax}}p(x|v,y)=\underset{x}{\mathrm{argmax}}\frac{p(y|x,v)p(x|v)}{p(y|v)}$$
又因为上式分母和$x$无关，可以把分母略去，同时在$x$已知的情况下$p(y|x,v)$中的v可以省略。
所以可以得到：
$$\hat{x}=\underset{x}{\mathrm{argmax}}p(x|v,y)=\underset{x}{\mathrm{argmax}}\frac{p(y|x,v)p(x|v)}{p(y|v)}=\underset{x}{\mathrm{argmax}}p(y|x)p(x|v)$$
假设所有时刻的$w_k$和$n_k$之间是无关的。则有：
$$p(y|x)=\coprod _{k=0}^{K}p(y_k|x_k)$$
还有：
$$p(x|v)=\coprod _{k=1}^{K}p(x_k|x_{k-1},v_k)$$
在线性系统中，高斯密度函数可展开为：
$$p(x_0|{\check{x}}_0)=\frac{1}{\sqrt{(2\pi {)}^{N}det{\check{P}}_0}}\times exp(-\frac{1}{2}(x_0-{\check{x}}_0{)}^T{\check{P}}_0^{-1}(x_0-{\check{x}}_0))$$
$$p(x_k|x_{k-1},v_k)=\frac{1}{\sqrt{(2\pi {)}^{N}det{Q}_k}}\times exp(-\frac{1}{2}(x_k-A_{k-1}{x}_{k-1}-v_k{)}^T{Q}_k^{-1}(x_k-A_{k-1}{x}_{k-1}-v_k))$$
$$p(y_k|,x_k)=\frac{1}{\sqrt{(2\pi {)}^{M}det{R}_k}}\times exp(-\frac{1}{2}(y_k-C_k{x}_k{)}^T{R}_k^{-1}(y_k-C_k{x}_k))$$
对等式两端取对数：
$$lnp(y|x)p(x|v))=lnp(x_0|{\check{x}}_0)+\sum _{k=1}^{K}lnp(x_k|x_{k-1},v_k)+\sum _{k=0}^{K}lnp(y_k|x_k))$$
有：
$$lnp(x_0|{\check{x}}_0)=-\frac{1}{2}(x_0-{\check{x}}_0{)}^T{\check{P}}_0^{-1}(x_0-{\check{x}}_0))-\frac{1}{2}ln((2\pi {)}^{N}det{\check{P}}_0)$$
$$lnp(x_k|x_{k-1},v_k)=-\frac{1}{2}(x_k-A_{k-1}{x}_{k-1}-v_k{)}^T{Q}_k^{-1}(x_k-A_{k-1}{x}_{k-1}-v_k))-\frac{1}{2}ln((2\pi {)}^{N}det{Q}_k)$$
$$lnp(y_k|,x_k)=-\frac{1}{2}(y_k-C_k{x}_k{)}^T{R}_k^{-1}(y_k-C_k{x}_k))-\frac{1}{2}ln((2\pi {)}^{M}det{R}_k)$$
上面三个式子中和$x$无关的项可以忽略掉,可定义如下等式:
$$\begin{array}{rl}& J_{v,k}(x)=\{\begin{array}{rl}& \frac{1}{2}(x_0-{\check{x}}_0{)}^T{\check{P}}_0^{-1}(x_0-{\check{x}}_0)k=0\\ & \frac{1}{2}(x_k-A_{k-1}{x}_{k-1}-v_k{)}^T{Q}_k^{-1}(x_k-A_{k-1}{x}_{k-1}-v_k),k=1...K\end{array}\\ & J_{y,k}(x)=\frac{1}{2}(y_k-C_k{x}_k{)}^T{R}_k^{-1}(y_k-C_k{x}_k),k=0...K\end{array}$$
定义整体目标函数，最小化这个目标函数，就是我们的解：
$$J(x)=\sum _{k=0}^K(J_{v,k}(x)+J_{y,k}(x))$$

声明

本文参考自《机器人学的状态估计》