IMU的预计分算法

1. IMU的测量值是什么？

IMU的测量值是三轴加速度和三轴角速度。它们都是在IMU当前时刻的坐标系下表达的向量值，记作(a_t, omega_t)

2.通过IMU的测量我们想获得什么？

很显然，我们要获得的是当前时刻的位姿，包括位移(P)和旋转(R)（如果用四元数表示就是(q)）。为了代码实现中计算上的方便我们同时还保留了计算(P)时的中间量，当前时刻的速度(V)。它们也是紧耦合模型中待优化状态变量的一部分

3. IMU的测量模型

[egin{align*} hat{a}_t &= a_t+b_a+R_w^t g^w +n_a\ hat{omega}_t&=omega_t+b_{omega}+n_{omega} end{align*} ag{1} ]

其中(hat{a},hat{omega})分别是加速度和角速度的测量值；(a,omega)分别是加速度和角速度的真实值；(b_a,b_{omega})分别是加速度和角速度的零偏（bias），它们用随机游走模型（布朗运动）来建模；(n_a,n_{omega})是噪声，它们用高斯白噪声建模；(g^w)是世界坐标系下的重力加速度（的反向，当IMU静止的时候，它测量到的是一个反向的重力加速度）；(R_w^t)是当前时刻的姿态的逆，将(g^w)从世界坐标系转换到IMU坐标系

4. 状态量的运动模型

由(b_k)时刻的状态量得到(b_{k+1})时刻的状态量，通过简单的运动学公式就可以得到。下面的公式中忽略了噪声的影响：

[egin{align*} P_{b_{k+1}}^w&=P_{b_k}^w+V_{b_k}^wDelta t+frac{1}{2}iint_{tin[k,k+1]}R_t^w(hat{a}_t-b_{a_t})-g^wdt^2\ V_{b_{k+1}}^w&=V_{b_k}^w+int_{tin[k,k+1]}R_t^w(hat{a}_t-b_{a_t})-g^wdt\ q_{b_{k+1}}^w&=q_{b_k}^wotimesint_{tin[k,k+1]}frac{1}{2}Omega(hat{omega}_t-b_{omega_t})q_t^{b_k}dt end{align*} ag{2} ]

5.为什么要预积分，怎么预积分以及什么是预积分？

很奇怪把什么是预积分是什么放在最后介绍。一般的介绍顺序都是先介绍什么，再介绍怎么，最后再介绍为什么。我们的顺序刚好反了过来。因为预积分就是一系列操作过程的一个名字，不先介绍完怎么做很难说什么是预积分。

先说为什么：

我们知道在紧耦合中待优化的状态变量是(P,R(q),V,b_a,b_{omega})，在优化的一次次迭代中这些变量是在不停变化的。由公式（2）可以看到当(b_k)时刻的状态发生改变的时候，我们需要一次次的积分来求取(b_{k+1})时刻的状态，这个是很耗费计算资源的，为了节省计算资源和时间，我们希望能够避免积分部分的重复计算。

怎么办呢？

先忽略零偏的影响。观察一下各个积分部分。首先(q)的积分部分很简单，它仅与角速度的测量值有关，注意(q_t^{b_k})与状态量是无关的（不包括零偏），因为它是在(b_k)坐标系下的。(P)和(V)的积分部分除了与加速度测量值有关外还有一个(R_t^w)，它是世界坐标系下的，与(R_{b_k}^w)有关：(q_t^w=q_{b_k}^wotimesint_{ auin[k,t]}frac{1}{2}Omega(hat{omega}_{ au}-b_{omega_ au})q_ au^{b_k}d au)。假如在（2）的三个公式两边同时乘以(q_{b_k}^w)的逆(q_w^{b_k})（或者(R_w^{b_k}))那么积分部分就仅与IMU的测量值（还有零偏，但暂时忽略）有关了。所以我们有

[egin{align*} R_w^{b_k}P_{b_{k+1}}^w&=R_w^{b_k}(P_{b_k}^w+V_{b_k}^wDelta t - frac{1}{2}g^wDelta t^2) +alpha_{b_{k+1}}^{b_k}\ R_w^{b_k}V_{b_{k+1}}^w&=R_w^{b_k}(V_{b_k}^w-g^wDelta t)+eta_{b_{k+1}}^{b_k}\ q_w^{b_k}q_{b_{k+1}^w}&=gamma_{b_{k+1}}^{b_k} end{align*} ag{3} ]

其中：

[egin{align*} alpha_{b_{k+1}}^{b_k}&=iint_{tin[k,k+1]}R_t^{b_k}(hat{a}_t-b_{a_t})dt^2\ eta_{b_{k+1}}^{b_k}&=int_{tin[k,k+1]}R_t^{b_k}(hat{a}_t-b_{a_t})dt\ gamma_{b_{k+1}}^{b_k}&=int_{tin[k,k+1]}frac{1}{2}Omega(hat{omega}_t-b_{omega_t})gamma_t^{b_k}dt end{align*} ag{4} ]

被称为预积分。

预积分是什么？

所以预积分其实是加速度和角速度引起的(b_{k+1})时刻的状态量关于(b_k)时刻的增量。
所谓的预积分，就是预先积分好，之后需要的时候拿来用。我们知道IMU测量的是线性加速度和角速度。状态量中速度是关于加速度的积分，位置是关于加速度的二次积分，姿态是关于角速度的积分。因此，在当前坐标系下（因为IMU的测量值就是在当前坐标系下的）把IMU测量值积分好，当需要的是，只需要根据情况进行坐标系转换和加减了。
可以看到预积分与待优化的状态量（忽略零偏）无关，它是一个固定的值。当优化过程中状态量改变时（世界坐标系下的值）只需要将它们乘上(R_{b_k}^w)并代入（2）中就可以了。这样重复的积分运算就简化为了乘法运算，大大节省了计算量。

