1. IMU的测量值是什么?
IMU的测量值是三轴加速度和三轴角速度。它们都是在IMU当前时刻的坐标系下表达的向量值,记作(a_t, omega_t)
2.通过IMU的测量我们想获得什么?
很显然,我们要获得的是当前时刻的位姿,包括位移(P)和旋转(R)(如果用四元数表示就是(q))。为了代码实现中计算上的方便我们同时还保留了计算(P)时的中间量,当前时刻的速度(V)。它们也是紧耦合模型中待优化状态变量的一部分
3. IMU的测量模型
其中(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})时刻的状态量,通过简单的运动学公式就可以得到。下面的公式中忽略了噪声的影响:
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的测量值(还有零偏,但暂时忽略)有关了。所以我们有
其中:
被称为预积分。
预积分是什么?
所以预积分其实是加速度和角速度引起的(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),我们可以通过系统状态定义:
由IMU的测量我们可以定义:
残差定义为:(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})的雅克比。涉及到关于零偏的雅克比,后一种是前一种推导的基础,或者说用后一种可以快速的推导出前一种。
({partial delta alpha^{b_k}_{b_{k+1}} over partial delta {b_a}_k})由IntegrationBase::midPointIntergration()递推的求出。