从零开始学习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ξ)=ip(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ξmaxip(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[ilogp(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=ξargminiriμ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<

评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值