预积分的数值实现，欧拉法和中值积分法

（待完成）

IMUFactor的残差和雅克比

残差

IMU Factor是用IMU的测量来约束系统状态([P_i, V_i, Q_i,ba_i, bg_i]^T)。残差定义的是(r =[delta alpha, delta eta, delta gamma, delta b_a, delta b_g]^T)， 也就是ESKF中的误差状态。由公式(3)，我们可以通过系统状态定义：

[z^{b_k}_{b_{k+1}} = left[egin{matrix} R^{b_k}_w(P^w_{b_{k+1}} - P^w_{b_k} + {1over 2}g^wDelta t^2 - V^w_{b_k}Delta t) \ R^{b_k}_w(V^w_{b_{k+1}}+g^wDelta t -V^w_{b_k}) \ {Q^{w}_{b_k}}^{-1} otimes Q^w_{b_{k+1}}\ {b_a}_{b_{k+1}} - {b_a}_{b_{k}} \ {b_g}_{b_{k+1}} - {b_g}_{b_{k}} end{matrix} ight]]

由IMU的测量我们可以定义：

[hat{z}^{b_k}_{b_{k+1}} = left[ egin{matrix} hat{alpha}^{b_k}_{b_{k+1}}\ hat{eta}^{b_k}_{b_{k+1}}\ hat{gamma}^{b_k}_{b_{k+1}}\ 0\ 0 end{matrix} ight]]

残差定义为：(r = z^{b_k}_{b_{k+1}} - hat{z}^{b_k}_{b_{k+1}})。零偏的残差项是认为两帧直接的零偏相差极小。

零偏对预积分的影响

前面我们讨论预积分的时候忽略了零偏的影响，但零偏也是待优化的状态量，每次优化迭代的时候它的值都会发生变化，进而影响了预积分的值，那么对于零偏怎么处理呢？这里分为两种策略：
1）如果零偏变化很大，那么就老老实实去做积分，再做一次传播，当然在代码的实现中是把新的(b_a,b_{omega})代入（4）来计算新的预积分，以备之后使用，世界坐标系下的状态量还是通过乘上(R_{b_k}^w)代入（2）获得。
2）如果零偏变化较小，因为(alpha,eta,gamma)都是关于零偏的非线性函数，将它们线性化(一阶泰勒展开)，求出预积分的近似值来代替传播。这就需要分别求(alpha,eta,gamma)关于(b_a,b_{omega})的雅克比。

所以在单目初始化的时候，解算出新的(b_g)之后就调用了preIntegration->rePropagate()。而在IntegrationBase::Evaluate()中计算(b_a, b_g)改变对预积分的影响就用雅克比乘以(delta b_g, delta b_g)：

        Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
        Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
        Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;

协方差矩阵的传播

在优化项中，残差是通过马氏距离表达的：(r^T Sigma^{-1}r)。因此这里就需要残差(delta z^{b_k}_{b_{k+1}})的协方差矩阵。要得到协方差就需要分析噪声的来源。这里我们参考ESKF中的分析方法，分为真值(True Value)，标称值或测量值(Nominal Value)和误差状态(Error State)。一般情况下有： (True Value = Nominal Value - Noise)
。但是ESKF中从另一个角度来考虑问题：它把标称值当作大信号，把误差状态当做小信号，所以定义了： (True Value = Nominal Value + Error State) 。因此有：(Error State = - Noise = True Value - Nominal Value)。根据我们上面对残差项的定义（系统值减去测量值），残差中的前三项其实就相当于Error State，而不是Noise。
这样我们就来分析误差状态的协方差矩阵。

协方差矩阵的数值形式

雅克比

这里涉及到两种雅克比矩阵：一种是IMUFactor中的，残差关于系统状态的雅克比；另一种是IMU预积分中的(delta z^{b_k}_{b_{k+1}})关于(delta z^{b_k}_{b_k})的雅克比。涉及到关于零偏的雅克比，后一种是前一种推导的基础，或者说用后一种可以快速的推导出前一种。

[egin{split}alpha^{b_k}_{b_{k+1}}& = &hat{alpha}^{b_k}_{b_{k+1}} + J^{alpha}_{{b_a}_k} delta {b_a}_k + J^{alpha}_{{b_g}_k} delta {b_g}_k \ alpha^{b_k}_{b_{k+1}} - hat{alpha}^{b_k}_{b_{k+1}} &=& J^{alpha}_{{b_a}_k} delta {b_a}_k + J^{alpha}_{{b_g}_k} delta {b_g}_k \ {partial delta alpha^{b_k}_{b_{k+1}} over partial delta {b_a}_k} &= &J^{alpha}_{{b_a}_k} end{split}]

[egin{split}{partial r over partial b_{a_k}} & = &{partial R^{b_k}_w(P^w_{b_{k+1}} - P^w_{b_k} + {1over 2}g^wDelta t^2 - V^w_{b_k}Delta t) - hat{alpha}^{b_k}_{b_{k+1}}over partial b_{a_k}} \ & = & -{partial alpha^{b_k}_{b_{k+1}} over partial b_{a_k}}\ & = & -{partial delta alpha^{b_k}_{b_{k+1}} over partial delta {b_a}_k} end{split}]

({partial delta alpha^{b_k}_{b_{k+1}} over partial delta {b_a}_k})由IntegrationBase::midPointIntergration()递推的求出。

[J_{t+delta t} = (I+F_t delta t)J_t, qquad J_{b_k} = I ]