IMU预积分模型

Catalogue

1. 1. 什么是预积分
2. 2. 预积分计算
3. 3. 预积分更新
4. 4. 预积分(误差)协方差计算
   1. 4.1. 预积分微分方程
      1. 4.1.1. $\delta \dot{\theta}_t^{b_k}$微分方程推导
      2. 4.1.2. $\delta \dot{\beta}_t^{b_k}$微分方程推导
      3. 4.1.3. $\delta \dot{\alpha}_t^{b_k}$微分方程推导
   2. 4.2. 预积分离散时间递推方程
      1. 4.2.1. $\delta \boldsymbol{\theta}_{k+1}$递推
      2. 4.2.2. $\delta \boldsymbol{\beta}_{k+1}$递推
      3. 4.2.3. $\delta \boldsymbol{\alpha}_{k+1}$递推
   3. 4.3. 预积分方差计算
   4. 4.4. 预积分残差关于待求状态量的雅可比
      1. 4.4.1. $\mathbf{r}_{p}$对状态量的雅可比
         1. 4.4.1.1. 对状态i求雅可比
         2. 4.4.1.2. 对状态j求雅可比
      2. 4.4.2. $\mathbf{r}_{q}$对状态量的雅可比
         1. 4.4.2.1. 对状态i求雅可比
         2. 4.4.2.2. 对状态j求雅可比
      3. 4.4.3. $\mathbf{r}_{v}$对状态量的雅可比
         1. 4.4.3.1. 对状态i求雅可比
         2. 4.4.3.2. 对状态j求雅可比
      4. 4.4.4. $\mathbf{r}_{ba}$对状态量的雅可比
         1. 4.4.4.1. 对状态i求雅可比
         2. 4.4.4.2. 对状态j求雅可比
      5. 4.4.5. $\mathbf{r}_{bg}$对状态量的雅可比
         1. 4.4.5.1. 对状态i求雅可比
         2. 4.4.5.2. 对状态j求雅可比
      6. 4.4.6. 小结
      7. 4.4.7. 残差
      8. 4.4.8. 待优化变量
      9. 4.4.9. 雅可比

什么是预积分

即对两个关键帧之间的imu数据进行整合，得到一个factor，并且该factor基本上不随两个关键帧的状态改变而变化

预积分计算

回顾惯导解算，已知系统位置、速度、姿态的微分方程如下：

$$\begin{array}{l} \dot{\mathbf{p}}_{w b_{t}}=\mathbf{v}_{t}^{w} \\ \dot{\mathbf{v}}_{t}^{w}=\mathbf{a}_{t}^{w} \\ \dot{\mathbf{q}}_{w b_{t}}=\mathbf{q}_{w b_{t}} \otimes\left[\begin{array}{c} 0 \\ \frac{1}{2} \boldsymbol{\omega}^{b_{t}} \end{array}\right] \end{array}$$

基于上述微分方程，可以得到连续时间下的状态传播方程：

$$\mathbf{p}_{w b_{j}}= \mathbf{p}_{w b_{i}}+\mathbf{v}_{i}^{w} \Delta t+\iint_{t \in[i, j]}\left(\mathbf{q}_{w b_{t}} \mathbf{a}^{b_{t}}-\mathbf{g}^{w}\right) \delta t^{2}$$

$$\mathbf{v}_{j}^{w}= \mathbf{v}_{i}^{w}+\int_{t \in[i, j]}\left(\mathbf{q}_{w b_{t}} \mathbf{a}^{b_{t}}-\mathbf{g}^{w}\right) \delta t$$

$$\mathbf{q}_{w b_{j}}=\int_{t \in[i, j]} \mathbf{q}_{w b_{t}} \otimes\left[\begin{array}{c}0 \\ \frac{1}{2} \boldsymbol{\omega}^{b_{t}}\end{array}\right] \delta t$$

由于上面三个公式都与前一个时刻的状态有关，因此需要进行变换：

根据$\mathbf{q}_{w b_{t}}=\mathbf{q}_{w b_{i}} \otimes \mathbf{q}_{b_{i} b_{t}}$，即可将上面三个公式中的前一个时刻的位姿项提取出来：

$$\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}+\mathbf{q}_{w b_{i}} \underbrace{\iint_{t \in[i, j]}\left(\mathbf{q}_{b_{i} b_{t}} \mathbf{a}^{b_{t}}\right) \delta t^{2}}_{\mathbf{\alpha}}$$

$$\mathbf{v}_{j}^{w}=\mathbf{v}_{i}^{w}-\mathbf{g}^{w} \Delta t+\mathbf{q}_{w b_{i}}\underbrace{\int_{t \in[i, j]}\left(\mathbf{q}_{b_{i} b_{t}} \mathbf{a}^{b_{t}}\right) \delta t}_{\mathbf{\beta}}$$

$$\mathbf{q}_{w b_{j}}=\mathbf{q}_{w b_{i}}\underbrace{\int_{t \in[i, j]} \mathbf{q}_{b_{i} b_{t}} \otimes\left[\begin{array}{c}0 \\ \frac{1}{2} \boldsymbol{\omega}^{b_{t}}\end{array}\right] \delta t}_{\mathbf{\gamma}}$$

将上式的积分项分别提取出来，有：

$$\boldsymbol{\alpha}_{b_{i} b_{j}}=\iint_{t \in[i, j]}\left(\mathbf{q}_{b_{i} b_{t}} \mathbf{a}^{b_{t}}\right) \delta t^{2}$$

$$\boldsymbol{\beta}_{b_{i} b_{j}}=\int_{t \in[i, j]}\left(\mathbf{q}_{b_{i} b_{t}} \mathbf{a}^{b_{t}}\right) \delta t$$

$$\mathbf{q}_{b_{i} b_{j}}=\int_{t \in[i, j]} \mathbf{q}_{b_{i} b_{t}} \otimes \left[ \begin{array}{c} 0 \\ \frac{1}{2} \boldsymbol{\omega}^{b_{t}} \end{array} \right] \delta t$$

由于上面的推导都是基于连续时间下的，在实际使用中，通常使用离散形式计算，采用中值积分法：

$$\boldsymbol{\omega}=\frac{1}{2}\left[\left(\boldsymbol{\omega}^{b_{k}}-\mathbf{b}_{k}^{g}\right)+\left(\boldsymbol{\omega}^{b_{k+1}}-\mathbf{b}_{k}^{g}\right)\right]$$

