VIN2.IMU预积分

0.引言

搬运工+自己阅读代码理解。

• 参考说明

pipeline如图：

本节则是阅读总结IMU(100hz)-->IMU Pre-integration部分。processIMU()函数、integration_base.h-->IntegrationBase类。

1.IMU测量模型

忽略地球旋转,IMU 测量方程为：
$$\begin{array}{c}{a}_t=a_{t(real)}+b_{a_t}+R_w^t{g}^w+n_a\\ w_t=w_{t(real)}+b_{w_t}+n_w\end{array}\text{\hspace{1em}(1)}$$上标 $g$ 表示gyro,$a$ 表示acc,$w$表示在世界坐标系world,$b$ 表示imu机体坐标系body.imu的真实值为 ω, a 测量值为$\omega$、$a$;测量值为$\tilde{\omega },\tilde{\mathbf{a}}$。

IMU 的测量信息是在body坐标系(即 IMU 坐标)中获取，假设噪声$n_a$和$n_w$服从高斯分布:

$$\begin{array}{rl}{n}_a& \sim N\left(0,{\sigma }_a^2\right)\\ n_w& \sim N\left(0,{\sigma }_w^2\right)\end{array}\text{\hspace{1em}(2)}$$
加速度偏置$b_{a_t}$和陀螺仪偏置$b_{w_t}$被建模为随机游走:
$${\dot{b}}_{a_i}=n_{b_a}$$$${\dot{b}}_{w_i}=n_{b_w}\text{\hspace{1em}(3)}$$其中$n_{b_a}$和$n_{b_w}$服从高斯分布:
$$\begin{array}{rl}{n}_{b_a}& \sim N\left(0,{\sigma }_{b_a}^2\right)\\ n_{b_w}& \sim N\left(0,{\sigma }_{b_w}^2\right)\end{array}\text{\hspace{1em}(4)}$$
既是其导数服从高斯分布！

2.当前时刻 PVQ 的连续形式

对于连续两个关键帧 $b_k$ 和$b_{k+1}$,它们对应的时刻分别为 $t_k,t_{k+1}$ 。可以根据 $\left[t_k,t_{k+1}\right]$时间间隔内 IMU 的测量值,对系统的位置、速度和旋转等状态进行传播(注意这里的四元数采用了实部在后,虚部在前的形式):
$$p_{b_{k+1}}^w=p_{b_k}^w+v_{b_k}^w\Delta t_k+{\iint }_{t\in \left[t_k,t_{k+1}\right]}\left(R_t^w\left(a_t-b_{a_i}-n_a\right)-g^w\right)d{t}^2\text{\hspace{1em}(5)}$$$$v_{b_{k+1}}^w=v_{b_k}^w+{\int }_{t\in \left[t_k,t_{k+1}\right]}\left(R_t^w\left(a_t-b_{a_i}-n_a\right)-g^w\right)dt\text{\hspace{1em}(6)}$$$$q_{b_{k+1}}^w=q_{b_k}^w\otimes {\int }_{t\in \left[t_k,t_{k+1}\right]}\frac{1}{2}{q}_t^{b_k}\otimes \left(w_t-b_{w_i}-n_w\right)dt$$$$=q_{b_k}^w\otimes {\int }_{t\in \left[t_k,t_{k+1}\right]}\frac{1}{2}\Omega \left(w_t-b_{w_t}-n_w\right)q_t^{b_k}dt\text{\hspace{1em}(7)}$$其中：$$\Omega (w)=\left[\begin{array}{cc}-[w{]}_{\times }& w\\ -w^T& 0\end{array}\right],[w{]}_{\times }=\left[\begin{array}{ccc}0& -w_z& w_y\\ w_z& 0& -w_x\\ -w_y& w_x& 0\end{array}\right]\text{\hspace{1em}(8)}$$$\Delta t_k$是$\left[t_k,t_{k+1}\right]$之间的时间间隔,$R_t^w$为 $t$ 时刻从body坐标系到世界坐标系的旋转矩阵,$q_t^{b_k}$为用四元数表示的 $t$ 时刻从body坐标系到世界坐标系的旋转。$a_t,w_t$为 IMU 测量的加速度和角速度,是在 Body 自身坐标系,world 坐标系是IMU 所在的惯导系。

3.当前时刻 PVQ 的中值法离散形式

前面给出的是连续时刻的相机当前 PVQ 的迭代公式,基于中值法的公式 , 即从第 i 个 IMU 时刻 到第 i+1 个 IMU 时刻的积分过程 ,这与Estimator::processIMU()函数中的 Ps[j]、Rs[j]和 Vs[j]是一致的(代码中的 j 时刻即为此处的i+1),IMU 积分出来的第 j 时刻的物理量可以作为第 j 帧图像的初始值。
$$p_{b_{i+1}}^w=p_{b_i}^w+v_{b_i}^w\delta t+\frac{1}{2}{\overline{a}}_i\delta t^2\text{\hspace{1em}(9)}$$$$v_{b_{i+1}}^w=v_{b_i}^w+{\overline{a}}_i\delta t\text{\hspace{1em}(10)}$$$$q_{b_{i+1}}^w=q_{b_i}^w\otimes \left[\frac{1}{2}{\overline{\omega }}_i\delta t\right]\text{\hspace{1em}(11)}$$其中:$${\overline{a}}_i=\frac{1}{2}\left[q_i\left(a_i-b_{a_i}\right)-g^w+q_{i+1}\left(a_{i+1}-b_{a_i}\right)-g^w\right]\text{\hspace{1em}(12)}$$$${\overline{\omega }}_i=\frac{1}{2}\left({\omega }_i+{\omega }_{i+1}\right)-b_{{\omega }_i}\text{\hspace{1em}(13)}$$
与代码中Estimator::processIMU():一一对应

		~ ~ ~
        int j = frame_count;
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
        Vs[j] += dt * un_acc;

