SAM推导

最新推荐文章于 2023-01-02 00:00:19 发布

weixin_43745120

最新推荐文章于 2023-01-02 00:00:19 发布

阅读量317

点赞数 1

本文链接：https://blog.csdn.net/weixin_43745120/article/details/104625905

版权

参考文献

Frank Dellaert，Michael Kaess，The International Journal of Robotics Research，2006，Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing

模型

SLAM问题可以表示成置信网(belief net)，因子图(factor graph)，马尔可夫随机场(markov random field)。首先从置信网(也就是贝叶斯网络)的角度分析。
假设第 $i$ 时刻机器人的状态为 $x_i$ ， $i\in0$ $\dots$ $M$ ，路标为 $l_j$ ， $j\in1$ $\dots$ $N$ ，量测为 $z_k$ ， $k\in1$ $\dots$ $K$ 。
对应上图的联合概率密度模型为 $P(x_0)\prod_{i=1}^M {P(x_i|x_{i-1},u_i)}\prod_{k=1}^K {P(z_k|x_{ik},l_{jk})}$ 其中， $P(x_0)$ 表示初始状态， $P(x_i|x_{i-1},u_i)$ 表示运动模型， $P (z ∣ x, l)$ 表示路标量测模型。引入标准的高斯模型， $x_i = f_i(x_{i-1},u_i) + w_i\iff P(x_i|x_{i-1},u_i) \propto exp(-\frac1{2} \Vert f_i(x_{i-1},u_i) - x_i) \Vert^2_{\Lambda_i} )$ 其中 $f_i()$ 是一个process model， $w_i$ 是零均值高斯白噪声，协方差为 $\Lambda_i$ 。量测模型为 $z_k = h_k(x_{ik},l_{jk}) + v_k \iff P(z_k|x_{ik},l{jk}) \propto exp(-\frac1{2}\Vert h_k(x_{ik},l_{jk}) - z_k \Vert^2_{\Sigma_k})$