$$\mathbf{a}=\frac{1}{2}\left[\mathbf{q}_{b_{i} b_{k}}\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right)+\mathbf{q}_{b_{i} b_{k+1}}\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]$$

那么预积分量$\boldsymbol{\alpha}_{b_{i} b_{j}},\boldsymbol{\beta}_{b_{i} b_{j}},\mathbf{q}_{b_{i} b_{j}}$可以通过迭代计算得到，当新的一帧imu数据到达时，计算该imu预积分的第k+1次迭代，有：

$$\boldsymbol{\alpha}_{b_{i} b_{k+1}}=\boldsymbol{\alpha}_{b_{i} b_{k}}+\boldsymbol{\beta}_{b_{i} b_{k}} \delta t+\frac{1}{2} \mathbf{a} \delta t^{2}$$

$$\boldsymbol{\beta}_{b_{i} b_{k+1}}=\boldsymbol{\beta}_{b_{i} b_{k}}+\mathbf{a} \delta t$$

$$\mathbf{q}_{b_{i} b_{k+1}}=\mathbf{q}_{b_{i} b_{k}} \otimes \left[ \begin{array}{c} 1 \\ \frac{1}{2} \boldsymbol{\omega} \delta t \end{array} \right]$$

预积分量计算完成

基于预积分量的导航状态更新公式为：

$$\left[ \begin{array}{c} \mathbf{p}_{w b_{j}} \\ \mathbf{v}_{j}^{w} \\ \mathbf{q}_{w b_{j}} \\ \mathbf{b}_{j}^{a} \\ \mathbf{b}_{j}^{g} \end{array} \right] = \left[ \begin{array}{c} \mathbf{p}_{w b_{i}}+\mathbf{v}_{i}^{w} \Delta t-\frac{1}{2} \mathbf{g}^{w} \Delta t^{2}+\mathbf{q}_{w b_{i}} \boldsymbol{\alpha}_{b_{i} b_{j}} \\ \mathbf{v}_{i}^{w}-\mathbf{g}^{w} \Delta t+\mathbf{q}_{w b_{i}} \boldsymbol{\beta}_{b_{i} b_{j}} \\ \mathbf{q}_{w b_{i}} \mathbf{q}_{b_{i} b_{j}} \\ \mathbf{b}_{i}^{a} \\ \mathbf{b}_{i}^{g} \end{array} \right]$$

此中，陀螺仪和加速度计的零偏不变是认为在这个预积分中，由于时间很短，bias基本没有变化。然而，在整个系统中，其实是认为bias在缓慢变化的，因此，陀螺仪加速度计的模型为：

预积分更新

从上面的推导可以发现，预积分量中包含了bias，而在后续的优化过程中，bias作为待优化状态量会随优化而发生改变，因此，预积分量应该随之更新，为了避免完全重新计算预积分，一个技巧是把预积分结果在bias处进行泰勒展开，通过线性近似得到更新的预积分：

$$\boldsymbol{\alpha}_{b_{i} b_{j}}=\overline{\boldsymbol{\alpha}}_{b_{i} b_{j}}+\mathbf{J}_{b_{i}^{a}}^{\alpha} \boldsymbol{b}_{i}^{a}+\mathbf{J}_{b_{i}^{g}}^{\alpha} \delta \mathbf{b}_{i}^{g}$$

$$\boldsymbol{\beta}_{b_{i} b_{j}}=\overline{\boldsymbol{\beta}}_{b_{i} b_{j}}+\mathbf{J}_{b_{i}^{a}}^{\beta} \delta \mathbf{b}_{i}^{a}+\mathbf{J}_{b_{i}^{g}}^{\beta} \delta \mathbf{b}_{i}^{g}$$

$$\mathbf{q}_{b_{i} b_{j}}=\overline{\mathbf{q}}_{b_{i} b_{j}} \otimes \left[ \begin{array}{c} 1 \\ \frac{1}{2} \mathbf{J}_{b_{i}^{g}}^{q} \delta \mathbf{b}_{i}^{g} \end{array} \right]$$

其中，（详细计算见后续）

$$\begin{aligned} \mathbf{J}_{b_{i}^{a}}^{\alpha} &=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{j}}}{\partial \delta \mathbf{b}_{i}^{a}} \\ \mathbf{J}_{b_{i}^{g}}^{\alpha} &=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{j}}}{\partial \delta \mathbf{b}_{i}^{g}} \\ \mathbf{J}_{b_{i}^{a}}^{\beta} &=\frac{\partial \boldsymbol{\beta}_{b_{i} b_{j}}}{\partial \delta \mathbf{b}_{i}^{a}} \\ \mathbf{J}_{b_{i}^{g}}^{\beta} &=\frac{\partial \beta_{b_{i} b_{j}}}{\partial \delta \mathbf{b}_{i}^{g}} \\ \mathbf{J}_{b_{i}^{g}}^{q} &=\frac{\mathbf{q}_{i_{i} b_{j}}}{\partial \mathbf{b}_{i}^{g}} \end{aligned}$$

预积分(误差)协方差计算

由于预积分量是由imu数据迭代计算得到的，然而imu单次测量包含噪声，时间越长，预积分量越不准确，因此，需要计算对应的协方差来表示其不确定性，方差计算公式如下：

$$\boldsymbol{P}_{i, k+1}=\mathbf{F}_{k} \boldsymbol{P}_{i, k} \mathbf{F}_{k}^{\top}+\mathbf{G}_{k} \boldsymbol{Q} \mathbf{G}_{k}^{\top}$$

注意：上式的$\mathbf{F}_{k},\mathbf{G}_{k}$是离散时间下的状态转移矩阵

下面需要求关于预积分误差的微分方程：

已知连续时间下的微分方程形式为：

$$\dot{\boldsymbol{X}}=\boldsymbol{F}_{t} \boldsymbol{X}+\boldsymbol{G}_{t} \boldsymbol{N}$$

其中，

$$\boldsymbol{X}= \left[ \begin{array}{l} \delta \boldsymbol{\alpha}_{t}^{b_{k}} \\ \delta \boldsymbol{\theta}_{t}^{b_{k}} \\ \delta \boldsymbol{\beta}_{t}^{b_{k}} \\ \delta \boldsymbol{b}_{a_{t}} \\ \delta \boldsymbol{b}_{w_{t}} \end{array} \right]$$