若采用欧拉积分，其中：
$$\mathbf{a}={\mathbf{q}}_{w{b}_k}({\mathbf{a}}^{b_k}-\left({\mathbf{b}}_k^a\right)-{\mathbf{g}}^w\text{\hspace{1em}(14)}$$$$\omega ={\omega }^{b_k}-{\mathbf{b}}_k^g\text{\hspace{1em}(15)}$$这两个等式表达式表达与上面不统一，不影响理解。

4.两帧之间 PVQ 增量的连续形式

IMU的数据频率一般远高于视觉，在视觉两帧k，k+1之间通常会有>10组IMU数据。IMU的数据通过积分，可以获取当前位姿（p位置，q四元数表达的姿态）、瞬时速度等参数。
　　在VIO中，如果参考世界坐标系对IMU进行积分，积分项中包含相对于世界坐标系的瞬时旋转矩阵，这样有几个问题：

1. 相对世界坐标系的旋转矩阵有drift，如果一直以其为基准进行积分，必然造成积分误差累积；
2. 在进行优化位姿调整时（通常是调整视觉KeyFrame的pose），相对于世界坐标系的pose会变化，因而优化后的瞬时旋转矩阵和积分时不同，那么积分自然也就存在问题；
3. 一般这个旋转矩阵不知道。。。

因此，一般的预积分的参考坐标系为k帧的IMU参考系，这样可以解决以上问题：

1. 相对k帧的IMU进行积分，不会有累积误差；
2. 即使后面调整了位姿，相对位置不变，因此预积分不存在问题；
3. 这个旋转矩阵为单位矩阵E，后面每出现一个IMU数据，都可以用任何一种数值积分的方法计算；同时可以将重力加速度提取到积分号外面不参加积分，相当于在重力参考系中积分，计算量也会减少。

注意这里是求的增量，即是在窗口优化过程中的产生的变化$\Delta$.优化状态向量包括滑动窗口内的所有相机状态（位置P、旋转Q、速度V、加速度偏置ba、陀螺仪偏置bw）、相机到IMU的外参、所有3D点的逆深度：
$$\mathcal{X}=\left[{\mathbf{x}}_0,{\mathbf{x}}_1,\cdots {\mathbf{x}}_n,{\mathbf{x}}_c^b,{\lambda }_0,{\lambda }_1,\cdots {\lambda }_m\right]$$$${\mathbf{x}}_k=\left[{\mathbf{p}}_{b_k}^w,{\mathbf{v}}_{b_k}^w,{\mathbf{q}}_{b_k}^w,{\mathbf{b}}_a,{\mathbf{b}}_g\right],k\in [0,n]$$$${\mathbf{x}}_c^b=\left[{\mathbf{p}}_c^b,{\mathbf{q}}_c^b\right]$$
故${\mathbf{q}}_{b_k}^w$随着优化是在改变的，导致公式(5)(6)(7)的积分随着优化需重复计算，耗时耗力，故将积分项转换到body坐标系计算，减少重复。
.　　通过公式(5)(6)(7)可知,IMU 的预积分需要依赖与第 k 帧的 v 和 R,当我们在后端进行非线性优化时,需要迭代更新第 k 帧的 v 和 R,这将导致我们需要根据每次迭代后值重新进行积分,这将非常耗时。因此,我们考虑将优化变量从第 k 帧到第 k+1 帧的 IMU 预积分项b中分离开来,通过对公式(1)左右两侧各乘$R_w^{b_k}$ ,可化简为:

　　从积分式可以看出,系统位置、速度和旋转等状态的传播需要关键帧$b_k$ 时刻的位置$p_{b_k}^w$ 、速度${\nu }_{b_k}^w$和旋转$q_{b_k}^w$,当这些起始状态发生改变时,就需要按照上诉积分式重新进行状态传播。在基于优化的算法中,每个关键帧时刻的状态需要频繁调整,所以就需要频繁地重新积分,这样会浪费大量的计算资源。IMU 预积分就是为了避免这种计算资源上的浪费。考虑将优化变量从第 k 帧到第 k+1 帧的 IMU 预积分项中分离开来,通过对公式(5)(6)(7)左右两侧各乘$R_w^{b_k}$ ,可化简为公式(16)(17)(18)(19)(20)(21).
　　IMU 预积分的思路简单来说,就是将参考坐标系从世界坐标系 $w$调整为第 $k$ 个关键帧时刻的body坐标系 $b_k$ :

　　$$R_w^{b_k}{p}_{b_{k+1}}^w=R_w^{b_k}\left(p_{b_k}^w+v_{b_k}^w\Delta t_k-\frac{1}{2}{g}^w\Delta t_k^2\right)+{\alpha }_{b_{k+1}}^{b_k}\text{\hspace{1em}(16)}$$$$R_w^{b_k}{v}_{b_{k+1}}^w=R_w^{b_k}\left(v_{b_k}^w-g^w\Delta t_k\right)+{\beta }_{b_{k+1}}^{b_k}\text{\hspace{1em}(17)}$$$$q_w^{b_k}\otimes q_{b_{k+1}}^w={\gamma }_{b_{k+1}}^{b_k}\text{\hspace{1em}(18)}$$其中：
$${\alpha }_{b_{k+1}}^{b_k}={\iint }_{t\in \left[t_k,t_{k+1}\right]}\left(R_t^{b_k}\left(a_t-b_{a_i}-n_a\right)\right)d{t}^2\text{\hspace{1em}(19)}$$$${\beta }_{b_{k+1}}^{b_k}={\int }_{t\in \left[t_k,t_{k+1}\right]}\left(R_t^{b_k}\left(a_t-b_{a_t}-n_a\right)\right)dt\text{\hspace{1em}(20)}$$$${\gamma }_{b_{k+1}}^{b_k}={\int }_{t\in \left[t_k,t_{k+1}\right]}\frac{1}{2}\Omega \left(w_t-b_{w_i}-n_w\right){\gamma }_t^{b_k}dt\text{\hspace{1em}(21)}$$ 　　新的积分项(12.13.14)中参考坐标系变成了$b_k$ ,可以理解为这时的积分结果为$b_{k+1}$对于$b_k$ 的相对运动量,即使在优化过程中对关键帧的位置、速度和旋转等状态进行调整,也不对新的积分项产生任何影响,从而避免了重复积分。（从world系转换为body系）

这里需要重新讨论下公式(19)~(21)的预积分公式,以${\alpha }_{b_{k+1}}^{b_k}$为例,发现它是与 IMU 的bias 相关的,而 bias 也是我们需要优化的变量,这将导致的问题是,当每次迭代时,我们得到一个新的 bias,又得根据公式中重新对第 k 帧和第 k+1 帧之间的 IMU 预积分,非常耗时。这里假设预积分的变化量与 bias 是线性关系,可以写成:
$${\alpha }_{b_{k+1}}^{b_k}\approx {\hat{\alpha }}_{b_{k+1}}^{b_k}+J_{b_a}^{\alpha }\delta b_a+J_{b_{\omega }}^{\alpha }\delta b_{\omega }$$$${\beta }_{b_{k+1}}^{b_k}\approx {\hat{\beta }}_{b_{k+1}}^{b_k}+J_{b_a}^{\beta }\delta b_a+J_{b_{\omega }}^{\beta }\delta b_{\omega }\text{\hspace{1em}(22)}$$$${\gamma }_{b_{k+1}}^{b_k}\approx {\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_{\omega }}^{\gamma }\delta b_{\omega }\end{array}\right]$$其中的Jacobi矩阵后面讲。

