在有限元刚度阵推导过程中，会遇到对矩阵进行积分的操作。

我们有两种方法来解决这个问题：

1. 通过map函数，把int操作映射到矩阵的各个元素上。具体操作如下：



2. 调用MTM软件包，就可以直接利用int命令对矩阵进行积分。具体操作如下：



 

文章目录
1. 前言
2. IMU模型
3. 基于世界坐标系下的IMU运动模型
3.1 连续形式下的IMU运动模型
3.2 离散形式下的IMU运动模型
3.2.1 欧拉法离散形式
3.2.2 中值法离散形式
4.IMU预积分 (基于第K帧IMU body坐标系下的运动模型)
4.1 连续形式下的IMU运动模型
4.2 离散形式下的IMU运动模型
4.2.1 两帧之间 PVQ 增量的欧拉法离散形式
4.2.2 两帧之间 PVQ 增量的中值法离散形式
5. 非线性方程的误差递推方程
5.1 一段时间内多个IMU数据积分形成的预积分量的协方差计算
5.2 非线性方程的误差递推方程推导
5.2.1 基于一阶泰勒展开的误差递推方程
5.2.2 基于误差随时间变化(误差导数)的递推方程
5.2.3 基于泰勒展开和误差随时间变化的方法对比
6. PVQ增量误差、协方差及雅克比矩阵的递推方程
6.1 连续形式下的误差、协方差及雅克比矩阵的递推方程
6.2 离散形式下(中值法)增量误差、协方差及雅克比矩阵的递推方程

1. 前言
本博客借鉴了崔华坤的《VINS论文推导及代码解析》和 VINS-Mono理论学习——IMU预积分 Pre-integration (Jacobian 协方差)的内容，因为确实写得太好了，然后有些地方加入自己一些理解。
VINS-MONO论文中的IV-B. IMU Pre-integration介绍了IMU预积分模型，Foster的倆篇论文对IMU预积分理论进行详细分析。
值得注意的是，本文使用了四元素中Hamilton（可参考《Quaternion kinematics for the error state Kalman filter》，右手系）的表示和JPL（可参考《Indirect Kalman Filter for 3D Attitude Estimation》，左手系）表示，因为Vins-mono里面也是用了俩种表示混用，俩者的具体区别可以看《Quaternion kinematics for the error state Kalman filter》的第17页，且论文中用到的JPL的右乘左乘定义还与《Indirect Kalman Filter for 3D Attitude Estimation》不同，这种问题估计让无数人心塞。／（ㄒｏㄒ）／～～
为什么需要对IMU进行预积分？

传统捷联惯性导航算法，在已知 $k$ 时刻下的IMU状态量(姿态、速度和位移)情况下，利用IMU测量的线加速度和角速度，通过积分预算得到$k+1$时刻下的状态量。
然而在非线性优化的VIO中，各个节点的状态量都是估计值，当算法对这些状态量优化时，每次调整都需要在它们之间重新积分，导致绝对位姿被优化时对状态量进行重复积分。IMU预积分的提出使得优化算法可对IMU的相对测量进行处理，使它与绝对位姿解耦，或者只要线性运算就可以进行矫正。
2. IMU模型
IMU测量值包括加速度计得到的 (测量值) 线加速度$\hat{a_{t}}$ 和陀螺仪得到的角加速度$\hat{w}_{t}$ 【论文式(1)】

$\hat{a}_{t}=a_{t}+b_{at}+R_{w}^{t}g^{w}+n_{a}$ $\hat{w}_{t}=w_{t}+b_{wt}+n_{w}$其中$t$下标表示在IMU的体(body)坐标系下，$a_{t}$、$w_{t}$分别表示IMU真实的线加速度和角速度，并受到加速度偏置(bias) $b_{at}$、陀螺仪偏置$b_{wt}$和附加噪声$n_a$、$n_{n_{w}}$的影响。计算得到的线加速度$\hat{a_{t}}$是重力加速度和物体加速度的合矢量。

假设附加噪声为高斯噪声：

$n_{a}\sim(0,\sigma_{a}^{2}),  \ n_{a}\sim(0,\sigma_{w}^{2})$ 加速度计偏置和陀螺仪偏置被建模为随机游走，其导数为高斯分布：【论文式(2)】

