【VIO】第3讲基于滑动窗口算法的VIO

最新推荐文章于 2026-04-16 11:26:00 发布

原创最新推荐文章于 2026-04-16 11:26:00 发布 · 663 阅读

0 ·

本内容遵循CC 4.0 BY-SA版权协议

标签

#算法 #人工智能

VIO 专栏收录该内容

4 篇文章

订阅专栏

本文探讨了SLAM中的信息矩阵概念及其稀疏性，介绍了基于边际概率的滑动窗口算法，并讨论了FEJ算法如何解决滑动窗口带来的问题。

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

（1）SLAM 问题概率建模

具体参见：https://blog.csdn.net/ASUNAchan/article/details/124654207?spm=1001.2014.3001.5501

（2）高斯分布和协方差矩阵

1）多元高斯分布

零均值高斯分布：
$\frac{1}{Z}e^{-\frac{1}{2}x^T\Sigma^{-1} x}$
其中： $Σ\Sigma$ 为协方差矩阵，协方差矩阵的逆记作 $Λ=Σ−1\Lambda = \Sigma^{-1}$ ， $Σij=E(xixj)\Sigma_{ij} = E(x_ix_j)$ 为对应元素求期望。

2）例1
$x_2 = v_2 \\ x_1 = w_1 x_2 + v_1 \\ x_3 = w_3 x_2 + v_3$
推导过程略

结论：

1 协方差逆矩阵中如果坐标为 $(i, j)$ 的元素为 0，表示元素 $i$ 和 $j$ 关于其他变量条件独立

2 协方差中非对角元素 $Σij>0\Sigma_{ij} > 0$ 表示两变量是正相关

3 信息矩阵中非对角元素为负数，甚至为 0。 $Λ12<0\Lambda_{12} < 0$ 表示在变量 $x_3$ 发生的条件下，元素 $x_1$ 和 $x_2$ 正相关。

3）例2
$x_2 = w_1 x_1 + w_3 x_3 + v_2$

2.舒尔补应用：边际概率, 条件概率

（1）舒尔补的概念

给定任意的矩阵块 M：
$\left[ \begin{matrix} A & B \\ C & D \end{matrix} \right]$
如果，矩阵块 D 是可逆的，则 $A−BD^{−1} C$ 称之为 D 关于 M 的舒尔补。
$\left[ \begin{matrix} I & -BD^{-1} \\ 0 & I \end{matrix} \right] \left[ \begin{matrix} A & B \\ C & D \end{matrix} \right] \left[ \begin{matrix} I & 0 \\ -D^{-1}C & I \end{matrix} \right] =\left[ \begin{matrix} A-BD^{-1}C & 0 \\ 0 & D \end{matrix} \right]$
如果，矩阵块 A 是可逆的，则 $D − CA^{−1} B$ 称之为 A 关于 M 的舒尔补。
$\left[ \begin{matrix} I & 0 \\ -CA^{-1} & I \end{matrix} \right] \left[ \begin{matrix} A & B \\ C & D \end{matrix} \right] \left[ \begin{matrix} I & -A^{-1}B \\ 0 & I \end{matrix} \right] =\left[ \begin{matrix} A & 0 \\ 0 & D-CA^{-1}B \end{matrix} \right]$
然后，从对角形恢复M矩阵
$\left[ \begin{matrix} I & BD^{-1} \\ 0 & I \end{matrix} \right] \left[ \begin{matrix} A-BD^{-1}C & 0 \\ 0 & D \end{matrix} \right] \left[ \begin{matrix} I & 0 \\ D^{-1}C & I \end{matrix} \right] =\left[ \begin{matrix} A & B \\ C & D \end{matrix} \right]$

$\left[ \begin{matrix} I & 0 \\ CA^{-1} & I \end{matrix} \right] \left[ \begin{matrix} A & 0 \\ 0 & D-CA^{-1}B \end{matrix} \right] \left[ \begin{matrix} I & A^{-1}B \\ 0 & I \end{matrix} \right] =\left[ \begin{matrix} A & B \\ C & D \end{matrix} \right]$