5.两帧之间 PVQ 增量的离散形式

欧拉法：
下面给出离散时刻的 IMU 预积分公式,首先按照论文中采用的欧拉法,给出第 i 个 IMU时刻与第 i+1 个 IMU 时刻的变量关系为:
$${\hat{\alpha }}_{i+1}^{b_k}={\hat{\alpha }}_i^{b_k}+{\hat{\beta }}_i^{b_k}\delta t+\frac{1}{2}R\left({\hat{\gamma }}_i^{b_k}\right)\left({\hat{a}}_i-b_{a_i}\right)\delta t^2$$$${\hat{\beta }}_{i+1}^{b_k}={\hat{\beta }}_i^{b_k}+R\left({\hat{\gamma }}_i^{b_k}\right)\left({\hat{a}}_i-b_{a_i}\right)\delta t\text{\hspace{1em}(23)}$$$${\hat{\gamma }}_{i+1}^{b_k}={\hat{\gamma }}_i^{b_k}\otimes {\hat{\gamma }}_{i+1}^i={\hat{\gamma }}_i^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\left({\hat{\omega }}_i-b_{{\omega }_i}\right)\delta t\end{array}\right]$$
中值法：
代码中采用的基于中值法的 IMU 预积分公式,这与 Estimator::processIMU()函数中的 IntegrationBase::push_back()上是一致的。特别注意这里跟公式(9)~(13)是不一样的,这里积分出来的是前后两帧之间的 IMU 增量信息,而公式(9)~(13)给出的当前帧时刻的物理量信息。这里相当于在求解优化过程中产生的增量，计算的是增量值。

