IMU的预计分算法

1. IMU的测量值是什么？

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

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

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

3. IMU的测量模型

\[\begin{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*}\tag{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}\)时刻的状态量，通过简单的运动学公式就可以得到。下面的公式中忽略了噪声的影响：

\[\begin{align*} P_{b_{k+1}}^w&=P_{b_k}^w+V_{b_k}^w\Delta t+\frac{1}{2}\iint_{t\in[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_{t\in[k,k+1]}R_t^w(\hat{a}_t-b_{a_t})-g^wdt\\ q_{b_{k+1}}^w&=q_{b_k}^w\otimes\int_{t\in[k,k+1]}\frac{1}{2}\Omega(\hat{\omega}_t-b_{\omega_t})q_t^{b_k}dt \end{align*}\tag{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}^w\otimes\int_{\tau\in[k,t]}\frac{1}{2}\Omega(\hat{\omega}_{\tau}-b_{\omega_\tau})q_\tau^{b_k}d\tau\)。假如在（2）的三个公式两边同时乘以\(q_{b_k}^w\)的逆\(q_w^{b_k}\)（或者\(R_w^{b_k}\))那么积分部分就仅与IMU的测量值（还有零偏，但暂时忽略）有关了。所以我们有

\[\begin{align*} R_w^{b_k}P_{b_{k+1}}^w&=R_w^{b_k}(P_{b_k}^w+V_{b_k}^w\Delta t - \frac{1}{2}g^w\Delta 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^w\Delta t)+\beta_{b_{k+1}}^{b_k}\\ q_w^{b_k}q_{b_{k+1}^w}&=\gamma_{b_{k+1}}^{b_k} \end{align*}\tag{3} \]

其中：

\[\begin{align*} \alpha_{b_{k+1}}^{b_k}&=\iint_{t\in[k,k+1]}R_t^{b_k}(\hat{a}_t-b_{a_t})dt^2\\ \beta_{b_{k+1}}^{b_k}&=\int_{t\in[k,k+1]}R_t^{b_k}(\hat{a}_t-b_{a_t})dt\\ \gamma_{b_{k+1}}^{b_k}&=\int_{t\in[k,k+1]}\frac{1}{2}\Omega(\hat{\omega}_t-b_{\omega_t})\gamma_t^{b_k}dt \end{align*}\tag{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 \beta, \delta \gamma, \delta b_a, \delta b_g]^T\)， 也就是ESKF中的误差状态。由公式(3)，我们可以通过系统状态定义：

\[z^{b_k}_{b_{k+1}} = \left[\begin{matrix} R^{b_k}_w(P^w_{b_{k+1}} - P^w_{b_k} + {1\over 2}g^w\Delta t^2 - V^w_{b_k}\Delta t) \\ R^{b_k}_w(V^w_{b_{k+1}}+g^w\Delta 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} \right]\]

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

\[\hat{z}^{b_k}_{b_{k+1}} = \left[ \begin{matrix} \hat{\alpha}^{b_k}_{b_{k+1}}\\ \hat{\beta}^{b_k}_{b_{k+1}}\\ \hat{\gamma}^{b_k}_{b_{k+1}}\\ 0\\ 0 \end{matrix}\right]\]

残差定义为：\(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,\beta,\gamma\)都是关于零偏的非线性函数，将它们线性化(一阶泰勒展开)，求出预积分的近似值来代替传播。这就需要分别求\(\alpha,\beta,\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}\)的雅克比。涉及到关于零偏的雅克比，后一种是前一种推导的基础，或者说用后一种可以快速的推导出前一种。

\[\begin{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}\]

\[\begin{split}{\partial r \over \partial b_{a_k}} & = &{\partial R^{b_k}_w(P^w_{b_{k+1}} - P^w_{b_k} + {1\over 2}g^w\Delta 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 \]