$\dot{b}_{at}=n_{ba},  \ \dot{b}_{wt}=n_{bw}$ $n_{ba}\sim N(0,\sigma_{ba}^{2}), \ n_{bw} \sim N(0, \sigma_{bw}^{2})$
3. 基于世界坐标系下的IMU运动模型
  
3.1 连续形式下的IMU运动模型
对于图像帧$k$和$k+1$, IMU body坐标系对应为$b_{k}$和$b_{k+1}$，位置、速度和姿态状态值PVQ(Pose、Velocity、Quaternion)可以根据$[t_{k}, t_{k+1}]$时间间隔内的IMU测量值，在世界坐标系下进行传递：【论文式(3)(4)】

$p^{w}_{b_{k+1}}=p^{w}_{bk}+v_{b_{k}}^{w}\Delta t_{k}+\int \int_{t\in[t_{k}, t_{k+1}]}(R_{t}^{w}(\hat{a}_{t}-b_{at}-n_{a})-g^{w})dt^{2}$  $v_{b_{k+1}}^{w}=v_{bk}^{w}+\int_{t\in[t_{k},t_{k+1}]}(R^{w}_{t}(\hat{a}_{t}-b_{at}-n_{a})-g^{w})dt$   $q_{b_{k+1}}^{w} = q_{b_{k}}^{w} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} q_{t}^{b_{k}} \otimes \begin{bmatrix}
(\hat{w}_{t}-b_{wt}-n_{w})\\ 
0
\end{bmatrix}dt \\ =  q_{bk} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} \Omega (\hat{w}_{t}-b_{wt}-n_{w})q_{t}^{bk}dt \tag{1}$  $\Omega(w)=\begin{bmatrix}
-[w]_{\times} & w\\ 
 -w^{T}& 0
\end{bmatrix}, \ [w]_{\times} = \begin{bmatrix}
0 & -w_{z} & w_{y}\\ 
w_{z} & 0 & -w_{x}\\ 
-w_{y} & w_{x} & 0
\end{bmatrix}$其中 $\Delta t_{k}$是$[t_{k}, t_{k+1}]$之间的时间间隔，$R_{t}^{w}$为t时刻IMU body坐标系到世界坐标系的旋转矩阵，$q_{t}^{bk}$为用四元素表示的 $t$ 时刻从IMU body坐标系到 $k$ 时刻IMU body坐标系的旋转，这里的四元素实部在后，虚部在前, 为了与论文保持一致，即四元素JPL表达。这里的$\Omega(w)$表示四元素右乘。

关于公式$(1)$的推导，这里首先引入四元素左乘右乘及导数定理：

这里与《Indirect Kalman Filter for 3D Attitude Estimation》中公式（10）不同，主要是因为左右手系不同，我们引入左乘和右乘符号如下：

$q_{a} \otimes q_{b} = R(q_{b})q_{a} = \begin{bmatrix}
s_{b} & z_{b} & -y_{b} & x_{b}\\ 
-z_{b} & s_{b} & x_{b} & y_{b}\\ 
 y_{b}& -x_{b} & s_{b} & z_{b}\\ 
 -x_{b}& -y_{b} & -z_{b} & s_{b}
\end{bmatrix}\begin{bmatrix}
x_{a}\\ 
y_{a}\\ 
z_{a}\\ 
s_{a}
\end{bmatrix}  \\ = L(q_{a})q_{b} = \begin{bmatrix}
s_{a} & -z_{a} & y_{a} & x_{a}\\ 
z_{a} & s_{a} & -x_{a} & y_{a}\\ 
 -y_{a}& x_{a} & s_{a} & z_{a}\\ 
 -x_{a}& -y_{a} & -z_{a} & s_{a}
\end{bmatrix}\begin{bmatrix}
x_{b}\\ 
y_{b}\\ 
z_{b}\\ 
s_{b}
\end{bmatrix}$

为了简化，令$q=[x \ y \ z \ s] = [w \ s]$, 则有：

$R(q) = \Omega(w)+sI_{4\times4} = \begin{bmatrix}
-[w]_{\times} & w\\ 
 -w^{T}& 0
\end{bmatrix} + sI_{4\times4}$ $L(q) = \Psi(w)+sI_{4\times4} = \begin{bmatrix}
[w]_{\times} & w\\ 
 -w^{T}& 0