$${\hat{\alpha }}_{i+1}^{b_k}={\hat{\alpha }}_i^{b_k}+{\hat{\beta }}_i^{b_k}\delta t+\frac{1}{2}{\overline{\hat{a}}}_i^{\prime }\delta t^2$$$${\hat{\beta }}_{i+1}^{b_k}={\hat{\beta }}_i^{b_k}+{\overline{\hat{a}}}_i^{\prime }\delta t\text{\hspace{1em}(24)}$$$${\hat{\gamma }}_{i+1}^{b_k}={\hat{\gamma }}_i^{b_k}\otimes {\hat{\gamma }}_{i+1}^i={\hat{\gamma }}_i^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\overline{\omega }}_i^{\prime }\delta t\end{array}\right]$$其中：$${\overline{\hat{a}}}_i^{\prime }=\frac{1}{2}\left[q_i\left({\hat{a}}_i-b_{a_i}\right)+q_{i+1}\left({\hat{a}}_{i+1}-b_{a_i}\right)\right]$$$${\overline{\hat{\omega }}}_i^{\prime }=\frac{1}{2}\left({\hat{\omega }}_i+{\hat{\omega }}_{i+1}\right)-b_{{\omega }_i}\text{\hspace{1em}(25)}$$

对应代码integration_base.h--> midPointIntegration():

        Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
        Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
        result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
        Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
        result_delta_v = delta_v + un_acc * _dt;
        result_linearized_ba = linearized_ba;
        result_linearized_bg = linearized_bg;         

6.连续形式下 PVQ 增量的误差、协方差及 Jacobian

.　　目前已经完成了IMU预积分测量值的梳理，而要将IMU预积分运用到非线性优化中，需要建立线性高斯误差状态递推方程，由线性高斯系统的协方差，推导方程协方差矩阵，并求解对应的雅可比矩阵。IMU 在每一个时刻积分出来的值都有误差,下面对误差进行分析。预积分误差：一段时间内 IMU 构建的预积分量作为测量值,对两时刻之间的状态量进行约束,

${\left[\begin{array}{l}{\mathbf{r}}_p\\ {\mathbf{r}}_q\\ {\mathbf{r}}_v\\ {\mathbf{r}}_{ba}\\ {\mathbf{r}}_{bg}\end{array}\right]}_{15\times 1}=$$\left[\begin{array}{c}{\mathbf{q}}_{b_{i}w}\left({\mathbf{p}}_{w{b}_j}-{\mathbf{p}}_{w{b}_i}-{\mathbf{v}}_i^w\Delta t+\frac{1}{2}{\mathbf{g}}^w\Delta t^2\right)-{\mathbf{\alpha }}_{b_i{b}_j}\\ 2{\left[{\mathbf{q}}_{b_j}{b}_i\otimes \left({\mathbf{q}}_{b_{i}w}\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}\\ {\mathbf{q}}_{b_{i}w}\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)-{\mathbf{\beta }}_{b_i{b}_j}\\ {\mathbf{b}}_j^a-{\mathbf{b}}_i^a\\ {\mathbf{b}}_j^g-{\mathbf{b}}_i^g\end{array}\right]$

上面误差中位移,速度,偏置都是直接相减得到。第二项是关于四元数的旋转误差,其中$[\cdot {]}_{xyz}$表示只取四元数的虚部$(x,y,z)$ 组成的三维向量。

问题的提出：一个 IMU 数据作为测量值的噪声方差我们能够标定。现在,一段时间内多个 IMU 数据积分形成的预积分量的方差呢?由高斯分布的线性组合性质：
$$\mathbf{y}=\mathbf{A}\mathbf{x},\mathbf{x}\in \mathcal{N}\left(0,{\mathbf{\Sigma }}_x\right)$$则有：
$${\mathbf{\Sigma }}_y=A{\mathbf{\Sigma }}_x{A}^{\top }$$
所以,要推导预积分量的协方差,我们需要知道 imu 噪声和预积分量之间的线性递推关系。类似于状态转移，回顾一下之前的卡尔曼滤波．

假设已知了相邻时刻误差的线性传递方程:
$${\mathbf{\eta }}_{ik}={\mathbf{F}}_{k-1}{\mathbf{\eta }}_{ik-1}+{\mathbf{G}}_{k-1}{\mathbf{n}}_{k-1}$$比如:状态量误差为${\mathbf{\eta }}_{ik}=\left[\delta {\mathbf{\theta }}_{ik},\delta {\mathbf{v}}_{ik},\delta {\mathbf{p}}_{ik}\right]$测量噪声为${\mathbf{n}}_k=\left[{\mathbf{n}}_k^g,{\mathbf{n}}_k^a\right]$.误差的传递由两部分组成:当前时刻的误差传递给下一时刻,当前时刻测量噪声传递给下一时刻。

协方差矩阵可以通过递推计算得到:
$${\mathbf{\Sigma }}_{ik}={\mathbf{F}}_{k-1}{\mathbf{\Sigma }}_{ik-1}{\mathbf{F}}_{k-1}^{\top }+{\mathbf{G}}_{k-1}{\mathbf{\Sigma }}_{\mathbf{n}}{\mathbf{G}}_{k-1}^{\top }$$其中,${\mathbf{\Sigma }}_{\mathbf{n}}$是测量噪声的协方差矩阵,方差从 i 时刻开始进行递推,${\mathbf{\Sigma }}_{ii}=0$。

6.1.基于误差随时间变化的递推方程

此处借助卡尔曼滤波过程理解。通常对于状态量之间的递推关系是非线性的方程如${\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_{k-1}\right)$,其中状态量为 x,u 为系统的输入量。可以用两种方法来推导状态误差传递的线性递推关系:

• 一种是基于一阶泰勒展开的误差递推方程。
• 一种是基于误差随时间变化的递推方程。

非线性系统${\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_{k-1}\right)$的状态误差的线性递推关系如下:
$$\delta {\mathbf{x}}_k=\mathbf{F}\delta {\mathbf{x}}_{k-1}+\mathbf{G}{\mathbf{n}}_{k-1}\text{\hspace{1em}(26)}$$基于泰勒展开的误差传递应用于 EKF 的协方差预测。论文中采用基于误差随时间变化的递推方程。如果我们能够推导状态误差随时间变化的导数关系,比如:
$$\dot{\delta \mathbf{x}}=\mathbf{A}\delta \mathbf{x}+\mathbf{B}\mathbf{n}$$则误差状态的传递方程为:
$$\begin{array}{rl}\delta {\mathbf{x}}_k& =\delta {\mathbf{x}}_{k-1}+\dot{\delta }{\mathbf{x}}_{k-1}\Delta t\\ \to \delta {\mathbf{x}}_k& =(\mathbf{I}+\mathbf{A}\Delta t)\delta {\mathbf{x}}_{k-1}+\mathbf{B}\Delta t{\mathbf{n}}_{k-1}\end{array}$$可以看出有等式(26)中:
$$\mathbf{F}=\mathbf{I}+\mathbf{A}\Delta t,\mathbf{G}=\mathbf{B}\Delta t$$

回顾一下预积分的误差递推公式,将测量噪声也考虑进模型:

$${\mathbf{q}}_{b_i{b}_{k+1}}={\mathbf{q}}_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]$$$${\mathbf{\alpha }}_{b_i{b}_{k+1}}={\mathbf{\alpha }}_{b_i{b}_k}+{\beta }_{b_i{b}_k}\delta t+\frac{1}{2}\mathbf{a}\delta t^2$$$${\beta }_{b_i{b}_{k+1}}={\beta }_{b_i{b}_k}+\mathbf{a}\delta t$$$$\begin{array}{rl}{\mathbf{b}}_{k+1}^a& ={\mathbf{b}}_k^a+{\mathbf{n}}_{{\mathbf{b}}_k^a}\delta t\\ {\mathbf{b}}_{k+1}^g& ={\mathbf{b}}_k^g+{\mathbf{n}}_{{\mathbf{b}}_k^g}\delta t\end{array}$$其中：
$$\mathbf{\omega }=\frac{1}{2}\left(\left({\mathbf{\omega }}^{b_k}+{\mathbf{n}}_k^g-{\mathbf{b}}_k^g\right)+\left({\mathbf{\omega }}^{b_{k+1}}+{\mathbf{n}}_{k+1}^g-{\mathbf{b}}_k^g\right)\right)$$$$\mathbf{a}=\frac{1}{2}\left({\mathbf{q}}_{b_i{b}_k}\left({\mathbf{a}}^{b_k}+{\mathbf{n}}_k^a-{\mathbf{b}}_k^a\right)+{\mathbf{q}}_{b_i{b}_{k+1}}\left({\mathbf{a}}^{b_{k+1}}+{\mathbf{n}}_{k+1}^a-{\mathbf{b}}_k^a\right)\right)$$确定误差传递的状态量,噪声量,然后开始构建传递方程。