$$\boldsymbol{N}= \left[ \begin{array}{l} \boldsymbol{n}_{a} \\ \boldsymbol{n}_{w} \\ \boldsymbol{n}_{b_{a}} \\ \boldsymbol{n}_{b_{w}} \end{array} \right]$$

预积分微分方程

$\delta \dot{\theta}_t^{b_k}$微分方程推导

符号简化：$\delta \dot{\theta}_t^{b_k} \rightarrow \delta \dot \theta$

此处的$\boldsymbol{q}_{t}$就是上面的$\mathbf{q}_{b_{i} b_{k+1}}$

1. 写出不考虑误差的微分方程

$$\dot{\boldsymbol{q}}_{t}=\frac{1}{2} \boldsymbol{q}_{t} \otimes \left[ \begin{array}{c} 0 \\ \boldsymbol{\omega}_{t}-\boldsymbol{b}_{\omega_{t}} \end{array} \right]$$

2. 写出考虑误差的微分方程

$$\dot{\tilde{\boldsymbol{q}}}_{t}=\frac{1}{2} \tilde{\boldsymbol{q}}_{t} \otimes \left[ \begin{array}{c} 0 \\ \tilde{\boldsymbol{\omega}}_{t}-\tilde{\boldsymbol{b}}_{\omega_{t}} \end{array} \right]$$

3. 写出带有误差的参数与理想真实值之间的关系

$$\tilde{\boldsymbol{q}}_{t}=\boldsymbol{q}_{t} \otimes \delta \boldsymbol{q}$$

$$\tilde{\boldsymbol{\omega}}_{t}=\boldsymbol{\omega}_{t}+\boldsymbol{n}_{\omega}$$

$$\tilde{\boldsymbol{b}}_{\omega_{t}}=\boldsymbol{b}_{\omega_{t}}+\delta \boldsymbol{b}_{\omega_{t}}$$

其中，$\delta \theta$是计算坐标系与真实导航坐标系的偏差 或者 在body系与计算误差body系之间的偏差

$$\delta \boldsymbol{q}= \left[ \begin{array}{c} \cos \left(\frac{|\delta \theta|}{2}\right) \\ \frac{\delta \boldsymbol{\theta}}{|\delta \theta|} \sin \left(\frac{|\delta \theta|}{2}\right) \end{array} \right] \approx \left[ \begin{array}{c} 1 \\ \frac{\delta \boldsymbol{\theta}}{2} \end{array} \right]$$

4. 将误差值与理想真实值的关系代入(2)

$$\left(\boldsymbol{q}_{t} \dot{\otimes} \delta \boldsymbol{q}\right)= \frac{1}{2} \boldsymbol{q}_{t} \otimes \delta \boldsymbol{q} \otimes \left[ \begin{array}{c} 0 \\ \boldsymbol{\omega}_{t}+\boldsymbol{n}_{\omega}-\boldsymbol{b}_{\omega_{t}}-\delta \boldsymbol{b}_{\omega_{t}} \end{array} \right]$$

其中，

$$\left(\boldsymbol{q}_{t} \dot{\otimes} \delta \boldsymbol{q}\right)=\dot{\boldsymbol{q}}_{t} \otimes \delta \boldsymbol{q}+\boldsymbol{q}_{t} \otimes \delta \dot{\boldsymbol{q}}$$

5. 把(1)中的关系代入(4)

$$\begin{aligned} \left(\boldsymbol{q}_{t} \dot{\otimes} \delta \boldsymbol{q}\right) &= \frac{1}{2} \boldsymbol{q}_{t} \otimes \delta \boldsymbol{q} \otimes\left[\begin{array}{c}0 \\ \boldsymbol{\omega}_{t}+\boldsymbol{n}_{\omega}-\boldsymbol{b}_{\omega_{t}}-\delta \boldsymbol{b}_{\omega_{t}}\end{array}\right] \\ \dot{\boldsymbol{q}}_{t} \otimes \delta \boldsymbol{q}+\boldsymbol{q}_{t} \otimes \delta \boldsymbol{q} &= \\ \frac{1}{2} \boldsymbol{q}_{t} \otimes\left[\begin{array}{c}0 \\ \boldsymbol{\omega}_{t}-\boldsymbol{b}_{\omega_{t}}\end{array}\right] \otimes \delta \boldsymbol{q}+\boldsymbol{q}_{t} \otimes \dot{\delta \boldsymbol{q}} &= \end{aligned}$$

6. 化简

(5)两边同时左乘$(\boldsymbol{q}_t)^{-1}$，然后移项得到：

$$\delta \dot{\boldsymbol{q}}= \frac{1}{2} \delta \boldsymbol{q} \otimes \left[ \begin{array}{c} 0 \\ \boldsymbol{\omega}_{t}+\boldsymbol{n}_{\omega}-\boldsymbol{b}_{\omega_{t}}-\delta \boldsymbol{b}_{\omega_{t}} \end{array} \right] -\frac{1}{2} \left[ \begin{array}{c} 0 \\ \boldsymbol{\omega}_{t}-\boldsymbol{b}_{\omega_{t}} \end{array} \right] \otimes \delta \boldsymbol{q}$$

根据四元数乘法性质，可以将四元数乘法转换成矩阵与向量相乘：

因此，可得：

$$\begin{aligned} \delta \dot{\boldsymbol{q}} &= \frac{1}{2} \left[ \begin{array}{c}0 \\ \boldsymbol{\omega}_{1} \end{array} \right]_{R} \delta \boldsymbol{q} - \frac{1}{2} \left[ \begin{array}{c}0 \\ \boldsymbol{\omega}_{2} \end{array} \right]_{L} \delta \boldsymbol{q} \\ &= \frac{1}{2} \left[ \begin{array}{cc} 0 & \left(\boldsymbol{\omega}_{2}-\boldsymbol{\omega}_{1}\right)^{T} \\ \left(\boldsymbol{\omega}_{1}-\boldsymbol{\omega}_{2}\right) & -\left[\boldsymbol{\omega}_{1}+\boldsymbol{\omega}_{2}\right]_{\times} \end{array} \right] \delta \boldsymbol{q} \end{aligned}$$

其中,

$$\boldsymbol{\omega}_{1}=\boldsymbol{\omega}_{t}+\boldsymbol{n}_{\omega}-\boldsymbol{b}_{\omega_{t}}-\delta \boldsymbol{b}_{\omega_{t}}$$

