目录
(一)SLAM概率问题定义
SLAM问题其实就是一个状态估计的问题,就是要根据一系列观测来推测状态量;一般情况下,我们把SLAM问题建立在概率论的框架下。
说白了,SLAM就是要是解决这样一类后验概率问题:
p ( x ∣ z ) p(x|z) p(x∣z)
其中 x x x是系统当前状态量, z z z是与状态量相关的观测量;在求出这个条件概率之后,把观测值 Z Z Z带入,就可以获得 p ( x ∣ z = Z ) p(x|z=Z) p(x∣z=Z),这也就是我们需要的分布。
1.1 概率分解
根据概率公式展开:
p ( x ∣ z = Z ) = p ( z = Z ∣ x ) p ( x ) p ( z = Z ) p(x|z=Z)=\frac{p(z=Z|x)p(x)}{p(z=Z)} p(x∣z=Z)=p(z=Z)p(z=Z∣x)p(x)
1.2 归一化因子
其中分母是常量,当作是归一化因子 η \eta η即可,所以也可写为:
p ( x ∣ z = Z ) = η p ( z = Z ∣ x ) p ( x ) p(x|z=Z)=\eta{p(z=Z|x)p(x)} p(x∣z=Z)=ηp(z=Z∣x)p(x)
1.3 似然
p ( z ∣ x ) p(z|x) p(z∣x)表示在 x x x已知的情况下, z z z的概率分布;在求出 p ( z ∣ x ) p(z|x) p(z∣x)之后,把 z = Z z=Z z=Z带入,得到 p ( z = Z ∣ x ) p(z=Z|x) p(z=Z∣x),它也被称为似然项,它表示在 x x x取不同值的情况下, z = Z z=Z z=Z的概率。
1.4 先验
p ( x ) p(x) p(x)也被称为先验,也就是在观测之前 x x x服从什么分布。这可以通过其他信息源得到,比如GPS,惯导等。如果事前不知道任何先验分布信息,那么可以将其设为1,表示不相信先验信息,只根据系统使用的量测来估计。
1.5 最小二乘
x M A P = a r g m a x x p ( z = Z ∣ x ) p ( x ) x_{MAP}= \mathop{argmax}\limits_{x}p(z=Z|x)p(x) xMAP=xargmaxp(z=Z∣x)p(x)
因为每次观测相互独立,所以上式可以写为:
x M A P = a r g m a x x ∏ i p ( z i = Z i ∣ x ) p ( x ) x_{MAP}= \mathop{argmax}\limits_{x}\prod_{i}p(z_i=Z_i|x)p(x) xMAP=xargmaxi∏p(zi=Zi∣x)p(x)
取负对数得到:
x M A P = a r g m i n x [ − ∑ i l o g p ( z i = Z i ∣ x ) − l o g p ( x ) ] x_{MAP}=\mathop{argmin}\limits_{x}\left[-\sum_ilogp(z_i=Z_i|x)-logp(x)\right] xMAP=xargmin[−i∑logp(zi=Zi∣x)−logp(x)]
如果假设观测 p ( z i ∣ x ) p(z_i|x) p(zi∣x)服从高斯分布 N ( h i ( x ) , ∑ i ) N(h_i(x),\sum_i) N(hi(x),∑i);先验 p ( x ) p(x) p(x)服从 N ( x p , ∑ x ) N(x_p,\sum_{x}) N(xp,∑x