推导出如下的形式，直接给出推导结果：
$$\left[\begin{array}{c}\delta {\mathbf{\alpha }}_{b_{k+1}{b}_{k+1}^{\prime }}\\ \delta {\mathbf{\theta }}_{b_{k+1}{b}_{k+1}^{\prime }}\\ \delta {\mathbf{\beta }}_{b_{k+1}{b}_{k+1}^{\prime }}\\ \delta {\mathbf{b}}_{k+1}^a\\ \delta {\mathbf{b}}_{k+1}^g\end{array}\right]=\mathbf{F}\left[\begin{array}{c}\delta {\mathbf{\alpha }}_{b_k{b}_k^{\prime }}\\ \delta {\mathbf{\theta }}_{b_k{b}_k^{\prime }}\\ \delta {\mathbf{\beta }}_{b_k{b}_k^{\prime }}\\ \delta {\mathbf{b}}_k^a\\ \delta {\mathbf{b}}_k^g\end{array}\right]+\mathbf{G}\left[\begin{array}{c}{\mathbf{n}}_k^a\\ {\mathbf{n}}_k^g\\ {\mathbf{n}}_k^a\\ {\mathbf{n}}_{k+1}^a\\ {\mathbf{n}}_{{\mathbf{b}}_k^g}\\ {\mathbf{n}}_{{\mathbf{b}}_k^g}\end{array}\right]\text{\hspace{1em}(27)}$$

$\mathbf{F},\mathbf{G}$为两个时刻间的协方差传递矩阵:

$\mathbf{F}=$$\left[\begin{array}{ccccc}\mathbf{I}& {\mathbf{f}}_{12}& \mathbf{I}\delta t& -\frac{1}{4}\left({\mathbf{q}}_{b_i{b}_k}+{\mathbf{q}}_{b_i{b}_{k+1}}\right)\delta t^2& {\mathbf{f}}_{15}\\ 0& \mathbf{I}-[\mathbf{\omega }{]}_{\times }& 0& 0& -\mathbf{I}\delta t\\ 0& {\mathbf{f}}_{32}& \mathbf{I}& -\frac{1}{2}\left({\mathbf{q}}_{b_i{b}_k}+{\mathbf{q}}_{b_i{b}_{k+1}}\right)\delta t& {\mathbf{f}}_{35}\\ 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& \mathbf{I}\end{array}\right]$