$$\boldsymbol{\omega}_{2}=\boldsymbol{\omega}_{t}-\boldsymbol{b}_{\omega_{t}}$$

又因为：

$$\delta \dot{\boldsymbol{q}}= \left[ \begin{array}{l} 0 \\ \frac{\delta \dot{\theta}}{2} \end{array} \right]$$

可以得到关于$\delta \dot{\theta}$的方程：

$$\begin{aligned} \delta \dot{\boldsymbol{\theta}} &= -\left[\boldsymbol{\omega}_{1}+\boldsymbol{\omega}_{2}\right] \times \frac{\delta \boldsymbol{\theta}}{2}+\left(\boldsymbol{\omega}_{1}-\boldsymbol{\omega}_{2}\right) \\ &= -\left[2 \boldsymbol{\omega}_{t}+\boldsymbol{n}_{\omega}-2 \boldsymbol{b}_{\omega_{t}}-\delta \boldsymbol{b}_{\omega_{t}}\right]_\times \frac{\delta \boldsymbol{\theta}}{2}+\boldsymbol{n}_{\omega}-\delta \boldsymbol{b}_{\omega_{t}} \end{aligned}$$

忽略上式中的二阶小项，可得$\delta \dot{\theta}_t^{b_k}$微分方程

$$\delta \dot{\boldsymbol{\theta}}=-\left[\boldsymbol{\omega}_{t}-\boldsymbol{b}_{\omega_{t}}\right]_{\times} \delta \boldsymbol{\theta}+\boldsymbol{n}_{\omega}-\delta \boldsymbol{b}_{\omega_{t}}$$

$\delta \dot{\beta}_t^{b_k}$微分方程推导

符号简化：$\delta \dot{\beta}_t^{b_k} \rightarrow \delta \dot \beta$，此处的$\boldsymbol{\beta}$就是上面的$\boldsymbol{\beta}_{b_{i} b_{k+1}}$

1. 写出不考虑误差的微分方程

$$\dot{\boldsymbol{\beta}}=\boldsymbol{R}_{t}\left(\boldsymbol{a}_{t}-\boldsymbol{b}_{a_{t}}\right)$$

其中，$\boldsymbol{R}_{t}$表示载体姿态

2. 写出考虑误差的微分方程

$$\dot{\tilde{\boldsymbol{\beta}}}=\tilde{\boldsymbol{R}}_{t}\left(\tilde{\boldsymbol{a}}_{t}-\tilde{\boldsymbol{b}}_{a_{t}}\right)$$

3. 写出带有误差的参数与理想真实值之间的关系

$$\tilde{\boldsymbol{\beta}}=\boldsymbol{\beta}+\delta \boldsymbol{\beta}$$

$$\tilde{\boldsymbol{a}}_{t}=\boldsymbol{a}_{t}+\boldsymbol{n}_{a}$$

$$\tilde{\boldsymbol{b}}_{a_{t}}=\boldsymbol{b}_{a_{t}}+\delta \boldsymbol{b}_{a_{t}}$$

$$\begin{aligned} \tilde{\boldsymbol{R}}_{t} &=\boldsymbol{R}_{t} \exp \left([\delta \boldsymbol{\theta}]_{\times}\right) \\ &=\boldsymbol{R}_{t}\left(\boldsymbol{I}+[\delta \boldsymbol{\theta}]_{\times}\right) \end{aligned}$$

关于姿态与理想真实值的关系： 因为 $\delta \boldsymbol{\theta}$表示的是在载体姿态上的误差，直接右乘即可

4. 将误差值与理想真实值的关系代入(2)

$$\begin{aligned} & \dot{\boldsymbol{\beta}}+\delta \dot{\boldsymbol{\beta}} \\ =& \boldsymbol{R}_{t}\left(\boldsymbol{I}+[\delta \boldsymbol{\theta}]_{\times}\right)\left(\boldsymbol{a}_{t}+\boldsymbol{n}_{a}-\boldsymbol{b}_{a_{t}}-\delta \boldsymbol{b}_{a_{t}}\right) \end{aligned}$$

5. 把(1)中的关系代入(4)

$$\begin{aligned} & \boldsymbol{R}_{t}\left(\boldsymbol{a}_{t}-\boldsymbol{b}_{a_{t}}\right)+\delta \boldsymbol{\beta} \\ =& \boldsymbol{R}_{t}\left(\boldsymbol{I}+[\delta \boldsymbol{\theta}]_{\times}\right)\left(\boldsymbol{a}_{t}+\boldsymbol{n}_{a}-\boldsymbol{b}_{a_{t}}-\delta \boldsymbol{b}_{a_{t}}\right) \end{aligned}$$

6. 化简（忽略二阶小项）

$$\begin{aligned} & \delta \dot{\boldsymbol{\beta}} \\ =& \boldsymbol{R}_{t}[\delta \boldsymbol{\theta}]_{\times}\left(\boldsymbol{a}_{t}-\boldsymbol{b}_{a_{t}}\right)+\boldsymbol{R}_{t}\left(\boldsymbol{n}_{a}-\delta \boldsymbol{b}_{a_{t}}\right) \\ =&-\boldsymbol{R}_{t}\left[\boldsymbol{a}_{t}-\boldsymbol{b}_{a_{t}}\right]_{\times} \delta \boldsymbol{\theta}+\boldsymbol{R}_{t}\left(\boldsymbol{n}_{a}-\delta \boldsymbol{b}_{a_{t}}\right) \end{aligned}$$

$\delta \dot{\alpha}_t^{b_k}$微分方程推导

符号简化：$\delta \dot{\alpha}_t^{b_k} \rightarrow \delta \dot \alpha$

1. 写出不考虑误差的微分方程

$$\dot{\alpha}=\boldsymbol{\beta}$$

2. 写出考虑误差的微分方程

$$\dot{\tilde{\boldsymbol{\alpha}}}=\tilde{\boldsymbol{\beta}}$$

3. 写出带有误差的参数与理想真实值之间的关系

$$\begin{aligned} \tilde \alpha = \alpha + \delta \alpha \\ \tilde \beta = \beta + \delta \beta \end{aligned}$$

4. 将误差值与理想真实值的关系代入(2)

$$\begin{aligned} \dot {(\alpha + \delta \alpha)} = \beta + \delta \beta \\ \dot {\alpha} + \dot{\delta \alpha} = \beta + \delta \beta \end{aligned}$$

