从零开始学习VIO笔记 --- 第四讲：滑动窗口（基于滑动窗口算法的 VIO 系统：可观性和一致性）

一. 从高斯分布到信息矩阵1.1 高斯分布1.2 高斯分布和协方差矩阵1.3 信息矩阵二. marginalization (边缘化) 与 舒尔补一. 滑动窗口算法滑动窗口算法导致的问题滑动窗口中的 FEJ 算法

前一讲中有提到整个残差函数由三部分组成，其中第一部分就是滑动窗口的先验，这一讲主要就是讲如何在去除一帧信息的时候向整个残差添加约束。

一. 从高斯分布到信息矩阵

1.1 高斯分布

首先对SLAM问题进行一个概率建模，利用贝叶斯的理论。

考虑某个状态 ξ\boldsymbol\xiξ , 以及一次与该变量相关的观测 ri\mathbf{r}_iri​。由于噪声的存在，观测服从概率分布 p(ri∣ξ)p(\mathbf{r}_i | \boldsymbol\xi)p(ri​∣ξ)。多次观测时，各个测量值相互独立，则多个测量 r=(r1,...,rn)T\mathbf{r} =(\mathbf{r}_1,...,\mathbf{r}_n)^Tr=(r1​,...,rn​)T 构建的似然概率为：

p(r∣ξ)=∏ip(ri∣ξ)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(ξ)​

通过最大后验估计，获得系统状态的最优估计：

ξMAP=arg⁡max⁡ξp(ξ∣r)\boldsymbol{\xi}_{\mathrm{MAP}}=\arg \max _{\boldsymbol{\xi}} p(\boldsymbol{\xi} \mid \mathbf{r})ξMAP​=argξmax​p(ξ∣r)

由于后验公式中分母跟状态量无关，舍弃。最大后验变成了：

ξMAP=arg⁡max⁡ξ∏ip(ri∣ξ)p(ξ)\boldsymbol{\xi}_{\mathrm{MAP}}=\arg \max _{\xi} \prod_{i} p\left(\mathbf{r}_{i} \mid \boldsymbol{\xi}\right) p(\boldsymbol{\xi}) ξMAP​=argξmax​i∏​p(ri​∣ξ)p(ξ)

即

