《视觉SLAM十四讲》学习笔记-状态估计问题

2023-05-16

最大后验与似然

经典slam模型可表示为：

{x⃗ k=f(x⃗ k−1,u⃗ k)+w⃗ kz⃗ k,j=h(y⃗ j,x⃗ k)+v⃗ k,j { x → k = f ( x → k − 1 , u → k ) + w → k z → k , j = h ( y → j , x → k ) + v → k , j

$\begin{cases} \vec{x}_k = f(\vec{x}_{k-1}, \vec{u}_k) + \vec{w}_k\\ \vec{z}_{k,j} = h(\vec{y}_j, \vec{x}_k) + \vec{v}_{k, j} \end{cases}$
注意这里的 x⃗ x → $\vec{x}$ 为相机的位姿，可以用李群 Tk T k $\mathbf{T}_k$ 或 李代数 exp(ξ∧k) exp ⁡ ( ξ k ∧ ) $\exp(\xi^\wedge_k)$ 表示。假设在 x⃗ k x → k $\vec{x}_k$ 对路标 y⃗ j y → j $\vec{y}_j$ 进行一次观测，对应到图像的位置为 z⃗ k,j z → k , j $\vec{z}_{k,j}$ ,则观测方程为：

sz⃗ k,j=Kexp(ξ∧k)y⃗ j s z → k , j = K exp ⁡ ( ξ k ∧ ) y → j

$s\vec{z}_{k,j}=\mathbf{K}\exp(\xi^\wedge_k)\vec{y}_j$
式中 s s $s$ 为像素点的距离。式中

{\vec{z}}_{k, j}

$\vec{z}_{k,j}$ 和 y⃗ j y → j $\vec{y}_j$ 皆为齐次坐标.
不妨假设噪声项 w⃗ k w → k $\vec{w}_k$ 和 v⃗ k,j v → k , j $\vec{v}_{k, j}$ 满足零均值高斯分布：

w⃗ k∼N(0,Rk), v⃗ k,j∼N(0,Qk,j) w → k ∼ N ( 0 , R k ) , v → k , j ∼ N ( 0 , Q k , j )

$\vec{w}_k \sim N(0, \mathbf{R}_k), ~~\vec{v}_{k, j} \sim N(0, \mathbf{Q}_{k,j})$
先把所有的待估计变量放在一个 状态变量中：

x⃗ =x⃗ 1,⋯,x⃗ N,y⃗ 1,⋯,y⃗ M x → = x → 1 , ⋯ , x → N , y → 1 , ⋯ , y → M

$\vec{x} = {\vec{x}_1, \cdots, \vec{x}_N, \vec{y}_1, \cdots, \vec{y}_M}$

对机器人状态的估计，转变为求已知输入数据 $\vec{u}$ 和观测数据 $\vec{z}$ 和条件下，计算状态 $\vec{x}$ 的条件概率： $P(\vec{x}|\vec{z}, \vec{u})$ .

当没有运动测量的传感器，即不存在 $\vec{u}$ 时，上述问题转变为Structure from Motion(SfM)问题。此时，利用贝叶斯法则有：

P(x⃗ |z⃗ )=P(z⃗ |x⃗ )P(x⃗ )P(z⃗ )∝P(z⃗ |x⃗ )P(x⃗ ) P ( x → | z → ) = P ( z → | x → ) P ( x → ) P ( z → ) ∝ P ( z → | x → ) P ( x → )

$P(\vec{x}|\vec{z}) = \frac{P(\vec{z}|\vec{x})P(\vec{x})}{P(\vec{z})}\propto P(\vec{z}|\vec{x})P(\vec{x})$
上式中， P(z⃗ |x⃗ ) P ( z → | x → ) $P(\vec{z}|\vec{x})$ 为似然, P(x⃗ ) P ( x → ) $P(\vec{x})$ 为先验。此时可用 MAP(Maximize a Posterior, MAP)求解：

x⃗ ∗MAP=argmax P(x⃗ |z⃗ )=argmax P(z⃗ |x⃗ )P(x⃗ ) x → ∗ M A P = arg ⁡ max P ( x → | z → ) = arg ⁡ max P ( z → | x → ) P ( x → )

$\vec{x}*_{MAP} = \arg\max~P(\vec{x}|\vec{z}) = \arg\max~ P(\vec{z}|\vec{x})P(\vec{x})$
进一步地，假设机器人无法获得自己的位置，即 x⃗ x → $\vec{x}$ 没有，则可求解 x⃗ x → $\vec{x}$ 的 最大似然估计(Maximize Likelihood Estimation, MLE):

x⃗ ∗MLE=argmax P(z⃗ |x⃗ ) x → ∗ M L E = arg ⁡ max P ( z → | x → )

$\vec{x}*_{MLE} = \arg\max~ P(\vec{z}|\vec{x})$
上式可理解为”在什么样的状态下，最可能产生现在观测到的数据”.

最小二乘问题：

对一次观测：

z⃗ k,j=h(y⃗ j,x⃗ k)+v⃗ k,j z → k , j = h ( y → j , x → k ) + v → k , j

$\vec{z}_{k,j} = h(\vec{y}_j, \vec{x}_k) + \vec{v}_{k, j}$
并且有假设 v⃗ k,j∼N(0,Qk,j) v → k , j ∼ N ( 0 , Q k , j ) $\vec{v}_{k, j} \sim N(0, \mathbf{Q}_{k,j})$ ,条件概率为：

P(z⃗ k,j|x⃗ k,y⃗ j)=N(h(y⃗ j,x⃗ k),Qk,j)) P ( z → k , j | x → k , y → j ) = N ( h ( y → j , x → k ) , Q k , j ) )