5. 把(1)中的关系代入(4)

$$\boldsymbol{\beta}+\delta \dot{\boldsymbol{\alpha}}=\boldsymbol{\beta}+\delta \boldsymbol{\beta}$$

6. 化简

$$\delta \dot{\boldsymbol{\alpha}}=\delta \boldsymbol{\beta}$$

预积分离散时间递推方程

实际上，计算预积分的方差，还需要离散时间下的递推方程，因此，需要对上面求出来的连续时间微分方程进行离散化，即：

已知连续时间下的微分方程形式为：

$$\dot{\boldsymbol{X}}=\boldsymbol{F}_{t} \boldsymbol{X}+\boldsymbol{G}_{t} \boldsymbol{N}$$

离散化后的递推方程形式为：

$$\boldsymbol{X}_{k+1}=\boldsymbol{F}_{k} \boldsymbol{X}_{k}+\boldsymbol{G}_{k} \boldsymbol{N}_{k}$$

其中，

$$\boldsymbol{X}_{k+1}=\left[\begin{array}{c} \delta \boldsymbol{\alpha}_{k+1} \\ \delta \boldsymbol{\theta}_{k+1} \\ \delta \boldsymbol{\beta}_{k+1} \\ \delta \boldsymbol{b}_{a_{k+1}} \\ \delta \boldsymbol{b}_{\omega_{k+1}} \end{array}\right] \quad \boldsymbol{X}_{k}=\left[\begin{array}{c} \delta \boldsymbol{\alpha}_{k} \\ \delta \boldsymbol{\theta}_{k} \\ \delta \boldsymbol{\beta}_{k} \\ \delta \boldsymbol{b}_{a_{k}} \\ \delta \boldsymbol{b}_{\omega_{k}} \end{array}\right] \quad \boldsymbol{N}_{k}=\left[\begin{array}{c} \boldsymbol{n}_{a_{k}} \\ \boldsymbol{n}_{w_{k}} \\ \boldsymbol{n}_{a_{k+1}} \\ \boldsymbol{n}_{w_{k+1}} \\ \boldsymbol{n}_{b_{a}} \\ \boldsymbol{n}_{b_{w}} \end{array}\right]$$

more detail:

$\delta \boldsymbol{\theta}_{k+1}$递推

这里直接写出上面推导的微分方程：

$$\delta \dot{\boldsymbol{\theta}}=-\left[\boldsymbol{\omega}_{t}-\boldsymbol{b}_{\omega_{t}}\right]_{\times} \delta \boldsymbol{\theta}+\boldsymbol{n}_{\omega}-\delta \boldsymbol{b}_{\omega_{t}}$$

改写成中值积分法：

$$\delta \dot{\boldsymbol{\theta}}_{k}=-\left[\frac{\boldsymbol{\omega}_{k}+\boldsymbol{\omega}_{k+1}}{2}-\boldsymbol{b}_{\omega_{t}}\right]_{\times} \delta \boldsymbol{\theta}_{k}+\boldsymbol{n}_{\omega}-\delta \boldsymbol{b}_{\omega_{t}}$$

因此，离散时间下，有：

$$\begin{aligned} \delta \boldsymbol{\theta}_{k+1}&= \delta \boldsymbol{\theta}_{k} + \left ( -\left[\frac{\boldsymbol{\omega}_{k}+\boldsymbol{\omega}_{k+1}}{2}-\boldsymbol{b}_{\omega_{t}}\right]_{\times} \delta \boldsymbol{\theta}_{k}+\boldsymbol{n}_{\omega}-\delta \boldsymbol{b}_{\omega_{t}} \right) \delta t \\ &=\left[\boldsymbol{I}-\left[\frac{\boldsymbol{\omega}_{k}+\boldsymbol{\omega}_{k+1}}{2}-\boldsymbol{b}_{\omega_{k}}\right]_{\times} \delta t\right] \delta \boldsymbol{\theta}_{k}+\delta t \frac{\boldsymbol{n}_{\omega_{k}}+\boldsymbol{n}_{\omega_{k+1}}}{2}-\delta t \delta \boldsymbol{b}_{\omega_{k}} \end{aligned}$$

令: $\overline{\boldsymbol{\omega}}=\frac{\boldsymbol{\omega}_{k}+\boldsymbol{\omega}_{k+1}}{2}-\boldsymbol{b}_{\omega_{k}}$，则上式可重写为:

$$\delta \boldsymbol{\theta}_{k+1}=\left[\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right] \delta \boldsymbol{\theta}_{k}+\frac{\delta t}{2} \boldsymbol{n}_{\omega_{k}}+\frac{\delta t}{2} \boldsymbol{n}_{\omega_{k+1}}-\delta t \delta \boldsymbol{b}_{\omega_{k}}$$

$\delta \boldsymbol{\beta}_{k+1}$递推

这里直接写出上面推导的微分方程，即连续时间下，有：

$$\delta \dot{\boldsymbol{\beta}}=-\boldsymbol{R}_{t}\left[\boldsymbol{a}_{t}-\boldsymbol{b}_{a_{t}}\right]_{\times} \delta \boldsymbol{\theta}+\boldsymbol{R}_{t}\left(\boldsymbol{n}_{a}-\delta \boldsymbol{b}_{a_{t}}\right)$$

改成中值积分法形式，有：

$$\begin{aligned} \delta \dot{\boldsymbol{\beta}}_{k}=&-\frac{1}{2} \boldsymbol{R}_{k}\left[\boldsymbol{a}_{k}-\boldsymbol{b}_{a_{k}}\right]_{\times} \delta \boldsymbol{\theta}_{k} \\ &-\frac{1}{2} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \delta \boldsymbol{\theta}_{k+1} \\ &+\frac{1}{2} \boldsymbol{R}_{k} \boldsymbol{n}_{a_{k}} +\frac{1}{2} \boldsymbol{R}_{k+1} \boldsymbol{n}_{a_{k+1}} \\ &-\frac{1}{2}\left(\boldsymbol{R}_{k}+\boldsymbol{R}_{k+1}\right) \delta \boldsymbol{b}_{a_{k}} \end{aligned}$$

根据上述$\delta \boldsymbol{\beta}_{k+1}$递推的结果，$\delta \boldsymbol{\theta}_{k+1}$如下：