所以M矩阵的逆矩阵 $M^{-1}$ 为：
$\left[ \begin{matrix} A & B \\ C & D \end{matrix} \right]^{-1} =\left[ \begin{matrix} I & 0 \\ -D^{-1}C & I \end{matrix} \right] \left[ \begin{matrix} (A-BD^{-1}C)^{-1} & 0 \\ 0 & D^{-1} \end{matrix} \right] \left[ \begin{matrix} I & -BD^{-1} \\ 0 & I \end{matrix} \right]$

$\left[ \begin{matrix} A & B \\ C & D \end{matrix} \right]^{-1} = \left[ \begin{matrix} I & -A^{-1}B \\ 0 & I \end{matrix} \right] \left[ \begin{matrix} A^{-1} & 0 \\ 0 & (D-CA^{-1}B)^{-1} \end{matrix} \right] \left[ \begin{matrix} I & 0 \\ -CA^{-1} & I \end{matrix} \right]$

（2）舒尔补应用于多元高斯分布

假设多元变量x服从高斯分布，且由两部分组成：
$x = [a, b]^T$
变量之间的协方差矩阵：
$\left[ \begin{matrix} A & C^T \\ C & D \end{matrix} \right]$
其中： $A = co v (a, a)$ ， $D = co v (b, b)$ ， $C = co v (a, b)$

所以：
$\propto exp( -\frac{1}{2} \left[ \begin{matrix} a & b \end{matrix} \right] \left[ \begin{matrix} A & C^T \\ C & D \end{matrix} \right]^{-1} \left[ \begin{matrix} a \\ b \end{matrix} \right] )$
使用舒尔补的公式对高斯分布分解：
$p(a,b)\\ \propto exp( \frac{1}{2}[(a^TA^{-1}a) + (b-CA^{-1}a)^T\Delta_A^{-1}(b-CA^{-1}a)] )\\ \propto exp( \frac{1}{2}a^TA^{-1}a)\; exp [\frac{1}{2}(b-CA^{-1}a)^T\Delta_A^{-1}(b-CA^{-1}a) ]$
其中：
$p(b|a)\sim N(CA^{-1}a, D−CA^{−1} B)\\ p(a) \sim N(0,A)$
$p (a), p (b ∣ a)$ 的信息矩阵：
在这里插入图片描述

由条件概率 $p (b ∣ a)$ 的协方差为 $ΔA\Delta_A$ 及上述公式，得信息矩阵 $ΔA−1=Λbb\Delta_A^{-1} = \Lambda_{bb}$

由边际概率 $p (a)$ 的协方差为 $A$ 及上述公式，得信息矩阵 $A−1=Λaa−ΛabΛbb−1ΛbaA^{-1} = \Lambda_{aa} - \Lambda_{ab}\Lambda_{bb}^{-1}\Lambda_{ba}$

（3）总结

边际概率对于协方差矩阵的操作是很容易的，但不好操作信息矩阵。条件概率恰好相反，对于信息矩阵容易操作，不好操作协方差矩阵。
在这里插入图片描述

3.滑动窗口算法

（1）例子

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

$\xi = arg\;min_{\xi}\;\frac{1}{2}\sum_i ||r_i||^2_{\Sigma_i}$
其中： $,ξ6]\xi = [\xi_1, \xi_2, \cdots, \xi_6]$ ^T， $r=[r_{12},r_{13},r_{14},r_{15},r_{56}]^T$

针对上述最小二乘问题，对应高斯牛顿求解：
$J^T\Sigma^{-1}J\Delta \xi = -J^T\Sigma^{-1}r$

所以可以将公式写成：
$\sum_{i=1}^{5}J_i^T\Sigma_i^{-1}J_i\Delta \xi = -\sum_{i=1}^{5}J_i^T\Sigma_i^{-1}r$

（2）信息矩阵的稀疏性

由于每个残差只和某几个状态量有关，因此，雅克比矩阵求导时，无关项的雅克比为 0。比如：
$J_2 = \frac{\partial r_{13}}{\partial \xi} = [\frac{\partial r_{13}}{\partial \xi_1}, 0, \frac{\partial r_{13}}{\partial \xi_3}, 0, 0, 0 ]$