\end{bmatrix} + sI_{4\times4}$

对于四元素的求导，我们定义$q_{t}$为$t$时刻下的单位四元素(这里的四元素实部在后，虚部在前)，$w$为$q_{t}$确定的角速度，则关于$q_{t}$的导数为： $\dot{q}_{t} = \frac{1}{2} \begin{bmatrix}
-[w]_{\times} & w\\ 
 -w^{T}& 0
\end{bmatrix}q_{t} = \frac{1}{2}\Omega(w)q_{t}=\frac{1}{2}R(\begin{bmatrix}
w\\ 
0
\end{bmatrix})q_{t} = \frac{1}{2}q_{t} \otimes \begin{bmatrix}
w\\ 
0
\end{bmatrix}$
当看到$\dot{q} = \frac{1}{2} q_{t} \otimes \begin{bmatrix} 0 \\ w \end{bmatrix}$ 说明该四元素实部在前，虚部在后（Hamilton表示），只是表示不同而已。
因此对于IMU连续形式下的旋转状态(用四元素表示)推导，我们有：

$q_{b_{k+1}^{w}}=q_{b_{k}}^{w} \otimes q_{b_{k+1}}^{b_{k}} = q_{b_{k}}^{w} \otimes \int _{t\in [t_{k}, t_{k+1}]} \dot{q_{t}}^{b_{k}} dt = q_{bk}^{w} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} q_{t}^{b_{k}}\otimes  \begin{bmatrix}
w_{t}^{b_{k}}\\ 
0\end{bmatrix}dt \\ =q_{b_{k}}^{w} \otimes \int_{t \in [t_{k}, t_{k+1}]} \frac{1}{2}q_{t}^{b_{k}}  \otimes \begin{bmatrix}
\hat{w_{t}}-b_{wt}-n_{w}\\ 
0\end{bmatrix} dt \\ = =  q_{bk} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} \Omega (\hat{w}_{t}-b_{wt}-n_{w})q_{t}^{bk}dt$

3.2 离散形式下的IMU运动模型
3.2.1 欧拉法离散形式
使用欧拉法，即$k+1$时刻的位姿是用第$k$时刻的测量值$\hat{a}_{b_{k}}$, $\hat{w}_{b_{k}}$来计算的：

$p^{w}_{b_{k+1}} = p_{b_{k}}^{w} + v_{b_{k}}^{w} \Delta t_{k} + \frac{1}{2}\hat{a}_{b_{k}}\delta t^{2}$ $v_{b_{k+1}}^{w} = v_{b_{k}}^{w} + \hat{a}_{b_{k}} \delta t$ $q^{w}_{b_{k+1}} = q^{w}_{b_{k}} \otimes \begin{bmatrix}
1\\
\frac{1}{2}\hat{w}_{b_{k}}\delta t
\end{bmatrix}$ 其中 $\hat{a}_{b_{k}} = q_{b_{k}}^{w}(a_{b_{k}}-b_{ak})-g^{w}$$\hat{w}_{b_{k}} = w_{b_{k}}-b_{wk}$
3.2.2 中值法离散形式
使用中值法，即$k+1$时刻的位姿是用俩个时刻$k$和$k+1$测量值$a$, $w$的平均值来计算的：

$p^{w}_{b_{k+1}} = p_{b_{k}}^{w} + v_{b_{k}}^{w} \Delta t_{k} + \frac{1}{2}\bar{\hat{a}}_{t}\delta t^{2}$ $v_{b_{k+1}}^{w} = v_{b_{k}}^{w} + \bar{\hat{a}}_{t} \delta t$ $q^{w}_{b_{k+1}} = q^{w}_{b_{k}} \otimes \begin{bmatrix}
1\\
\frac{1}{2}\bar{\hat{w}}_{t}\delta t
\end{bmatrix}$ 其中 $\bar{\hat{a}}_{t} = 
\frac{1}{2}[q_{b_{k}}^{w}(a_{b_{k}}-b_{ak})-g^{w} + q_{b_{k+1}}^{w}(a_{b_{k+1}}-b_{ak+1})-g^{w}]$$\bar{\hat{w}}_{t} = \frac{1}{2}(w_{b_{k}}-b_{wk}+w_{b_{k+1}}-b_{wk+1}) \\ = \frac{1}{2}(w_{b_{k}}+w_{b_{k+1}})-b_{w{k}}$ 假设在短时间内加速度计和陀螺仪的偏置不变，则有：$b_{ak}=b_{ak+1}, \ b_{wk} = b_{wk+1}$
4.IMU预积分 (基于第K帧IMU body坐标系下的运动模型)
 
