最大后验与似然
经典slam模型可表示为:
{x⃗ k=f(x⃗ k−1,u⃗ k)+w⃗ kz⃗ k,j=h(y⃗ j,x⃗ k)+v⃗ k,j
注意这里的
x⃗
为相机的位姿,可以用
李群
Tk
或
李代数
exp(ξ∧k)
表示。假设在
x⃗ k
对路标
y⃗ j
进行一次观测,对应到图像的位置为
z⃗ k,j
,则观测方程为:
sz⃗ k,j=Kexp(ξ∧k)y⃗ j
式中
s
为像素点的距离。式中
z⃗ k,j
和
y⃗ j
皆为齐次坐标.
不妨假设噪声项
w⃗ k
和
v⃗ k,j
满足零均值高斯分布:
w⃗ k∼N(0,Rk), v⃗ k,j∼N(0,Qk,j)
先把所有的待估计变量放在一个
状态变量
中:
x⃗ =x⃗ 1,⋯,x⃗ N,y⃗ 1,⋯,y⃗ M
对机器人状态的估计,转变为求已知输入数据
u⃗
和观测数据
z⃗
和条件下,计算状态
x⃗
的条件概率:
P(x⃗ |z⃗ ,u⃗ )
.
当没有运动测量的传感器,即不存在
u⃗
时,上述问题转变为Structure from Motion
(SfM)问题。此时,利用贝叶斯法则有:
P(x⃗ |z⃗ )=P(z⃗ |x⃗ )P(x⃗ )P(z⃗ )∝P(z⃗ |x⃗ )P(x⃗ )
上式中,
P(z⃗ |x⃗ )
为
似然,
P(x⃗ )
为
先验。此时可用
MAP
(Maximize a Posterior, MAP)求解:
x⃗ ∗MAP=argmax P(x⃗ |z⃗ )=argmax P(z⃗ |x⃗ )P(x⃗ )
进一步地,假设机器人无法获得自己的位置,即
x⃗
没有,则可求解
x⃗
的
最大似然估计(Maximize Likelihood Estimation, MLE):
x⃗ ∗MLE=argmax P(z⃗ |x⃗ )
上式可理解为”在什么样的状态下,最可能产生现在观测到的数据”.
最小二乘问题:
对一次观测:
z⃗ k,j=h(y⃗ j,x⃗ k)+v⃗ k,j
并且有假设
v⃗ k,j∼N(0,Qk,j)
,条件概率为:
P(z⃗ k,j|x⃗ k,y⃗ j)=N(h(y⃗ j,x⃗ k),Qk,j))
为求解该问题,回忆一个任意的高维高斯分布
x⃗ ∼N(u⃗ ,Σ)
,其概率密度函数为:
P(x⃗ )=1(2π)Ndet(Σ)‾‾‾‾‾‾‾‾‾‾‾√exp(−12(x⃗ −u⃗ ⊤Σ−1(x⃗ −u⃗ )))
两边取负对数后变为:
−ln(P(x⃗ ))=12ln((2π)Ndet(Σ))+12(x⃗ −u⃗ ⊤Σ−1(x⃗ −u⃗ ))
应用上述公式,第一项与
x⃗
无关可移除,代入slam观测模型结果为:
x⃗ ∗=argmin((z⃗ k,j−h(x⃗ k,y⃗ i))⊤Q−1k,j(z⃗ k,j−h(x⃗ k,y⃗ i)))
定义数据与误差之间误差为:
{e⃗ v,k=x⃗ k−f(x⃗ k−1,u⃗ k)e⃗ y,j,k=z⃗ k,j−h(x⃗ k,y⃗ j)
则误差平方项之和为:
J(x⃗ )=∑ke⃗ ⊤v,kR−1ke⃗ v,k+∑k∑je⊤y,k,jQ−1k,jey,k,j
此式即为总体意义下的
最小二乘问题
(Least Square Problem).