$\Lambda_2 = J_2^T\Sigma_2^{-1}J_2$

同理，可以计算 $\Lambda_1 , \Lambda_3 , \Lambda_4 ,\Lambda_5 $，并且也是稀疏的。

可以结合十四讲第9章以及第8章直接法部分来更好理解。

（3）基于边际概率的滑动窗口算法

1 为什么 SLAM 需要滑动窗口算法?

随着 VSLAM 系统不断往新环境探索，就会有新的相机姿态以及看到新的环境特征，最小二乘残差就会越来越多，信息矩阵越来越大，计算量将不断增加。

为了保持优化变量的个数在一定范围内，需要使用滑动窗口算法动态增加或移除优化变量。

2 滑动窗口算法大致流程

增加新的变量进入最小二乘系统优化；如果变量数目达到了一定的维度，则移除老的变量。SLAM 系统不断循环前面两步。

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

直接丢弃变量和对应的测量值，会损失信息。正确的做法是使用边际概率，将丢弃变量所携带的信息传递给剩余变量。

如果是直接丢弃，信息矩阵如何变化？用边际概率来操作又会带来什么问题？

（4）例子

如下图优化系统中，随着 $x_{t+1}$ 的进入，变量 $x_t$ 被移除。

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

4.滑动窗口中的 FEJ 算法

（1）回顾例子

1 如图所示，在 $t∈[0,k]st\in[0, k]s$ 时刻, 系统中状态量为 $ξi,i∈[1,6]\xi_i , i \in [1, 6]$ 。第 $k^′$ 时刻，加入新的观测和状态量 $ξ7\xi_7$

2 在第 k 时刻，最小二乘优化完以后，marg 掉变量 $ξ1\xi_1$ 。被 marg 的状态量记为 $x_m$ , 剩余的变量 $ξi,i∈[2,5]\xi_i , i ∈ [2, 5]$ 记为 $x_r$ 。

3 marg 发生以后， $x_m$ 所有的变量以及对应的测量将被丢弃。同时，这部分信息通过 marg 操作传递给了保留变量 $x_r$ ，marg 变量的信息跟 $ξ6\xi_6$ 不相关。

4 第 $k^′$ 时刻，加入新的状态量 $ξ7\xi_7$ (记作 $x_n$ ) 以及对应的观测，开始新一轮最小二乘优化。

（2）marg 前后

已知的是：
$\sum_{i=1}^{5}J_i^T\Sigma_i^{-1}J_i\Delta \xi = -\sum_{i=1}^{5}J_i^T\Sigma_i^{-1}r$
marg 前，变量 $x_m$ 以及对应测量 $S_m$ 构建的最小二乘信息矩阵为：
在这里插入图片描述

marg 后，变量 $x_m$ 的测量信息传递给了变量 $x_r$
$b_p(k) = b_{mr}(k) -\Lambda_{rm}(k)\Lambda_{mm}(k)^{-1}b_{mm}(k)$

$\Lambda_p(k) = \Lambda_{rr}(k) -\Lambda_{rm}(k)\Lambda_{mm}^{-1}(k)\Lambda_{mr}(k)$

下标 p 表示 prior. 即这些信息将构建一个关于 $x_r$ 的先验信息。

我们可以从 $b_p(k)$ , $Λp(k)\Lambda_p(k)$ 中反解出一个残差 $r_p(k)$ 和对应的雅克比矩阵 $J_p(k)$ . 需要注意的是，随着变量 $x_r(k)$ 的后续不断优化变化，残差 $r_p(k)$ 或者 $b_p(k)$ 也将跟着变化，但雅克比 $J_p(k)$ 则固定不变了。？？？

（3）新测量信息和旧测量信息构建新的系统

在 $k^′$ 时刻，新残差 $r_{27}$ 和先验信息 $b_p(k)$ , $Λp(k)\Lambda_p(k)$ 以及残差 $r_{56}$ 构建新的最小二乘问题：
$b(k^\prime) = \Pi^T b_{p}(k) -\sum_{(i,j)\in S_a(k^\prime)} J_{ij}(k^\prime)^T\Sigma_{ij}^{-1}r_{ij}(k^\prime)$