ξMAP=arg⁡min⁡ξ[−∑ilog⁡p(ri∣ξ)−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(ri∣ξ)=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(μξ​,Σξ​)

则有：ξMAP=argmin⁡ξ∑i∥ri−μi∥Σi2+∥ξ−μξ∥Σξ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​=ξargmin​i∑​∥ri​−μi​∥Σi​2​+∥∥​ξ−μξ​∥∥​Σξ​2​

这个最小二乘的求解可以使用上节课的解法：

J⊤Σ−1Jδξ=−J⊤Σ−1r\mathbf{J}^{\top} \mathbf{\Sigma}^{-1} \mathbf{J} \delta \boldsymbol{\xi}=-\mathbf{J}^{\top} \boldsymbol{\Sigma}^{-1} \mathbf{r}J⊤Σ−1Jδξ=−J⊤Σ−1r

这里我们可以注意到和上一讲中的形式相比它多了一个协方差矩阵的逆 Σ−1\mathbf{\Sigma}^{-1}Σ−1,这是因为之前并没有考虑噪声，直接认为观测值r\mathbf{r}r和 ξ\boldsymbol\xiξ 相等，协方差矩阵的逆我们称之为信息矩阵，记为Λ\boldsymbol\ ΛΛ

1.2 高斯分布和协方差矩阵

1.3 信息矩阵

协方差矩阵的逆我们称之为信息矩阵，记为Λ\boldsymbol\ ΛΛ

二. marginalization (边缘化) 与 舒尔补

引入 marginalization (边缘化) 和 Schur’s complement (舒尔

补)

能快速求解矩阵 M 的逆

能从多元高斯分布P(a,b) 中分解得到边际概率 p(a) 和条件概率 p(b|a)

例：假设多元变量 x=[ab]x=\begin{bmatrix}a\\b\end{bmatrix}x=[ab​] 服从高斯分布,变量之间构成的协方差矩阵为

K=[cov(a,a)cov(b,a)cov(a,b)cov(b,b)]=[AC⊤CD]\mathbf K=\begin{bmatrix}cov(a,a)&cov(b,a)\\cov(a,b)&cov(b,b)\end{bmatrix}=\begin{bmatrix}A&C^\top\\C&D\end{bmatrix}K=[cov(a,a)cov(a,b)​cov(b,a)cov(b,b)​]=[AC​C⊤D​]

且零均值的多元高斯分布有如下概率形式：p(x)=1Zexp⁡(−12x⊤Σ−1x)p(\mathbf{x})=\frac{1}{Z} \exp \left(-\frac{1}{2} \mathbf{x}^{\top} \mathbf{\Sigma}^{-1} \mathbf{x}\right)p(x)=Z1​exp(−21​x⊤Σ−1x)

整体分布可以写成条件概率P(b|a)和边际概率P(a)的乘积，而这个乘积可以正比于:

P(a,b)=P(a)P(b∣a)∝exp⁡(−12[ab]⊤[AC⊤CD]−1[ab])∝exp⁡(−12[ab]⊤[AC⊤CD]−1[ab])∝exp⁡(−12[ab]⊤[I−A−1C⊤0I][A−100ΔA−1][I0−CA−1I][ab])∝exp⁡(−12[a⊤(b−CA−1a)⊤][A−100ΔA−1][ab−CA−1a])∝exp⁡(−12(a⊤A−1a)+(b−CA−1a)⊤ΔA−1(b−CA−1a))∝exp⁡(−12a⊤A−1a)⏟p(a)exp⁡(−12(b−CA−1a)⊤ΔA−1(b−CA−1a))⏟p(b∣a)\begin{aligned} &P(a, b)=P(a) P(b \mid a) \propto \exp \left(-\frac{1}{2}\left[\begin{array}{l} a \\ b \end{array}\right]^{\top}\left[\begin{array}{cc} A & C^{\top} \\ C & D \end{array}\right]^{-1}\left[\begin{array}{l} a \\ b \end{array}\right]\right)\\ &\propto \exp \left(-\frac{1}{2}\left[\begin{array}{l} a \\ b \end{array}\right]^{\top}\left[\begin{array}{cc} A & C^{\top} \\ C & D \end{array}\right]^{-1}\left[\begin{array}{l} a \\ b \end{array}\right]\right)\\ &\propto \exp \left(-\frac{1}{2}\left[\begin{array}{c} a \\ b \end{array}\right]^{\top}\left[\begin{array}{cc} I & -A^{-1} C^{\top} \\ 0 & I \end{array}\right]\left[\begin{array}{cc} A^{-1} & 0 \\ 0 & \Delta_{A}^{-1} \end{array}\right]\left[\begin{array}{cc} I & 0 \\ -C A^{-1} & I \end{array}\right]\left[\begin{array}{l} a \\ b \end{array}\right]\right)\\ &\propto \exp \left(-\frac{1}{2}\left[\begin{array}{lll} a^{\top} & \left(b-C A^{-1} a\right)^{\top} \end{array}\right]\left[\begin{array}{cc} A^{-1} & 0 \\ 0 & \Delta_{\mathbf{A}}^{-1} \end{array}\right]\left[\begin{array}{c} a \\ b-C A^{-1} a \end{array}\right]\right)\\ &\propto \exp \left(-\frac{1}{2}\left(a^{\top} A^{-1} a\right)+\left(b-C A^{-1} a\right)^{\top} \Delta_{\mathrm{A}}^{-1}\left(b-C A^{-1} a\right)\right)\\ &\propto \underbrace{\exp \left(-\frac{1}{2} a^{\top} A^{-1} a\right)}_{p(a)} \underbrace{\exp \left(-\frac{1}{2}\left(b-C A^{-1} a\right)^{\top} \Delta_{\mathbf{A}}^{-1}\left(b-C A^{-1} a\right)\right)}_{p(b\mid a)} \end{aligned}​P(a,b)=P(a)P(b∣a)∝exp(−21​[ab​]⊤[AC​C⊤D​]−1[ab​])∝exp(−21​[ab​]⊤[AC​C⊤D​]−1[ab​])∝exp(−21​[ab​]⊤[I0​−A−1C⊤I​][A−10​0ΔA−1​​][I−CA−1​0I​][ab​])∝exp(−21​[a⊤​(b−CA−1a)⊤​][A−10​0ΔA−1​​][ab−CA−1a​])∝exp(−21​(a⊤A−1a)+(b−CA−1a)⊤ΔA−1​(b−CA−1a))∝p(a)exp(−21​a⊤A−1a)​​p(b∣a)exp(−21​(b−CA−1a)⊤ΔA−1​(b−CA−1a))​​​

得 p(a)∝exp⁡(−12a⊤A−1a)∼N(0,A)p(a)\propto \exp \left(-\frac{1}{2} a^{\top} A^{-1} a\right) \sim N(0,A)p(a)∝exp(−21​a⊤A−1a)∼N(0,A)协方差为 AAA;得 p(b∣a)∝exp⁡(−12(b−CA−1a)⊤ΔA−1(b−CA−1a))∼N(CA−1a,ΔA)p(b\mid a) \propto \exp \left(-\frac{1}{2}\left(b-C A^{-1} a\right)^{\top} \Delta_{\mathbf{A}}^{-1}\left(b-C A^{-1} a\right)\right) \sim N(C A^{-1} a,\Delta_{\mathbf{A}})p(b∣a)∝exp(−21​(b−CA−1a)⊤ΔA−1​(b−CA−1a))∼N(CA−1a,ΔA​)协方差为 ΔA\Delta_{\mathbf{A}}ΔA​;

于是利用舒尔补我们求出了协方差中的边际概率P(a)和条件概率P(b|a)，对于信息矩阵来说，利用舒尔补可以得到：(协方差矩阵各块和信息矩阵之间的对应关系)

由上式我们可以知道 ΔA−1=Λbb\Delta_{\mathbf{A}}^{-1} = \boldsymbol\ Λ_{bb}ΔA−1​=Λbb​ , 通过矩阵消元，我们得 A−1=Λaa−ΛabΛbb−1ΛbaA^{-1} = \boldsymbol\ Λ_{aa}-\boldsymbol\ Λ_{ab}\boldsymbol\ Λ_{bb}^{-1}\boldsymbol\ Λ_{ba}A−1=Λaa​−Λab​Λbb−1​Λba​;边际概率 p(a)p(a)p(a)协方差为 AAA，它的信息矩阵Λp(a)=A−1=Λaa−ΛabΛbb−1Λba\boldsymbol\ Λ_{p(a)}=A^{-1}=\boldsymbol\ Λ_{aa}-\boldsymbol\ Λ_{ab}\boldsymbol\ Λ_{bb}^{-1}\boldsymbol\ Λ_{ba}Λp(a)​=A−1=Λaa​−Λab​Λbb−1​Λba​;

条件概率 p(b∣a)p(b|a)p(b∣a)协方差为 ΔA\Delta_{\mathbf{A}}ΔA​，它的信息矩阵Λp(b∣a)=ΔA−1=Λbb\boldsymbol\ Λ_{p(b|a)}=\Delta_{\mathbf{A}}^{-1}=\boldsymbol\ Λ_{bb}Λp(b∣a)​=ΔA−1​=Λbb​;

理论应用（边缘某个变量之后，信息矩阵变换）：

系统联合分布 P(x1,x2,x3)P(x_1,x_2,x_3)P(x1​,x2​,x3​)的信息矩阵如下：（可以通过协方差的定义获得系统协方差矩阵，然后使用多元高斯分布概率形式获得联合分布的概率公式，即可以得到信息矩阵Σ−1\mathbf{\Sigma}^{-1}Σ−1，见上面的例【计算联合高斯分布从而得到协方差矩阵的逆】）

边缘化变量x3x_3x3​

从联合分布 P(x1,x2,x3)P(x_1,x_2,x_3)P(x1​,x2​,x3​) 中 marg 掉变量 x3x_3x3​,即P(x1,x2)P(x_1,x_2)P(x1​,x2​) 对应的信息矩阵为：

对比信息矩阵Σ−1\mathbf{\Sigma}^{-1}Σ−1，成功的去掉了有关蓝色的x3x_3x3​的信息；得到 x1,x2x_1,x_2x1​,x2​新对应的信息矩阵;总结：边际概率对于协方差矩阵的操作是很容易的，但不好操作信息矩阵；Λaa\boldsymbol\ Λ_{aa}Λaa​

条件概率恰好相反，对于信息矩阵容易操作，不好操作协方差矩阵；

如：边际概率 p(a)p(a)p(a)协方差为 AAA，它的信息矩阵Λp(a)=A−1=Λaa−ΛabΛbb−1Λba\boldsymbol\ Λ_{p(a)}=A^{-1}=\boldsymbol\ Λ_{aa}-\boldsymbol\ Λ_{ab}\boldsymbol\ Λ_{bb}^{-1}\boldsymbol\ Λ_{ba}Λp(a)​=A−1=Λaa​−Λab​Λbb−1​Λba​;

条件概率 p(b∣a)p(b|a)p(b∣a)协方差为 ΔA\Delta_{\mathbf{A}}ΔA​，它的信息矩阵Λp(b∣a)=ΔA−1=Λbb\boldsymbol\ Λ_{p(b|a)}=\Delta_{\mathbf{A}}^{-1}=\boldsymbol\ Λ_{bb}Λp(b∣a)​=ΔA−1​=Λbb​;

一. 滑动窗口算法

图优化：

圆圈：表示顶点，需要优化估计的变量.边：表示顶点之间构建的残差。有一元边（只连一个顶点），二元边，多元边…

例：有如下最小二乘系统，对应的图模型如上图所示:

ξ=argmin⁡ξ12∑i∥ri∥Σi2\boldsymbol{\xi}=\underset{\boldsymbol{\xi}}{\operatorname{argmin}} \frac{1}{2} \sum_{i}\left\|\mathbf{r}_{i}\right\|_{\boldsymbol{\Sigma}_{i}}^{2}ξ=ξargmin​21​i∑​∥ri​∥Σi​2​

其中ξ=[ξ1ξ2⋯ξ6],r=[r12r13r14r15r56]\boldsymbol{\xi}=\left[\begin{array}{l} \boldsymbol{\xi}_{1} \\ \boldsymbol{\xi}_{2} \\ \cdots \\ \boldsymbol{\xi}_{6} \end{array}\right], \mathbf{r}=\left[\begin{array}{l} \mathbf{r}_{12} \\ \mathbf{r}_{13} \\ \mathbf{r}_{14} \\ \mathbf{r}_{15} \\ \mathbf{r}_{56} \end{array}\right]ξ=⎣⎢⎢⎡​ξ1​ξ2​⋯ξ6​​⎦⎥⎥⎤​,r=⎣⎢⎢⎢⎢⎡​r12​r13​r14​r15​r56​​⎦⎥⎥⎥⎥⎤​

上述最小二乘问题，对应的高斯牛顿求解为：

J⊤Σ−1J⏟HorΛδξ=−J⊤Σ−1r⏟b\underbrace{\mathbf{J}^{\top} \boldsymbol{\Sigma}^{-1} \mathbf{J}}_{\mathbf{H} \text { or } \mathbf{\Lambda}} \delta \boldsymbol{\xi}=\underbrace{-\mathbf{J}^{\top} \boldsymbol{\Sigma}^{-1} \mathbf{r}}_{\mathbf{b}}HorΛJ⊤Σ−1J​​δξ=b−J⊤Σ−1r​​

注意，这里的 Λ=Σnew−1≠Σ−1\mathbf{\Lambda}= \Sigma^{-1}_{new} \neq \Sigma^{-1}Λ=Σnew−1​​=Σ−1 ,因为 Λ\mathbf{\Lambda}Λ 反应的是求解的方差，而Σ−1\Sigma^{-1}Σ−1反应的是残差的方差。公式中的雅克比矩阵为：

矩阵乘法公式可以写成累加:

∑i=15Ji⊤Σi−1Jiδξ=−∑i=15J⊤Σi−1r\sum_{i=1}^{5} \mathbf{J}_{i}^{\top} \boldsymbol{\Sigma}_{i}^{-1} \mathbf{J}_{i} \delta \boldsymbol{\xi}=-\sum_{i=1}^{5} \mathbf{J}^{\top} \boldsymbol{\Sigma}_{i}^{-1} \mathbf{r}i=1∑5​Ji⊤​Σi−1​Ji​δξ=−i=1∑5​J⊤Σi−1​r

雅克比矩阵，信息矩阵的稀疏性

将五个残差的信息矩阵加起来，得到样例最终的信息矩阵 Λ, 可视化如下：

可以看到，整个ΛΛ的结构是稀疏的，这是由于每个顶点并不是和其他每个顶点都连接，于是会有0的块。

利用边际概率移除老的变量

直接丢弃变量和对应的测量值，会损失信息。正确的做法是使用边际概率，将丢弃变量所携带的信息传递给剩余变量。(利用上面讲的舒尔补能从多元高斯分布P(a,b) 中分解得到边际概率 p(a) 和条件概率 p(b|a))

例：边缘移除变量 ξ1\xi_1ξ1​

但是 marginalization 会使得信息矩阵变稠密！原先条件独立的变量，可能变得相关。

例：随着 xt+1x_{t+1}xt+1​的进入，变量 xtx_{t}xt​ 被移除

在这里插入图片描述

滑动窗口算法导致的问题

滑动窗口算法优化的时候，信息矩阵变成了两部分（一部分为marg变量之后的矩阵 +加上新加入的变量构建的残差对应的信息矩阵），且这两部分计算雅克比时的线性化点不同。这可能会导致信息矩阵的零空间发生变化，从而在求解时引入错误信息

例：

多个解的问题，变成了一个确定解。不可观的变量，变成了可观的

即：对于 SLAM 系统而言（如单目 VO），当我们改变状态量时，测量不变意味着损失函数不会改变，更意味着求解最小二乘时对应的信息矩阵 Λ 存在着零空间。

单目 SLAM 系统 7 自由度不可观：6 自由度姿态 + 尺度；

单目 + IMU 系统是 4 自由度不可观：yaw 角 + 3 自由度位置不可观。roll 和 pitch 由于重力的存在而可观，尺度因子由于加速度计的存在而可观；

解决办法：First Estimated JacobianFEJ 算法

滑动窗口中的 FEJ 算法

滑动窗口算法中，对于同一个变量，不同残差对其计算雅克比矩阵时线性化点可能不一致，导致信息矩阵可以分成两部分，相当于在信息矩阵中多加了一些信息，使得其零空间出现了变化。

FEJ 算法：不同残差对同一个状态求雅克比时，线性化点必须一致。这样就能避免零空间退化而使得不可观变量变得可观

比如: toy example 3 中计算 r27r_{27}r27​ 对 ξ2\xi_2ξ2​ 的雅克比时， ξ2\xi_2ξ2​ 的线性话点必须和 r12r_{12}r12​ 对其求导时一致。（不懂？ 后续再补吧）

从零开始学习VIO笔记 --- 第四讲：滑动窗口（基于滑动窗口算法的 VIO 系统：可观性和一致性）