通过公式$(1)$可以看到，IMU 的积分需要依赖与第 $k$ 帧的 $v$ 和 $R$ (基于世界坐标系下的)，当我们在后端进行非线性优化时，需要迭代更新第 $k$ 帧的 $v$ 和 $R$，这将导致我们需要根据每次迭代后的值重新进行积分，这将非常耗时。我们考虑将优化变量从第 $k$ 帧到第 $k+1$ 帧的 IMU 积分项中分离开来。
4.1 连续形式下的IMU运动模型
IMU预积分的思想就是将参考坐标系从世界坐标系$w$调整为第$k$帧的IMU body坐标系$b_{k}$下，可通过在等式俩端同时乘以$R^{b_{k}}_{w}$得到：【论文式[5][6]】

$R^{b_{k}}_{w}p^{w}_{b_{k+1}}=R^{b_{k}}_{w}(p_{b_{k}}^{w}+v^{w}_{b_{k}}\Delta t_{k}-\frac{1}{2}g^{w}\Delta t_{k}^{2}) + \alpha^{b_{k}}_{b_{k+1}}$ $R^{b_{k}}_{w}v_{b_{k+1}}^{w} = R_{w}^{b_{k}}(v_{b_{k}}^{w}-g^{w}\Delta t_{k})+\beta^{b_{k}}_{b_{k+1}}$ $q_{w}^{b_{k}} \otimes q_{b_{k+1}}^{w} = \gamma _{b_{k+1}} ^{b_{k}} \tag{2}$

其中 $\alpha^{b_{k}}_{b_{k+1}} = \int \int _{t\in [t_{k}, t_{k+1}]} R_{t}^{b_{k}} (\hat{a}_{t}-b_{at}-n_{a}) dt^{2}$ $\beta_{b_{k+1}}^{b_{k}} = \int _{t\in [t_{k}, t_{k+1}]}R_{t}^{b_{k}}(\hat{a}_{t}-b_{at}-n_{a})dt$ $\gamma_{b_{k+1}} = \int _{t \in [t_{k}, t_{k+1}]} \frac{1}{2} \Omega(\hat{w}_{t}-b_{wt}-n_{w}) \gamma_{t}^{b_{k}}dt$
此时的积分结果$\alpha^{b_{k}}_{b_{k+1}}$、$\beta^{b_{k}}_{b_{k+1}}$、$\gamma^{b_{k}}_{b_{k+1}}$可以理解为$b_{k+1}$对$b_{k}$的相对运动量，$b_{k}$的状态并不会对其产生影响，因此将其作为非线性优化变量，可以避免状态的重复传递。
注意，这是在假设IMU偏置$b_{a}$、$b_{w}$已经确定的情况下，实际上偏置也是需要优化的变量，那么每次迭代时，$b_{a}$、$b_{w}$发生改变，得重新根据公式求得所有帧之间的IMU预积分。
当偏置变换很小时，可以将$\alpha_{b_{k+1}}^{b_{k}}$、$\beta_{b_{k+1}}^{b_{k}}$、$\gamma_{b_{k+1}}^{b_{k}}$按其偏置的一阶近似来调整，否则就进行重新传递。【论文式[12]】(这部分只是抛出一个概念，后面会讲为什么这样写)

$\alpha_{b_{k+1}}^{b_{k}} \approx  \hat{\alpha}_{b_{k+1}}^{b_{k}} + J_{b_{a}}^{ \alpha} \delta b_{a} + J_{b_{w}}^{ \alpha} \delta b_{w}$ $\beta_{b_{k+1}}^{b_{k}} \approx  \hat{\beta}_{b_{k+1}}^{b_{k}} + J_{b_{a}}^{ \beta} \delta b_{a} + J_{b_{w}}^{ \beta} \delta b_{w}$ $\gamma_{b_{k+1}}^{b_{k}} \approx  \hat{\gamma}_{b_{k+1}}^{b_{k}} \begin{bmatrix} 1 \\ \frac{1}{2}J_{b_{w}}^{\gamma}\delta b_{w} \end{bmatrix}$
4.2 离散形式下的IMU运动模型
4.2.1 两帧之间 PVQ 增量的欧拉法离散形式
欧拉法给出第i时刻与第i+1时刻的预积分量估计值的关系： 【论文式[7]】

