从零开始学习VIO笔记 --- 第四讲:滑动窗口(基于滑动窗口算法的 VIO 系统:可观性和一致性)
前一讲中有提到整个残差函数由三部分组成,其中第一部分就是滑动窗口的先验,这一讲主要就是讲如何在去除一帧信息的时候向整个残差添加约束。
一. 从高斯分布到信息矩阵
1.1 高斯分布
首先对SLAM问题进行一个概率建模,利用贝叶斯的理论。
考虑某个状态 ξ \boldsymbol\xi ξ , 以及一次与该变量相关的观测 r i \mathbf{r}_i ri。由于噪声的存在,观测服从概率分布 p ( r i ∣ ξ ) p(\mathbf{r}_i | \boldsymbol\xi) p(ri∣ξ)。多次观测时,各个测量值相互独立,则多个测量 r = ( r 1 , . . . , r n ) T \mathbf{r} =(\mathbf{r}_1,...,\mathbf{r}_n)^T r=(r1,...,rn)T 构建的似然概率为:
p ( r ∣ ξ ) = ∏ i p ( r i ∣ ξ ) p(\mathbf{r} \mid \boldsymbol{\xi})=\prod_{i} p\left(\mathbf{r}_{i} \mid \boldsymbol{\xi}\right) p(r∣ξ)=i∏p(ri∣ξ)
如果知道机器人状态的先验信息 p ( ξ ) p( \boldsymbol\xi) p(ξ),如 GPS, 车轮码盘信息等,则根据 Bayes 法则,有后验概率: p ( ξ ∣ r ) = p ( r ∣ ξ ) p ( ξ ) p ( r ) p(\boldsymbol{\xi} \mid \mathbf{r})=\frac{p(\mathbf{r} \mid \boldsymbol{\xi}) p(\boldsymbol{\xi})}{p(\mathbf{r})} p(ξ∣r)=p(r)p(r∣ξ)p(ξ)
通过最大后验估计,获得系统状态的最优估计:
ξ M A P = arg max ξ p ( ξ ∣ r ) \boldsymbol{\xi}_{\mathrm{MAP}}=\arg \max _{\boldsymbol{\xi}} p(\boldsymbol{\xi} \mid \mathbf{r}) ξMAP=argξmaxp(ξ∣r)
由于后验公式中分母跟状态量无关,舍弃。最大后验变成了:
ξ M A P = arg max ξ ∏ i p ( r i ∣ ξ ) p ( ξ ) \boldsymbol{\xi}_{\mathrm{MAP}}=\arg \max _{\xi} \prod_{i} p\left(\mathbf{r}_{i} \mid \boldsymbol{\xi}\right) p(\boldsymbol{\xi}) ξMAP=argξmaxi∏p(ri∣ξ)p(ξ)
即
ξ M A P = arg min ξ [ − ∑ i log p ( r i ∣ ξ ) − log p ( ξ ) ] \boldsymbol{\xi}_{\mathrm{MAP}}=\arg \min _{\boldsymbol{\xi}}\left[-\sum_{i} \log p\left(\mathbf{r}_{i} \mid \boldsymbol{\xi}\right)-\log p(\boldsymbol{\xi})\right] ξMAP=argξmin[−i∑logp(ri∣ξ)−logp(ξ)]
如果假设观测值服从多元高斯分布: p ( r i ∣ ξ ) = N ( μ i , Σ i ) , p ( ξ ) = N ( μ ξ , Σ ξ ) p\left(\mathbf{r}_{i} \mid \boldsymbol{\xi}\right)=\mathcal{N}\left(\boldsymbol{\mu}_{i}, \boldsymbol{\Sigma}_{i}\right), p(\boldsymbol{\xi})=\mathcal{N}\left(\boldsymbol{\mu}_{\xi}, \boldsymbol{\Sigma}_{\xi}\right) p(ri∣ξ)=N(μi,Σi),p(ξ)=N(μξ,Σξ)
则有: ξ M A P = argmin ξ ∑ i ∥ r i − μ i ∥ Σ i 2 + ∥ ξ − μ ξ ∥ Σ ξ 2 \boldsymbol{\xi}_{\mathrm{MAP}}=\underset{\xi}{\operatorname{argmin}} \sum_{i}\left\|\mathbf{r}_{i}-\boldsymbol{\mu}_{i}\right\|_{\Sigma_{i}}^{2}+\left\|\boldsymbol{\xi}-\boldsymbol{\mu}_{\xi}\right\|_{\Sigma_{\xi}}^{2} ξMAP=ξargmini∑∥ri−μi∥Σi2+∥∥ξ−μξ∥∥Σξ2
这个最小二乘的求解可以使用上节课的解法:
J ⊤ Σ − 1 J δ ξ = − J ⊤ Σ − 1 r \mathbf{J}^{\top} \mathbf{\Sigma}^{-1} \mathbf{J} \delta \boldsymbol{\xi}=-\mathbf{J}^{\top} \boldsymbol{\Sigma}^{-1} \mathbf{r} J⊤Σ−1Jδξ=−J⊤Σ−1r<