VIO（4）—— 基于滑动窗口算法的 VIO 系统：舒尔布、边缘化、可观性和 一致性

@

目录

• 一、从高斯分布到信息矩阵
  • 1.1 SLAM 问题概率建模
  • 1.2 SLAM 问题求解
  • 1.3 高斯分布和协方差矩阵
  • 1.4 样例
    • 1.4.1 样例1
    • 1.4.2 样例2
• 二、舒尔补应用：边际概率, 条件概率
  • 2.1 舒尔补的概念
  • 2.2 舒尔补的来由
  • 2.3 使用舒尔补分解的好处
  • 2.4 舒尔补应用于多元高斯分布
  • 2.5 关于 P(a), P(b|a) 的协方差矩阵
  • 2.6 关于 P(a), P(b|a) 的信息矩阵
  • 2.7 回顾样例
  • 2.8 总结
  • 三、滑动窗口算法
  • 3.1 最小二乘用图表示
  • 3.2 最小二乘问题信息矩阵的构成
  • 3.3 信息矩阵的稀疏性
  • 3.4 信息矩阵组装过程的可视化
  • 3.5 基于边际概率的滑动窗口算法
  • 3.6 样例
• 四、滑动窗口中的 FEJ 算法
  • 4.1 新测量信息和旧测量信息构建新的系统
  • 4.2 信息矩阵的零空间变化
  • 4.3 可观性的一种定义

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

1.1 SLAM 问题概率建模


1.2 SLAM 问题求解


1.3 高斯分布和协方差矩阵

因为一般可以假设\(x_{i}和 x_{j}\)是相互独立的：
\(\Sigma_{i j}=E\left(x_{i} x_{j}\right)=E(x_i)E(x_j)=(x-u)^T(x-u)\)

1.4 样例

1.4.1 样例1

因为实际过程中是协方差矩阵里面各个值是一个数，已经没有办法单独去掉某一部分。

1.4.2 样例2


二、舒尔补应用：边际概率, 条件概率

2.1 舒尔补的概念

更多定义参见：Wiki. Schur Complement.

2.2 舒尔补的来由

同理舒尔补还可以写成另一种形式

\[\left[\begin{array}{cc} \mathrm{I} & \mathrm{BD}^{-1} \\ 0 & \mathrm{I} \end{array}\right]\left[\begin{array}{cc} \Delta_{\mathrm{D}} & 0 \\ 0 & \mathrm{D} \end{array}\right]\left[\begin{array}{cc} \mathrm{I} & 0 \\ \mathrm{D}^{-1} \mathrm{C} & \mathrm{I} \end{array}\right]=\left[\begin{array}{cc} \mathrm{A} & \mathrm{B} \\ \mathrm{C} & \mathrm{D} \end{array}\right] \]

\[\Delta_{\mathrm{D}} =A-BD^{-1}C \]

2.3 使用舒尔补分解的好处


2.4 舒尔补应用于多元高斯分布


2.5 关于 P(a), P(b|a) 的协方差矩阵


2.6 关于 P(a), P(b|a) 的信息矩阵


2.7 回顾样例


2.8 总结


三、滑动窗口算法

3.1 最小二乘用图表示


3.2 最小二乘问题信息矩阵的构成


3.3 信息矩阵的稀疏性


3.4 信息矩阵组装过程的可视化


3.5 基于边际概率的滑动窗口算法

公式表示：
假设要被边缘化的状态是\(\delta x_a\)
因为在实际滑窗中\(\delta x_a\)的状态已经被移出去了，所以不会再产生约束，所以只展开矩阵第二行。
可以看到新的方程只和\(\delta x_b\)相关，但是\(\delta x_a\)的信息又被保留了下来。接下来只需把最后的公式重写分解成以下形式就又形成了常见后端中的边缘化约束

\[\underbrace{\mathbf{J}^{\top} \mathbf{J}}_{\mathbf{H} \text { or } \boldsymbol{\Lambda}} \delta \boldsymbol{\xi}=\underbrace{-\mathbf{J}^{\top} \mathbf{r}}_{\mathbf{b}} \]

对H矩阵作特征值分解 \(H = V\Sigma V^T\).\(V是特征向量,\Sigma是特征值构成的对角矩阵\) 同时又\(H = J^TJ\)，所以 \(J = \sqrt{\Sigma}V^T\)
同理\(r = -(J^T)^{-1}*b\)

3.6 样例

详细步骤如下：
左边为因子图，右边为其对应的H矩阵
如果边缘化掉pose1会发生什么？

四、滑动窗口中的 FEJ 算法


4.1 新测量信息和旧测量信息构建新的系统


4.2 信息矩阵的零空间变化


4.3 可观性的一种定义