$$\delta \boldsymbol{\theta}_{k+1}=\left[\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right] \delta \boldsymbol{\theta}_{k}+\frac{\delta t}{2} \boldsymbol{n}_{\omega_{k}}+\frac{\delta t}{2} \boldsymbol{n}_{\omega_{k+1}}-\delta t \delta \boldsymbol{b}_{\omega_{k}}$$

其中，$\overline{\boldsymbol{\omega}}=\frac{\boldsymbol{\omega}_{k}+\boldsymbol{\omega}_{k+1}}{2}-\boldsymbol{b}_{\omega_{k}}$

将$\delta \boldsymbol{\theta}_{k+1}$代入到$\delta \dot{\boldsymbol{\beta}}_{k}$，可得：

$$\begin{aligned} \delta \dot{\boldsymbol{\beta}}_{k}=&-\frac{1}{2} \boldsymbol{R}_{k}\left[\boldsymbol{a}_{k}-\boldsymbol{b}_{a_{k}}\right]_{\times} \delta \boldsymbol{\theta}_{k} \\ &-\frac{1}{2} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times}\left\{\left[\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right] \delta \boldsymbol{\theta}_{k}+\frac{\delta t}{2} \boldsymbol{n}_{\omega_{k}}+\frac{\delta t}{2} \boldsymbol{n}_{\omega_{k+1}}-\delta t \delta \boldsymbol{b}_{\omega_{k}}\right\} \\ &+\frac{1}{2} \boldsymbol{R}_{k} \boldsymbol{n}_{a_{k}} +\frac{1}{2} \boldsymbol{R}_{k+1} \boldsymbol{n}_{a_{k+1}} \\ &-\frac{1}{2}\left(\boldsymbol{R}_{k}+\boldsymbol{R}_{k+1}\right) \delta \boldsymbol{b}_{a_{k}} \end{aligned}$$

进一步化简，合并同类项，可以得到微分方程中间结果：

$$\begin{aligned} \delta \dot{\boldsymbol{\beta}}_{k}=&-\frac{1}{2}\left[\boldsymbol{R}_{k}\left[\boldsymbol{a}_{k}-\boldsymbol{b}_{a_{k}}\right]_{\times}+\boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times}\left(\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right)\right] \delta \boldsymbol{\theta}_{k} \\ &-\frac{\delta t}{4} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \boldsymbol{n}_{\omega_{k}} \\ &-\frac{\delta t}{4} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \boldsymbol{n}_{\omega_{k+1}} \\ &+\frac{\delta t}{2} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \delta \boldsymbol{b}_{\omega_{k}} \\ &+\frac{1}{2} \boldsymbol{R}_{k} \boldsymbol{n}_{a_{k}} \\ &+\frac{1}{2} \boldsymbol{R}_{k+1} \boldsymbol{n}_{a_{k+1}} \\ &-\frac{1}{2}\left(\boldsymbol{R}_{k}+\boldsymbol{R}_{k+1}\right) \delta \boldsymbol{b}_{a_{k}} \end{aligned}$$

对$\delta t$进行积分，即由导数形式可以得到递推形式如下：

$$\begin{aligned} \delta \boldsymbol{\beta}_{k+1} = & \enspace \delta \boldsymbol{\beta}_{k} + \delta \dot{\boldsymbol{\beta}}_{k} \delta t \\ =& \enspace \delta \boldsymbol{\beta}_{k} \\ &-\frac{\delta t}{2}\left[\boldsymbol{R}_{k}\left[\boldsymbol{a}_{k}-\boldsymbol{b}_{a_{k}}\right]_{\times}+\boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times}\left(\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right)\right] \delta \boldsymbol{\theta}_{k} \\ &-\frac{\delta t^{2}}{4} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \boldsymbol{n}_{\omega_{k}} \\ &-\frac{\delta t^{2}}{4} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \boldsymbol{n}_{\omega_{k+1}} \\ &+\frac{\delta t^{2}}{2} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \delta \boldsymbol{b}_{\omega_{k}} \\ &+\frac{\delta t}{2} \boldsymbol{R}_{k} \boldsymbol{n}_{a_{k}} \\ &+\frac{\delta t}{2} \boldsymbol{R}_{k+1} \boldsymbol{n}_{a_{k+1}} \\ &-\frac{\delta t}{2}\left(\boldsymbol{R}_{k}+\boldsymbol{R}_{k+1}\right) \delta \boldsymbol{b}_{a_{k}} \end{aligned}$$

$\delta \boldsymbol{\alpha}_{k+1}$递推

这里直接写出上面推导的微分方程，即连续时间下，有：

$$\delta \dot{\boldsymbol{\alpha}}=\delta \boldsymbol{\beta}$$

改成中值积分法形式，有：

$$\delta \dot{\boldsymbol{\alpha}}_{k}=\frac{1}{2} \delta \boldsymbol{\beta}_{k}+\frac{1}{2} \delta \boldsymbol{\beta}_{k+1}$$

将$\delta \boldsymbol{\beta}_{k+1}$的递推结果代入到可得：

$$\begin{aligned} \delta \dot{\boldsymbol{\alpha}}_{k}=& \frac{1}{2} \delta \boldsymbol{\beta}_{k}+\frac{1}{2} \delta \boldsymbol{\beta}_{k+1} \\ =& \enspace \delta \boldsymbol{\beta}_{k} \\ &-\frac{\delta t}{4}\left[\boldsymbol{R}_{k}\left[\boldsymbol{a}_{k}-\boldsymbol{b}_{a_{k}}\right]_{\times}+\boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times}\left(\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right)\right] \delta \boldsymbol{\theta}_{k} \\ &-\frac{\delta t^{2}}{8} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \boldsymbol{n}_{\omega_{k}} \\ &-\frac{\delta t^{2}}{8} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \boldsymbol{n}_{\omega_{k+1}} \\ &+\frac{\delta t^{2}}{4} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \delta \boldsymbol{b}_{\omega_{k}} \\ &+\frac{\delta t}{4} \boldsymbol{R}_{k} \boldsymbol{n}_{a_{k}} \\ &+\frac{\delta t}{4} \boldsymbol{R}_{k+1} \boldsymbol{n}_{a_{k+1}} \\ &-\frac{\delta t}{4}\left(\boldsymbol{R}_{k}+\boldsymbol{R}_{k+1}\right) \delta \boldsymbol{b}_{a_{k}} \end{aligned}$$

对$\delta t$进行积分，即由导数形式可以得到递推形式如下：