$P(\vec{z}_{k,j} |\vec{x}_k, \vec{y}_j ) = N( h(\vec{y}_j, \vec{x}_k), \mathbf{Q}_{k,j}))$
为求解该问题，回忆一个任意的高维高斯分布 x⃗ ∼N(u⃗ ,Σ) x → ∼ N ( u → , Σ ) $\vec{x} \sim N(\vec{u}, \Sigma)$ ,其概率密度函数为：

P(x⃗ )=1(2π)Ndet(Σ)‾‾‾‾‾‾‾‾‾‾‾√exp(−12(x⃗ −u⃗ ⊤Σ−1(x⃗ −u⃗ ))) P ( x → ) = 1 ( 2 π ) N det ( Σ ) exp ⁡ ( − 1 2 ( x → − u → ⊤ Σ − 1 ( x → − u → ) ) )

$P(\vec{x}) = \frac{1}{\sqrt{(2\pi)^N \det(\Sigma)} } \exp\left(-\frac{1}{2} (\vec{x}-\vec{u}^\top \Sigma^{-1}(\vec{x}-\vec{u})) \right)$
两边取负对数后变为：

−ln(P(x⃗ ))=12ln((2π)Ndet(Σ))+12(x⃗ −u⃗ ⊤Σ−1(x⃗ −u⃗ )) − ln ⁡ ( P ( x → ) ) = 1 2 ln ⁡ ( ( 2 π ) N det ( Σ ) ) + 1 2 ( x → − u → ⊤ Σ − 1 ( x → − u → ) )

$-\ln (P(\vec{x})) = \frac{1}{2}\ln \left( (2\pi)^N \det(\Sigma) \right) + \frac{1}{2}(\vec{x}-\vec{u}^\top\Sigma^{-1} (\vec{x}-\vec{u}))$
应用上述公式，第一项与 x⃗ x → $\vec{x}$ 无关可移除，代入slam观测模型结果为：

x⃗ ∗=argmin((z⃗ k,j−h(x⃗ k,y⃗ i))⊤Q−1k,j(z⃗ k,j−h(x⃗ k,y⃗ i))) x → ∗ = arg ⁡ min ( ( z → k , j − h ( x → k , y → i ) ) ⊤ Q k , j − 1 ( z → k , j − h ( x → k , y → i ) ) )

$\vec{x}* = \arg\min \left( (\vec{z}_{k,j} - h(\vec{x}_k, \vec{y}_i) )^\top\mathbf{Q}^{-1}_{k,j} (\vec{z}_{k,j} - h(\vec{x}_k, \vec{y}_i) ) \right)$
定义数据与误差之间误差为：

{e⃗ v,k=x⃗ k−f(x⃗ k−1,u⃗ k)e⃗ y,j,k=z⃗ k,j−h(x⃗ k,y⃗ j) { e → v , k = x → k − f ( x → k − 1 , u → k ) e → y , j , k = z → k , j − h ( x → k , y → j )

$\begin{cases} \vec{e}_{v,k} = \vec{x}_k - f(\vec{x}_{k-1}, \vec{u}_k)\\ \vec{e}_{y,j,k} = \vec{z}_{k,j} - h(\vec{x}_{k}, \vec{y}_j) \end{cases}$

则误差平方项之和为：

J(x⃗ )=∑ke⃗ ⊤v,kR−1ke⃗ v,k+∑k∑je⊤y,k,jQ−1k,jey,k,j J ( x → ) = ∑ k e → v , k ⊤ R k − 1 e → v , k + ∑ k ∑ j e y , k , j ⊤ Q k , j − 1 e y , k , j

$J(\vec{x}) = \sum_{k}\vec{e}^\top_{v,k} \mathbf{R}^{-1}_k \vec{e}_{v,k} + \sum_{k}\sum_{j} e_{y,k,j}^\top\mathbf{Q}^{-1}_{k,j}e_{y,k,j}$
此式即为总体意义下的 最小二乘问题(Least Square Problem).

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)