$\hat{\alpha}^{b_{k}}_{i+1} = \hat{\alpha}^{b_{k}}_{i}  + \hat{\beta}_{i}^{b_{k}}\delta t + \frac{1}{2} R(\hat{\gamma}_{i}^{b_{k}})(\hat{a}_{i}-b_{ai})\delta t^{2}$ $\hat{\beta}^{b_{k}}_{i+1} = \hat{\beta}^{b_{k}}_{i} + \frac{1}{2} R(\hat{\gamma}_{i}^{b_{k}})(\hat{a}_{i}-b_{ai})\delta t$ $\hat{\gamma}^{b_{k}}_{i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \hat{\gamma}^{i}_{i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \begin{bmatrix}
1\\
\frac{1}{2}(\hat{w}_{i}-b_{wi})\delta t
\end{bmatrix}$

其中$i$是$[t_{k}, t_{k+1}]$之间的离散时间
4.2.2 两帧之间 PVQ 增量的中值法离散形式
代码中采用的基于中值法的 IMU 预积分公式，这在Estimator::processIMU()函数中的IntegrationBase::push_back()函数中得以实现，注意这里积分出来的是前后两帧之间的 IMU 增量信息。

$\hat{\alpha}^{b_{k}}_{i+1} = \hat{\alpha}^{b_{k}}_{i}  + \hat{\beta}_{i}^{b_{k}}\delta t+ \frac{1}{2} \bar{\hat{a}}_{i}\delta t^{2}$  $\hat{\beta}^{b_{k}}_{i+1} = \hat{\beta}^{b_{k}}_{i} + \bar{\hat{a}}_{i}\delta t$ $\hat{\gamma}^{b_{k}}_{i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \hat{\gamma}^{i}_{i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \begin{bmatrix}
1\\
\frac{1}{2} \bar{\hat{w}}_{i}\delta t
\end{bmatrix}$  其中 $\bar{\hat{a}}_{t} = 
\frac{1}{2}[\hat{\gamma}^{b_{k}}_{i}(\hat{a}_{i}-b_{i}) + \hat{\gamma}^{b_{k}}_{i+1}(\hat{a}_{i+1}-b_{ai})]$ $\bar{\hat{w}}_{i} = \frac{1}{2}(\hat w_{i} + \hat w_{i+1})-b_{wi}$

初始状态下$\alpha_{b_{k}}^{b_{k}}$、$\beta_{b_{k}}^{b_{k}}$为0, $\gamma_{b_{k}}^{b_{k}}$为单位四元素，$n_{a}$、$n_{w}$被视为0，$i$为在$[k, k+1]$中IMU测量值的某一时刻，$\delta t$为IMU测量值$i$和$i+1$之间的时间间隔。
5. 非线性方程的误差递推方程
5.1 一段时间内多个IMU数据积分形成的预积分量的协方差计算
一个 IMU 数据作为测量值的噪声方差我们能够标定。现在，一段时间内多个 IMU 数据积分形成的预积分量的方差呢？
要推导预积分量的协方差，需要知道IMU噪声和预积分量之间的线性递推关系。
假设已知了相邻时刻误差的线性传递方程：

$\eta_{ik+1} = F_{k}\eta_{ik} + G_{k}n_{k}$其中$\eta_{ik}$为状态量误差且$\eta_{ik}=[\delta\theta_{ik}, \delta v_{ik}, \delta p_{ik}]$，$n_{k}$为测量噪声且$n_{k}=[n_{k}^{g}, n_{k}^{g}]$。

可以看出误差的传递由倆部分组成：当前时刻的误差传递给下一时刻，当前时刻测量噪声传递给下一时刻。
5.2 非线性方程的误差递推方程推导
通常对于状态量之间的递推关系是非线性的方程如$x_{k+1} = f(x_{k}, u_{k})$，其中状态量$x$、$u$为系统的输入量。
可以用俩种方法来推导状态误差传递的线性递推关系：

一种是基于一阶泰勒展开的误差递推方程
一种是基于误差随时间变化的递推方程 (论文是基于误差随时间变化来推导的)

5.2.1 基于一阶泰勒展开的误差递推方程
令状态量为$\hat{x} = x +\delta x$，其中$\hat{x}$表示计算得到的值，带有误差，真值为$x$，误差为$\delta x$。另外，输入量$u$的噪声为$n$。

应用一阶泰勒展开(EKF的协方差预测也是用了泰勒展开)，非线性系统$\hat{x}_{k+1}=f(\hat{x}_{k},\hat{u}_{k})$的状态误差的线性递推关系为：

$\delta x_{k+1} = F\delta x_{k}+Gn_{k}$ 其中，F是状态量$x_{k+1}$对状态量$x_{k}$的雅克比矩阵，G是状态量$x_{k+1}$对输入量$u_{k}$的雅克比矩阵。
证明：对非线性状态方程进行一阶泰勒展开有：

$\hat{x}_{k+1} = f(\hat{x}_{k}, \hat{u}_{k})$ $x_{k+1}+\delta x_{k+1} = f(x_{k} + \delta x_{k}, u_{k} + n_{k})$ $\underline{x_{k+1}}+\delta x_{k+1} =  \underline{f(x_{k}, u_{k})} + F \delta x_{k} + Gn_{k}  \tag{3}$
5.2.2 基于误差随时间变化(误差导数)的递推方程
如果我们能够推导状态误差随时间变化的导数关系，如：

$\dot {\delta}_{x} = A\delta x + Bn$ 则误差状态的传递方程为：

$\delta x_{k+1} = \delta x_{k} + \dot{\delta} x_{k} \Delta t$ $\rightarrow \delta x_{k+1} = (I+A\Delta t)\delta x_{k} + B\Delta tn_{k} \tag{4}$
由公式$(3)(4)$得   $F=I+A\Delta t$, $G=B\Delta t$
5.2.3 基于泰勒展开和误差随时间变化的方法对比
基于一阶泰勒展开的误差递推方程不香吗，为什么会用误差随时间的变化来进行误差递推方程的构建呢？
在VIO系统中，我们已经知道了状态的导数和状态之间的转移矩阵，如我们已经知道速度和状态量之间的关系：

$\dot{v}^{w} = R^{w}_{b}a^{b}+g^{w}$ 那我们就可以推导速度的误差导数和状态误差之间的关系，在每一项上都加上各自的误差就有：

$\dot{v}^{w}+\dot{\delta }v^{w} = R_{b}^{w}exp([\delta \theta]_{\times})(a^{b} + \delta a^{b})+g+\delta g$  $\dot{v}^{w}+\dot{\delta }v^{w} = R_{b}^{w}(I+[\delta \theta]_{\times})(a^{b} + \delta a^{b})+g+\delta g$ $\underline{\dot{v}}^{w} + \dot{\delta} v^{w}= \underline{R_{b}^{w} a^{b}+ g} +R_{b}^{w}\delta a^{b} + R_{b}^{w} [\delta \theta]_{\times} (a^{b} + \delta a^{b}) + \delta g$$\dot{\delta}{v}^{w} = R_{b}^{w}\delta a^{b} + R_{b}^{w} [\delta \theta]_{\times} (a^{b} + \delta a^{b}) + \delta g$ $\dot{\delta} v^{w} = R_{b}^{w}\delta a^{b} - R_{b}^{w} [a^{b}]_{\times} \delta \theta + \delta g \tag{5}$ 由此就能以此类推，轻易写出整个 A 和 B 其他方程了。
考虑到公式的编辑篇幅，为了对一些求导公式进行简化，对求导公式进行了简化 (下同)：

$\frac{\partial a^{w}}{\partial \delta \theta } = \underset{\delta \theta \rightarrow 0}{lim} \frac{R_{b}^{w}exp([\delta \theta]_{\times})a^{b}-R_{b}^{w}a^{b}}{\delta \theta} \rightarrow \frac{\partial a^{w}}{\partial \delta \theta} = \frac{\partial R_{b}^{w}exp([\delta \theta]_{\times}) a^{b}}{\partial \delta \theta}$
6. PVQ增量误差、协方差及雅克比矩阵的递推方程
6.1 连续形式下的误差、协方差及雅克比矩阵的递推方程
在第4部分我们已经完成了IMU预积分测量值的推导，而要将IMU预积分运用到非线性优化中，需要建立线性高斯误差状态递推方程 (可参考第5部分)，由线性高斯系统的协方差，推到方程协方差矩阵，并纠结对应的雅克比矩阵。

首先由于四元素$\gamma_{t}^{b_{k}}$被参数化过，我们将其误差定义为围绕其均值的扰动：【论文式(8)】

$\gamma_{t}^{b_{k}} \approx \hat{\gamma}_{t}^{b_{k}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \delta \theta_{t}^{b_{k}}\end{bmatrix}$

由预积分的连续形式建立的运动模型，由公式(2)可得一段时间内IMU构建的预积分量作为测量值，对俩时刻之间的状态量进行约束，可得到其在$k$和$k+1$时刻下的误差项为：【论文公式(24)】

$\begin{bmatrix} \delta \alpha_{b_{k+1}}^{b_{k}} \\ \delta \beta_{b_{k+1}}^{b_{k}} \\ \delta \theta_{b_{k+1}}^{b_{k}} \\ \delta b_{a} \\  \delta b_{w} \end{bmatrix} = \begin{bmatrix} 
R^{b_{k}}_{w}(p^{w}_{b_{k+1}}-p_{b_{k}}^{w} - v_{k}^{w} \Delta t + \frac{1}{2}g^{w} \Delta t ^{2}) - \alpha_{b_{k+1}}^{b_{k}}\\ R^{b_{k}}_{w}(v_{b_{k+1}}^{w}-v_{b_{k}}^{w}+g^{w}\Delta t) - \beta ^{b_{k}}_{b_{k+1}} \\
 2[ q_{w}^{b_{k}} \otimes q^{w}_{b_{k+1}}  \otimes  \gamma_{b_{k}}^{b_{k+1}} ]_{xyz} \\
 b_{ab_{k+1}}-b_{ab_{k}} \\ b_{wb_{k+1}}-b_{wb_{k}}\end{bmatrix}$
我们给出在$t$时刻误差项的线性化递推方程为：【论文式(9)】:

$\begin{bmatrix} \delta \dot{\alpha}_{t}^{b_{k}} \\ \delta \dot{\beta}_{t}^{b_{k}} \\ \delta \dot{\theta}_{t}^{b_{k}} \\ \delta \dot{b}_{at} \\  \delta \dot{b}_{wt} \end{bmatrix} = \begin{bmatrix}
0 & I & 0 & 0 & 0\\ 
0 & 0 & -R_{t}^{b_{k}}[\hat{a}_{t}-b_{at}]_{\times} & -R_{t}^{b_{k}} & 0\\ 
0 & 0 & -[\hat{w}-b_{wt}]_{\times} & 0 & -I\\ 
0 & 0 & 0 & 0 & 0 \\ 
0 & 0 & 0 & 0 & 0
\end{bmatrix} 
\begin{bmatrix} \delta \alpha_{t}^{b_{k}} \\ \delta \beta_{t}^{b_{k}} \\ \delta \theta_{t}^{b_{k}} \\ \delta b_{at} \\  \delta b_{wt} \end{bmatrix} + 
\begin{bmatrix}
0 & 0 & 0 & 0\\ 
-R_{t}^{b_{k}} & 0 & 0 & 0\\ 
 0& -I & 0 & 0 \\ 
 0 & 0 & I & 0 \\ 
 0 & 0 & 0 & I
\end{bmatrix} \begin{bmatrix} n_{a} \\ n_{w} \\ n_{ba} \\ n_{bw} \end{bmatrix} \tag{6} \\ = F_{t}\delta z_{t}^{b_{k}} + G_{t}n_{t}$ 可以简写为：

$\delta \dot{z}_{t}^{b_{k}} =  F_{t}\delta z_{t}^{b_{k}} + G_{t}n_{t}$ 其中$F_{t}$是15x15，$G_{t}$是12x12，$\delta z_{t}^{b_{k}}$是15x1，$n_{t}$是12x1。

根据导数的定义有：

$\delta \dot{z}_{t}^{b_{k}} = \underset{\delta t\rightarrow 0}{lim} \frac{\delta z_{t+\delta t}^{b_{k}}-\delta z_{t}^{b_{k}}}{\delta t}$ 即：$\delta z_{t+\delta t}^{b_{k}} = \delta z_{t}^{b_{k}} + \delta \dot{z}_{t}^{b_{k}} \delta t = \delta z_{t}^{b_{k}} + (F_{t}\delta z_{t}^{b_{k}} + G_{t}n_{t})\delta t = (I+F_{t} \delta t) \delta z_{t}^{b_{k}} + G_{t}\delta t n_{t}$ 这个形式与公式(4)不谋而合，一般连续形式下的微小时间用$\delta t$表示，离散形式下的时间区间用$\Delta t$表示，其实都是一个意思。进一步的，令$F=(I+F_{t} \delta t), V=G_{t}\delta t$, 有：

$\delta z_{t+\delta t}^{b_{k}}  = F \delta z_{t}^{b_{k}} + Vn_{t} \tag{7}$ 由此我们已经知道了该非线性系统的线性化递推方程。
至于下一时刻的协方差，首先对于一个高斯分布的变量线性变换后得到新的变量的协方差计算，我们有以下推导：

已知一个变量$y=Ax$，$x \sim(0, \Sigma_{x})$，则有 $\Sigma_{y} = A \Sigma_{x} A^{T}$，即：

$\Sigma_{y} = E((Ax)(Ax)^{T}) = E(Axx^{T}A^{T}) = A \Sigma_{x} A^{T}$
得到了递推方程$(7)$之后，根据高斯分布线性变换协方差的传递规律，我们可以预测下一时刻的协方差：【论文式(10)】

$P_{t+\delta t}^{b_{k}} = (I + F_{t} \delta t) P_{t}^{b_{k}}  (I + F_{t} \delta t)^{T} + (G \delta t) Q (G_{t} \delta t)^{T}$

其中初始协方差 $P_{b_{k}}^{b_{k}} = 0$, $Q$代表噪声项的对角协方差矩阵：

$Q^{12 \times 12} = \begin{bmatrix} \sigma _{a}^{2} & 0 & 0 &0 \\ 0 & \sigma _{w}^{2} &0 & 0 \\  0 & 0 &\sigma_{ba}^{2} & 0 \\ 0 & 0 &0 & \sigma_{bw} ^{2} \end{bmatrix}$
同时也可以根据递推方程得到对应的雅克比矩阵迭代公式：【论文式[11]】

$J_{t+\delta t} = (I+F_{t} \delta t) J_{t}$ 其中初始雅克比矩阵为：$J_{b_{k}} = I$。

关于公式$(6)$的推导：

先把之前的预积分再写一遍：

$\alpha^{b_{k}}_{b_{k+1}} = \int \int _{t\in [t_{k}, t_{k+1}]} R_{t}^{b_{k}} (\hat{a}_{t}-b_{at}-n_{a}) dt^{2}$ $\beta_{b_{k+1}}^{b_{k}} = \int _{t\in [t_{k}, t_{k+1}]}R_{t}^{b_{k}}(\hat{a}_{t}-b_{at}-n_{a})dt$ $\gamma_{b_{k+1}} = \int _{t \in [t_{k}, t_{k+1}]} \frac{1}{2} \Omega(\hat{w}_{t}-b_{wt}-n_{w}) \gamma_{t}^{b_{k}}dt$ 1）首先对于$\delta \dot{\alpha}_{t}^{b_{k}}$、$\delta \dot {b}_{at}$、$\delta \dot{b}_{wt}$, 显然根据定义有：

$\delta \dot{\alpha}_{t}^{b_{k}} = \hat{\dot {\alpha}}_{t}^{b_{k}} - \dot {\alpha}_{t}^{b_{k}} =  \hat{\beta}_{t}^{b_{k}} - \beta_{t}^{b_{k}} = \delta \beta_{t}^{b_{k}}$ $\delta \dot{b}_{at} = \dot{b}_{at} - 0 = n_{b_{a}}$