$$\begin{aligned} \delta \boldsymbol{\alpha}_{k+1}= & \enspace \delta \boldsymbol{\alpha}_{k} + \delta \dot{\boldsymbol{\alpha}}_{k} \delta t \\ =& \enspace \delta \boldsymbol{\alpha}_{k} +\delta t \delta \boldsymbol{\beta}_{k} \\ &-\frac{\delta t^{2}}{4}\left[\boldsymbol{R}_{k}\left[\boldsymbol{a}_{k}-\boldsymbol{b}_{a_{k}}\right]_{\times}+\boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times}\left(\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right)\right] \delta \boldsymbol{\theta}_{k} \\ &-\frac{\delta t^{3}}{8} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \boldsymbol{n}_{\omega_{k}} \\ &-\frac{\delta t^{3}}{8} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \boldsymbol{n}_{\omega_{k+1}} \\ &+\frac{\delta t^{3}}{4} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \delta \boldsymbol{b}_{\omega_{k}} \\ &+\frac{\delta t^{2}}{4} \boldsymbol{R}_{k} \boldsymbol{n}_{a_{k}} \\ &+\frac{\delta t^{2}}{4} \boldsymbol{R}_{k+1} \boldsymbol{n}_{a_{k+1}} \\ &-\frac{\delta t^{2}}{4}\left(\boldsymbol{R}_{k}+\boldsymbol{R}_{k+1}\right) \delta \boldsymbol{b}_{a_{k}} \end{aligned}$$

预积分方差计算

由上面的离散时间下的递推方程，可以写出预积分离散时间的状态方程：

$$\boldsymbol{X}_{k+1}=\boldsymbol{F}_{k} \boldsymbol{X}_{k}+\boldsymbol{G}_{k} \boldsymbol{N}_{k}$$

其中$\mathbf{F}_{k}$如下，

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

且：

$$\begin{aligned} &\boldsymbol{f}_{12}=-\frac{\delta t^{2}}{4}\left[\boldsymbol{R}_{k}\left[\boldsymbol{a}_{k}-\boldsymbol{b}_{a_{k}}\right]_{\times}+\boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times}\left(\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right)\right] \\ &\boldsymbol{f}_{15}=\frac{\delta t^{3}}{4} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \delta \boldsymbol{b}_{\omega_{k}} \\ &\boldsymbol{f}_{32}=-\frac{\delta t}{2}\left[\boldsymbol{R}_{k}\left[\boldsymbol{a}_{k}-\boldsymbol{b}_{a_{k}}\right]_{\times}+\boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times}\left(\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right)\right] \\ &\boldsymbol{f}_{35}=\frac{\delta t^{2}}{2} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \end{aligned}$$

其中$\mathbf{G}_{k}$如下，

$$\mathbf{G}_{k}=\left[\begin{array}{cccccc} \frac{1}{4} \boldsymbol{R}_{k} \delta t^{2} & \mathbf{g}_{12} & \frac{1}{4} \boldsymbol{R}_{k+1} \delta t^{2} & \mathbf{g}_{14} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \frac{1}{2} \mathbf{I} \delta t & \mathbf{0} & \frac{1}{2} \mathbf{I} \delta t & \mathbf{0} & \mathbf{0} \\ \frac{1}{2} \boldsymbol{R}_{k} \delta t & \mathbf{g}_{32} & \frac{1}{2} \boldsymbol{R}_{k+1} \delta t & \mathbf{g}_{34} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} \delta t & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} \delta t \end{array}\right]$$

且：

$$\begin{aligned} &\boldsymbol{g}_{12}=-\frac{\delta t^{3}}{8} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \\ &\boldsymbol{g}_{14}=-\frac{\delta t^{3}}{8} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \\ &\boldsymbol{g}_{32}=-\frac{\delta t^{2}}{4} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \\ &\boldsymbol{g}_{34}=-\frac{\delta t^{2}}{4} \boldsymbol{R}_{k+1}\left[\boldsymbol{a}_{k+1}-\boldsymbol{b}_{a_{k}}\right]_{\times} \end{aligned}$$

需要注意的是，上面的$\boldsymbol{R}_{k}, \boldsymbol{R}_{k+1}$都是相对于这个imu预积分起始时刻的，即$\boldsymbol{R}_{k} = \mathbf{R}_{b_{i} b_{k}}, \quad \boldsymbol{R}_{k+1} = \mathbf{R}_{b_{i} b_{k+1}}$

计算得到矩阵$\mathbf{F}_{k}$和$\mathbf{G}_{k}$后，即可按照如下公式来计算方差：

$$\boldsymbol{P}_{i, k+1}=\mathbf{F}_{k} \boldsymbol{P}_{i, k} \mathbf{F}_{k}^{\top}+\mathbf{G}_{k} \boldsymbol{Q} \mathbf{G}_{k}^{\top}$$

预积分残差关于待求状态量的雅可比

在优化时，需要求出残差对待求解的状态量的雅可比。

已知基于预积分量的状态更新如下：

$$\left[\begin{array}{c} \mathbf{p}_{w b_{j}} \\ \mathbf{q}_{w b_{j}} \\ \mathbf{v}_{j}^{w} \\ \mathbf{b}_{j}^{a} \\ \mathbf{b}_{j}^{g} \end{array}\right]=\left[\begin{array}{c} \mathbf{p}_{w b_{i}}+\mathbf{v}_{i}^{w} \Delta t-\frac{1}{2} \mathbf{g}^{w} \Delta t^{2}+\mathbf{q}_{w b_{i}} \boldsymbol{\alpha}_{b_{i} b_{j}} \\ \mathbf{q}_{w b_{i}} \mathbf{q}_{b_{i} b_{j}} \\ \mathbf{v}_{i}^{w}-\mathbf{g}^{w} \Delta t+\mathbf{q}_{w b_{i}} \boldsymbol{\beta}_{b_{i} b_{j}} \\ \mathbf{b}_{i}^{a} \\ \mathbf{b}_{i}^{g} \end{array}\right]$$

把上式左侧状态移到右侧，在理想的情况下左侧应该只剩下0，但是由于误差的存在，可以使用残差小量$\mathbf{r}$代替，因此有：