$\mathbf{G}=$$\left[\begin{array}{cccccc}\frac{1}{4}{\mathbf{q}}_{b_i{b}_k}\delta t^2& {\mathbf{g}}_{12}& \frac{1}{4}{\mathbf{q}}_{b_i{b}_{k+1}}\delta t^2& {\mathbf{g}}_{14}& 0& 0\\ 0& \frac{1}{2}\mathbf{I}\delta t& 0& \frac{1}{2}\mathbf{I}\delta t& 0& 0\\ \frac{1}{2}{\mathbf{q}}_{b_i{b}_k}\delta t& {\mathbf{g}}_{32}& \frac{1}{2}{\mathbf{q}}_{b_i{b}_{k+1}}\delta t& {\mathbf{g}}_{34}& 0& 0\\ 0& 0& 0& 0& \mathbf{I}\delta t& 0\\ 0& 0& 0& 0& 0& \mathbf{I}\delta t\end{array}\right]$
其中：
${\mathbf{f}}_{12}=\frac{\partial {\mathbf{\alpha }}_{b_i{b}_{k+1}}}{\partial \delta {\mathbf{\theta }}_{b_k{b}_k^{\prime }}}=-\frac{1}{4}\left({\mathbf{R}}_{b_i{b}_k}{\left[{\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a\right]}_{\times }\delta t^2+{\mathbf{R}}_{b_i{b}_{k+1}}\left[\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\right]\times \left(\mathbf{I}-[\mathbf{\omega }{]}_{\times }\delta t\right)\delta t^2\right)$

${\mathbf{f}}_{12}=\frac{\partial {\mathbf{\alpha }}_{b_i{b}_{k+1}}}{\partial \delta {\mathbf{\theta }}_{b_k{b}_k^{\prime }}}=-\frac{1}{4}\left({\mathbf{R}}_{b_i{b}_k}{\left[{\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a\right]}_{\times }\delta t^2+{\mathbf{R}}_{b_i{b}_{k+1}}\left[\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\right]\times \left(\mathbf{I}-[\omega {]}_{\times }\delta t\right)\delta t^2\right)$

${\mathbf{f}}_{15}=\frac{\partial {\mathbf{\alpha }}_{b_i{b}_{k+1}}}{\partial \delta {\mathbf{b}}_k^g}=-\frac{1}{4}\left({\mathbf{R}}_{b_i{b}_{k+1}}{\left[\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\right]}_{\times }\delta t^2\right)(-\delta t)$

${\mathbf{f}}_{35}=\frac{\partial {\mathbf{\beta }}_{b_i{b}_{k+1}}}{\partial \delta {\mathbf{b}}_k^g}=-\frac{1}{2}\left({\mathbf{R}}_{b_i{b}_{k+1}}{\left[\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\right]}_{\times }\delta t\right)(-\delta t)$

${\mathbf{g}}_{12}=\frac{\partial {\mathbf{\alpha }}_{b_i{b}_{k+1}}}{\partial {\mathbf{n}}_k^g}={\mathbf{g}}_{14}=\frac{\partial {\mathbf{\alpha }}_{b_i{b}_{k+1}}}{\partial {\mathbf{n}}_{k+1}^g}=\frac{1}{4}\left({\mathbf{R}}_{b_i{b}_{k+1}}\left[\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\right]\times \delta t^2\right)\left(\frac{1}{2}\delta t\right)$

${\mathbf{g}}_{32}=\frac{\partial {\mathbf{\beta }}_{b_i{b}_{k+1}}}{\partial {\mathbf{n}}_k^g}={\mathbf{g}}_{34}=\frac{\partial {\mathbf{\beta }}_{b_i{b}_{k+1}}}{\partial {\mathbf{n}}_{k+1}^g}=-\frac{1}{2}\left({\mathbf{R}}_{b_i{b}_{k+1}}\left[\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\right]\times \delta t^2\right)\left(\frac{1}{2}\delta t\right)$

代码中$\mathbf{F},\mathbf{G}$为$\mathbf{F},\mathbf{V}$。

这里对公式(26)(27)的 IMU 误差运动方程(状态方程)再说明,将上式和 EKF 对比可知,上式恰好给出了如 EKF 一般对非线性系统线性化的过程,这里的意义是表示下一个时刻的 IMU 测量误差与上一个时刻的成线性关系,这样我们根据当前时刻的值,可以预测出下一个时刻的均值和协方差,而公式(26)给出的是均值预测,协方差预测公式如下(对比卡尔曼滤波的第二个公式):
$$P_{t+\delta t}^{b_k}=\left(\mathrm{I}+F_t\delta t\right)P_t^{b_k}{\left(\mathrm{I}+F_t\delta t\right)}^T+\left(G_t\delta t\right)Q{\left(G_t\delta t\right)}^T$$其中：$$F=\mathrm{I}+F_t\delta t,V=G_t\delta t$$上式给出了协方差的迭代公式,初始值$P_{b_k}^{b_k}=0$。其中,$Q$ 为表示噪声项的对角协方差矩阵:$$Q^{12\times 12}=\left[\begin{array}{cccc}{\sigma }_a^2& 0& 0& 0\\ 0& {\sigma }_w^2& 0& 0\\ 0& 0& {\sigma }_{b_a}^2& 0\\ 0& 0& 0& {\sigma }_{b_w}^2\end{array}\right]$$误差项的 Jacobian 的迭代公式:$$J_{t+\delta t}=\left(\mathrm{I}+F_t\delta t\right)J_t$$其中 Jacobian 的初始值为$J_{b_k}=I$

7.离散形式的 PVQ 增量误差分析

首先直接给出 PVQ 增量误差在离散形式下的矩阵形式,为了与代码一致,我们修改下变量顺序,这和代码中 midPointIntegration()函数是一致的。(但不知为何计算的 V 中与前四个噪声项相关的差个负号?)

$$\left[\begin{array}{c}\delta {\alpha }_{k+1}\\ \delta {\theta }_{k+1}\\ \delta {\beta }_{k+1}\\ \delta b_{k+1}\\ \delta b_{w_{k+1}}\end{array}\right]=\left[\begin{array}{ccccc}I& f_{01}& \delta t& f_{03}& f_{04}\\ 0& f_{11}& 0& 0& -\delta t\\ 0& f_{21}& I& f_{23}& f_{24}\\ 0& 0& 0& I& 0\\ 0& 0& 0& 0& I\end{array}\right]\left[\begin{array}{c}\delta {\alpha }_k\\ \delta {\theta }_k\\ \delta {\beta }_k\\ \delta b_{a_k}\\ \delta b_{w_k}\end{array}\right]$$
$$+\left[\begin{array}{cccccc}{v}_{00}& v_{01}& v_{02}& v_{03}& 0& \\ 0& \frac{-\delta t}{2}& 0& \frac{-\delta t}{2}& 0& \\ -\frac{R_k\delta t}{2}& v_{21}& -\frac{R_{k+1}\delta t}{2}& v_{23}& 0& \\ 0& 0& 0& 0& \delta t& 0\\ 0& 0& 0& 0& 0& \delta t\end{array}\right]\left[\begin{array}{c}{n}_{a_k}\\ n_{w_k}\\ n_{a_{k+1}}\\ n_{b_a}\\ n_{b_w}\end{array}\right]\text{\hspace{1em}(28)}$$
其中：
$f_{01}=\frac{\delta t}{2}{f}_{21}=-\frac{1}{4}{R}_k{\left({\hat{a}}_k-b_{a_k}\right)}^{\wedge }\delta t^2-\frac{1}{4}{R}_{k+1}{\left({\hat{a}}_{k+1}-b_{a_k}\right)}^{\wedge }\left[I-{\left(\frac{{\hat{\omega }}_k+{\hat{\omega }}_{k+1}}{2}-b_{{\omega }_k}\right)}^{\wedge }\delta t\right]\delta t^2$

$f_{03}=-\frac{1}{4}\left(R_k+R_{k+1}\right)\delta t^2$

$f_{04}=\frac{\delta t}{2}{f}_{24}=\frac{1}{4}{R}_{k+1}{\left({\hat{a}}_{k+1}-b_{a_k}\right)}^{\wedge }\delta t^3$

$f_{11}=I-{\left(\frac{{\hat{\omega }}_k+{\hat{\omega }}_{k+1}}{2}-b_{{\omega }_k}\right)}^{\wedge }\delta t$
$f_{21}=-\frac{1}{2}{R}_k{\left({\hat{a}}_k-b_{a_k}\right)}^{\wedge }\delta t-\frac{1}{2}{R}_{k+1}{\left({\hat{a}}_{k+1}-b_{a_k}\right)}^{\wedge }\left[I-{\left(\frac{{\hat{\omega }}_k+{\hat{\omega }}_{k+1}}{2}-b_{{\omega }_k}\right)}^{\wedge }\delta t\right]\delta t$
$f_{23}=-\frac{1}{2}\left(R_k+R_{k+1}\right)\delta t$

$f_{24}=\frac{1}{2}{R}_{k+1}{\left({\hat{a}}_{k+1}-b_{a_k}\right)}^{\wedge }\delta t^2$

$v_{00}=-\frac{1}{4}{R}_k\delta t^2$

$v_{01}=v_{03}=\frac{\delta t}{2}{v}_{21}=\frac{1}{4}{R}_{k+1}{\left({\hat{a}}_{k+1}-b_{a_k}\right)}^{\wedge }\delta t^2\frac{\delta t}{2}$

$v_{02}=-\frac{1}{4}{R}_{k+1}\delta t^2$

$v_{21}=v_{23}=\frac{1}{4}{R}_{k+1}{\left({\hat{a}}_{k+1}-b_{a_k}\right)}^{\wedge }\delta t^2$

8.离散形式的 PVQ 增量误差的 Jacobian 和协方差

将公式(28)简写为:

$$\delta z_{k+1}^{15\times 1}=F^{15\times 15}\delta z_k^{15\times 1}+V^{15\times 18}{Q}^{18\times 1}$$
则 $Jacobian$ 的迭代公式为:
$$J_{k+1}^{15\times 15}=F{J}_k\text{\hspace{1em}(29)}$$
其中,$Jacobian$ 的初始值为$J_k=I$。这里计算出来的$J_{k+1}$ 只是为了给后面提供对 bias 的
$Jacobian$。

协方差的迭代公式为:
$$P_{k+1}^{15\times 15}=F{P}_k{F}^T+VQ{V}^T\text{\hspace{1em}(30)}$$
其中,初始值$P_k=0$。$Q$ 为表示噪声项的对角协方差矩阵:
$$Q^{18\times 18}=\left[\begin{array}{cccccc}{\sigma }_a^2& 0& 0& 0& 0& 0\\ 0& {\sigma }_w^2& 0& 0& 0& 0\\ 0& 0& {\sigma }_a^2& 0& 0& 0\\ 0& 0& 0& {\sigma }_w^2& 0& 0\\ 0& 0& 0& 0& {\sigma }_{b_a}^2& 0\\ 0& 0& 0& 0& 0& {\sigma }_{b_w}^2\end{array}\right]$$

代码integration_base.h-->midPointIntegration():

    void midPointIntegration(double _dt, 
                            const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
                            const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
                            const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
                            const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
                            Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
                            Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
    {
        //ROS_INFO("midpoint integration");
        Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
        Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
        result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
        Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
        result_delta_v = delta_v + un_acc * _dt;
        result_linearized_ba = linearized_ba;
        result_linearized_bg = linearized_bg;         

        if(update_jacobian)
        {
            Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
            Vector3d a_0_x = _acc_0 - linearized_ba;
            Vector3d a_1_x = _acc_1 - linearized_ba;
            Matrix3d R_w_x, R_a_0_x, R_a_1_x;

            R_w_x<<0, -w_x(2), w_x(1),
                w_x(2), 0, -w_x(0),
                -w_x(1), w_x(0), 0;
            R_a_0_x<<0, -a_0_x(2), a_0_x(1),
                a_0_x(2), 0, -a_0_x(0),
                -a_0_x(1), a_0_x(0), 0;
            R_a_1_x<<0, -a_1_x(2), a_1_x(1),
                a_1_x(2), 0, -a_1_x(0),
                -a_1_x(1), a_1_x(0), 0;

			//构造F、V矩阵
            MatrixXd F = MatrixXd::Zero(15, 15);
            F.block<3, 3>(0, 0) = Matrix3d::Identity();
            F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                                  -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
            F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
            F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
            F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
            F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
            F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
            F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                                  -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
            F.block<3, 3>(6, 6) = Matrix3d::Identity();
            F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
            F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
            F.block<3, 3>(9, 9) = Matrix3d::Identity();
            F.block<3, 3>(12, 12) = Matrix3d::Identity();
            //cout<<"A"<<endl<<A<<endl;

            MatrixXd V = MatrixXd::Zero(15,18);
            V.block<3, 3>(0, 0) =  0.25 * delta_q.toRotationMatrix() * _dt * _dt;
            V.block<3, 3>(0, 3) =  0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * _dt * 0.5 * _dt;
            V.block<3, 3>(0, 6) =  0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
            V.block<3, 3>(0, 9) =  V.block<3, 3>(0, 3);
            V.block<3, 3>(3, 3) =  0.5 * MatrixXd::Identity(3,3) * _dt;
            V.block<3, 3>(3, 9) =  0.5 * MatrixXd::Identity(3,3) * _dt;
            V.block<3, 3>(6, 0) =  0.5 * delta_q.toRotationMatrix() * _dt;
            V.block<3, 3>(6, 3) =  0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * 0.5 * _dt;
            V.block<3, 3>(6, 6) =  0.5 * result_delta_q.toRotationMatrix() * _dt;
            V.block<3, 3>(6, 9) =  V.block<3, 3>(6, 3);
            V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
            V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;

            //step_jacobian = F;
            //step_V = V;
            jacobian = F * jacobian;  \\公式（29）
            covariance = F * covariance * F.transpose() + V * noise * V.transpose();\\公式（30）
        }

    }

总述：基于滑动窗口的 VIO Bundle Adjustment，总的loss funcution如下所示：

${\min }_{\mathcal{X}}\underset{\text{ prior }}{\underbrace{\rho \left({\Vert {\mathbf{r}}_p-{\mathbf{J}}_p\mathcal{X}\Vert }_{{\Sigma }_p}^2\right)}}+\underset{\text{ IMU error }}{\underbrace{\sum _{i\in B}\rho \left({\Vert {\mathbf{r}}_b\left({\mathbf{z}}_{b_i{b}_{i+1}},\mathcal{X}\right)\Vert }_{{\Sigma }_{b_i{b}_{i+1}}}^2\right)}}$$+\underset{\text{ image error }}{\underbrace{\sum _{(i,j)\in F}\rho \left({\Vert {\mathbf{r}}_f\left({\mathbf{z}}_{{\mathbf{f}}_j}^{c_i},\mathcal{X}\right)\Vert }_{{\Sigma }_{{\mathbf{f}}_j}^{c_i}}^2\right)}}$

故，6、7、8小节实际是在求的IMU error项将会用到的量，为后端优化做准备！