VINS-Mono 阅读

VINS-Mono: A Robust and Versatile Monocular
Visual-Inertial State Estimator

Abstract

本文介绍了一种单目-视觉惯性状态估计，在平面上运动。单目图像和imu的组合提供了一种最少的传感器组合，三自由度的状态估计。我们将视觉和惯性在一个窗口中计算。

本系统在各个方面都比较鲁邦，比如：状态未知条件下的初始化、在线标定、闭环检测、四自由度的pose graph。这些性质让本系统简单易用。

1. Overview

本系统分为三个步骤

• 视觉IMU预处理
• 初始化过程
• 非线性优化

表示方法：$({)}^w$表示世界坐标系、$({)}^b$表示body坐标系，$({)}^c$表示cam坐标系

2. 观测预处理

A. 视觉处理比较简单
B. IMU 预积分
这里有一个疑问，貌似这里把body系下的加速度，直接乘以旋转矩阵，就到了全局系
$$p_{b_{k+1}}^w=p_{b_k}^w+v_{b_k}^w+\int \int R_t^w\ast {\hat{a}}_{t}d{t}^2$$
$$v_{b_{k+1}}^w=v_{b_k}^w+\int R_t^w\ast {\hat{a}}_{t}dt$$
$$q_{b_{k+1}}^w=q_{b_k}^w\otimes \int 1/2\Omega (\hat{w})q_t^{b_k}dt$$
因为观察到上面的式子是依赖$b_k$的，在公式两边同时乘以$R_w^{b_k}$， 将上面的式子转换到局部坐标系下：
$$R_w^{b_k}{p}_{b_{k+1}}^w=R_w^{b_k}(p_{b_k}^w+v_{b_k}^w)+R_w^{b_k}\int \int R_t^w\ast {\hat{a}}_{t}d{t}^2$$
$$R_w^{b_k}{v}_{b_{k+1}}^w=R_w^{b_k}{v}_{b_k}^w+R_w^{b_k}\int R_t^w\ast {\hat{a}}_{t}dt$$
$$q_w^{b_k}{q}_{b_{k+1}}^w=q_w^{b_k}{q}_{b_k}^w\otimes \int 1/2\Omega (\hat{w})q_t^{b_k}dt$$

因此得到预积分的三个公式：
$${\alpha }_{b_{k+1}}^{b_k}=\int \int R_t^{b_k}\ast {\hat{a}}_{t}d{t}^2$$
$${\beta }_{b_{k+1}}^{b_k}=\int R_t^{b_k}\ast {\hat{a}}_{t}dt$$
$${\gamma }_{b_{k+1}}^{b_k}=\int 1/2\Omega (\hat{w})q_t^{b_k}dt$$

上面的式子定义的递推公式，可以看成是只需要使用imu，将$b_k$当做是基础pose，就可以完全得到的结果。从下往上，可以求出$\alpha ,\beta ,\gamma$的所有结果。

对于离线形式，因为都是积分，其实就是累加的形式得到最终的结果。
$${\alpha }_{i+1}^{b_k}={\alpha }_i^{b_k}+$$
公式5中的$\alpha$, 为什么会和$\beta$有关系？有关系的，通过下面的实例可以验证。

这里可以使用一个实际的例子来进行验证。初速度=0，以加速度1m/s^2向前移动，对v和s进行积分，结果一致。

从这个例子可以看出，这三个预积分，其实对应的就是这一段的速度增量、距离增量、角度增量。

有了观测量之后，在后续的使用过程中，需要使用该观测量的协方差。这里因为假设没有bias，所以误差的来源就是角速度和加速度的高斯误差。

因为这里的观测量变成了$\alpha ,\beta ,\gamma$, 所以要通过协方差传播公式来传递。

又因为是离散的，所以又需要求一下每一个delta量的协方差。求得协方差的迭代公式。协方差迭代公式，就是利用偏导的平方进行传播的。

notes：为了简化公式推导，假设没有bias,没有重力。pose的移动在一个平面上发生。