$$\left[\begin{array}{c} \mathbf{r}_{p} \\ \mathbf{r}_{q} \\ \mathbf{r}_{v} \\ \mathbf{r}_{b a} \\ \mathbf{r}_{b g} \end{array}\right]=\left[\begin{array}{c} \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}-\mathbf{q}_{w b_{i}} \boldsymbol{\alpha}_{b_{i} b_{j}} \\ 2\left[\mathbf{q}_{b_{i} b_{j}}^{*} \otimes\left(\mathbf{q}_{w b_{i}}^{*} \otimes \mathbf{q}_{w b_{j}}\right)\right]_{x y z} \\ \mathbf{v}_{j}^{w}-\mathbf{v}_{i}^{w}+\mathbf{g}^{w} \Delta t-\mathbf{q}_{w b_{i}} \boldsymbol{\beta}_{b_{i} b_{j}} \\ \mathbf{b}_{j}^{a}-\mathbf{b}_{i}^{a} \\ \mathbf{b}_{j}^{g}-\mathbf{b}_{i}^{g} \end{array}\right]$$

---

其中，关于姿态残差$\mathbf{r}_{q}$部分，需要将四元数拆开来看，根据四元数与等轴旋转矢量$\mathbf{\phi}$的关系：

$$\mathbf{q}=\cos \frac{\phi}{2}+\left(u_{x} i+u_{y} j+u_{z} k\right) \sin \frac{\phi}{2}=\left[\begin{array}{c} \cos (\phi / 2) \\ \mathbf{u} \sin (\phi / 2) \end{array}\right]$$

等效旋转矢量可以用向量$\mathbf{\phi}$，并用单位向量$\mathbf{u}$表示它的朝向，$\phi$表示它的大小，因此有： $\mathbf{\phi}=\phi \boldsymbol{u}$

其中，

$$\mathbf{r}_{q} = 2\left[\mathbf{q}_{b_{i} b_{j}}^{*} \otimes\left(\mathbf{q}_{w b_{i}}^{*} \otimes \mathbf{q}_{w b_{j}}\right)\right]_{x y z}$$

取$[]_{xyz}$就是取四元数的虚部$\mathbf{u} \sin (\phi / 2)$，特别的，当旋转角度$\phi$是小量时，$\sin (\phi / 2) \approx \phi / 2$，对其乘个2，就得到了上面的姿态残差$\mathbf{r}_{q}$。

---

在上面的预积分误差中，和预积分相关的量，仍然与上一时刻的姿态有关，如$\mathbf{r}_{p},\mathbf{r}_{v}$，无法直接加减（啥意思），因此，把预积分残差进行修正，得到：

$$\left[\begin{array}{c} \mathbf{r}_{p} \\ \mathbf{r}_{q} \\ \mathbf{r}_{v} \\ \mathbf{r}_{b a} \\ \mathbf{r}_{b g} \end{array}\right]=\left[\begin{array}{c} \mathbf{q}_{w b_{i}}^{*}\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)-\boldsymbol{\alpha}_{b_{i} b_{j}} \\ 2\left[\mathbf{q}_{b_{i} b_{j}}^{*} \otimes\left(\mathbf{q}_{w b_{i}}^{*} \otimes \mathbf{q}_{w b_{j}}\right)\right]_{x y z} \\ \mathbf{q}_{w b_{i}}^{*}\left(\mathbf{v}_{j}^{w}-\mathbf{v}_{i}^{w}+\mathbf{g}^{w} \Delta t\right)-\boldsymbol{\beta}_{b_{i} b_{j}} \\ \mathbf{b}_{j}^{a}-\mathbf{b}_{i}^{a} \\ \mathbf{b}_{j}^{g}-\mathbf{b}_{i}^{g} \end{array}\right]$$

其中，待优化的变量是前后两帧的状态量：

$$\left[\begin{array}{lllll} \mathbf{p}_{w b_{i}} & \mathbf{q}_{w b_{i}} & \mathbf{v}_{i}^{w} & \mathbf{b}_{i}^{a} & \mathbf{b}_{i}^{g} \end{array}\right]$$

$$\left[\begin{array}{lllll} \mathbf{p}_{w b_{j}} & \mathbf{q}_{w b_{j}} & \mathbf{v}_{j}^{w} & \mathbf{b}_{j}^{a} & \mathbf{b}_{j}^{g} \end{array}\right]$$

而实际上，求导使用了扰动量，即实际上是对以下扰动量求雅可比：

$$\left[\begin{array}{lllll} \delta \mathbf{p}_{w b_{i}} & \delta \theta_{w b_{i}} & \delta \mathbf{v}_{i}^{w} & \delta \mathbf{b}_{i}^{a} & \delta \mathbf{b}_{i}^{g} \end{array}\right]$$

$$\left[\begin{array}{lllll} \delta \mathbf{p}_{w b_{j}} & \delta \theta_{w b_{j}} & \delta \mathbf{v}_{j}^{w} & \delta \mathbf{b}_{j}^{a} & \delta \mathbf{b}_{j}^{g} \end{array}\right]$$

$\mathbf{r}_{p}$对状态量的雅可比

首先，直接写出残差：

$$\mathbf{r}_{p} = \mathbf{q}_{w b_{i}}^{*}\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)-\boldsymbol{\alpha}_{b_{i} b_{j}}$$

对状态i求雅可比

对状态j求雅可比

$\mathbf{r}_{q}$对状态量的雅可比

首先，直接写出残差：

$$\mathbf{r}_{q} = 2\left[\mathbf{q}_{b_{i} b_{j}}^{*} \otimes\left(\mathbf{q}_{w b_{i}}^{*} \otimes \mathbf{q}_{w b_{j}}\right)\right]_{x y z}$$

对状态i求雅可比

对状态j求雅可比

$\mathbf{r}_{v}$对状态量的雅可比

对状态i求雅可比

对状态j求雅可比

$\mathbf{r}_{ba}$对状态量的雅可比

对状态i求雅可比

对状态j求雅可比

$\mathbf{r}_{bg}$对状态量的雅可比

对状态i求雅可比

对状态j求雅可比

小结

残差

上面与vins-mono代码中 IntegrationBase::evaluate()对应

待优化变量

雅可比

计算 Jacobian 时，残差对应的求偏导对象为上面的优化变量，但是计算时采用扰动方式 计算，即扰动为：

上面公式在vins-mono代码中对应：class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> 对于 Evaluate 输入 double const const parameters, parameters[0], parameters[1], parameters[2], parameters[3]分别对应 4 个输入参数, 它们的长度依次是 7,9,7,9，分别对应 4 个优化变量的 参数块