VINS-Mono之IMU预积分，预积分误差、协方差及误差对状态量雅克比矩阵的递推方程的推导

文章目录

- 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测量值包括加速度计得到的 (测量值) 线加速度 $at^\hat{a_{t}}$ 和陀螺仪得到的角加速度 $w^t\hat{w}_{t}$ 【论文式(1)】
$a^t=at+bat+Rwtgw+na\hat{a}_{t}=a_{t}+b_{at}+R_{w}^{t}g^{w}+n_{a}$ $w^t=wt+bwt+nw\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}}$ 的影响。计算得到的线加速度 $at^\hat{a_{t}}$ 是重力加速度和物体加速度的合矢量。
假设附加噪声为高斯噪声：
$na∼(0,σa2),na∼(0,σw2)n_{a}\sim(0,\sigma_{a}^{2}), \ n_{a}\sim(0,\sigma_{w}^{2})$ 加速度计偏置和陀螺仪偏置被建模为随机游走，其导数为高斯分布：【论文式(2)】
$b˙at=nba,b˙wt=nbw\dot{b}_{at}=n_{ba}, \ \dot{b}_{wt}=n_{bw}$ $nba∼N(0,σba2),nbw∼N(0,σbw2)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)】
$pbk+1w=pbkw+vbkwΔtk+∫∫t∈[tk,tk+1](Rtw(a^t−bat−na)−gw)dt2p^{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}$ $vbk+1w=vbkw+∫t∈[tk,tk+1](Rtw(a^t−bat−na)−gw)dtv_{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$ $qbk+1w=qbkw⊗∫t∈[tk,tk+1]12qtbk⊗[(w^t−bwt−nw)0]dt=qbk⊗∫t∈[tk,tk+1]12Ω(w^t−bwt−nw)qtbkdt(1)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}$ $Ω(w)=[−[w]×w−wT0],[w]×=[0−wzwywz0−wx−wywx0]\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}$ 其中 $Δtk\Delta t_{k}$ 是 $t_{k}, t_{k+1}]$ 之间的时间间隔， $R_{t}^{w}$ 为t时刻IMU body坐标系到世界坐标系的旋转矩阵， $q_{t}^{bk}$ 为用四元素表示的 $t$ 时刻从IMU body坐标系到 $k$ 时刻IMU body坐标系的旋转，这里的四元素实部在后，虚部在前, 为了与论文保持一致，即四元素JPL表达。这里的 $Ω(w)\Omega(w)$ 表示四元素右乘。

关于公式 $(1)$ 的推导，这里首先引入四元素左乘右乘及导数定理：
这里与《Indirect Kalman Filter for 3D Attitude Estimation》中公式（10）不同，主要是因为左右手系不同，我们引入左乘和右乘符号如下：
$qa⊗qb=R(qb)qa=[sbzb−ybxb−zbsbxbybyb−xbsbzb−xb−yb−zbsb][xayazasa]=L(qa)qb=[sa−zayaxazasa−xaya−yaxasaza−xa−ya−zasa][xbybzbsb]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}$
为了简化，令 $\ y \ z \ s] = [w \ s]$ , 则有：
$\Omega(w)+sI_{4\times4} = \begin{bmatrix} -[w]_{\times} & w\\ -w^{T}& 0 \end{bmatrix} + sI_{4\times4}$ $\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}$ 的导数为： $q˙t=12[−[w]×w−wT0]qt=12Ω(w)qt=12R([w0])qt=12qt⊗[w0]\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}$

当看到 $q˙=12qt⊗[0w]\dot{q} = \frac{1}{2} q_{t} \otimes \begin{bmatrix} 0 \\ w \end{bmatrix}$ 说明该四元素实部在前，虚部在后（Hamilton表示），只是表示不同而已。

因此对于IMU连续形式下的旋转状态(用四元素表示)推导，我们有：
$qbk+1w=qbkw⊗qbk+1bk=qbkw⊗∫t∈[tk,tk+1]qt˙bkdt=qbkw⊗∫t∈[tk,tk+1]12qtbk⊗[wtbk0]dt=qbkw⊗∫t∈[tk,tk+1]12qtbk⊗[wt^−bwt−nw0]dt==qbk⊗∫t∈[tk,tk+1]12Ω(w^t−bwt−nw)qtbkdtq_{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$ 时刻的测量值 $a^bk\hat{a}_{b_{k}}$ , $w^bk\hat{w}_{b_{k}}$ 来计算的：
$pbk+1w=pbkw+vbkwΔtk+12a^bkδt2p^{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}$ $vbk+1w=vbkw+a^bkδtv_{b_{k+1}}^{w} = v_{b_{k}}^{w} + \hat{a}_{b_{k}} \delta t$ $qbk+1w=qbkw⊗[112w^bkδ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}$ 其中 $a^bk=qbkw(abk−bak)−gw\hat{a}_{b_{k}} = q_{b_{k}}^{w}(a_{b_{k}}-b_{ak})-g^{w}$ $w^bk=wbk−bwk\hat{w}_{b_{k}} = w_{b_{k}}-b_{wk}$

3.2.2 中值法离散形式

使用中值法，即 $k + 1$ 时刻的位姿是用俩个时刻 $k$ 和 $k + 1$ 测量值 $a$ , $w$ 的平均值来计算的：
$pbk+1w=pbkw+vbkwΔtk+12a^ˉtδt2p^{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}$ $vbk+1w=vbkw+a^ˉtδtv_{b_{k+1}}^{w} = v_{b_{k}}^{w} + \bar{\hat{a}}_{t} \delta t$ $qbk+1w=qbkw⊗[112w^ˉtδ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}$ 其中 $a^ˉt=12[qbkw(abk−bak)−gw+qbk+1w(abk+1−bak+1)−gw]\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}]$ $w^ˉt=12(wbk−bwk+wbk+1−bwk+1)=12(wbk+wbk+1)−bwk\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}$ 下，可通过在等式俩端同时乘以 $RwbkR^{b_{k}}_{w}$ 得到：【论文式[5][6]】
$Rwbkpbk+1w=Rwbk(pbkw+vbkwΔtk−12gwΔtk2)+αbk+1bkR^{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}}$ $Rwbkvbk+1w=Rwbk(vbkw−gwΔtk)+βbk+1bkR^{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}}$ $qwbk⊗qbk+1w=γbk+1bk(2)q_{w}^{b_{k}} \otimes q_{b_{k+1}}^{w} = \gamma _{b_{k+1}} ^{b_{k}} \tag{2}$
其中 $αbk+1bk=∫∫t∈[tk,tk+1]Rtbk(a^t−bat−na)dt2\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}$ $βbk+1bk=∫t∈[tk,tk+1]Rtbk(a^t−bat−na)dt\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$ $γbk+1=∫t∈[tk,tk+1]12Ω(w^t−bwt−nw)γtbkdt\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$

此时的积分结果 $αbk+1bk\alpha^{b_{k}}_{b_{k+1}}$ 、 $βbk+1bk\beta^{b_{k}}_{b_{k+1}}$ 、 $γbk+1bk\gamma^{b_{k}}_{b_{k+1}}$ 可以理解为 $b_{k+1}$ 对 $b_{k}$ 的相对运动量， $b_{k}$ 的状态并不会对其产生影响，因此将其作为非线性优化变量，可以避免状态的重复传递。

注意，这是在假设IMU偏置 $b_{a}$ 、 $b_{w}$ 已经确定的情况下，实际上偏置也是需要优化的变量，那么每次迭代时， $b_{a}$ 、 $b_{w}$ 发生改变，得重新根据公式求得所有帧之间的IMU预积分。

当偏置变换很小时，可以将 $αbk+1bk\alpha_{b_{k+1}}^{b_{k}}$ 、 $βbk+1bk\beta_{b_{k+1}}^{b_{k}}$ 、 $γbk+1bk\gamma_{b_{k+1}}^{b_{k}}$ 按其偏置的一阶近似来调整，否则就进行重新传递。【论文式[12]】(这部分只是抛出一个概念，后面会讲为什么这样写)
$αbk+1bk≈α^bk+1bk+Jbaαδba+Jbwαδbw\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}$ $βbk+1bk≈β^bk+1bk+Jbaβδba+Jbwβδbw\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}$ $γbk+1bk≈γ^bk+1bk[112Jbwγδbw]\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]】
$α^i+1bk=α^ibk+β^ibkδt+12R(γ^ibk)(a^i−bai)δt2\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}$ $β^i+1bk=β^ibk+12R(γ^ibk)(a^i−bai)δt\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$ $γ^i+1bk=γ^ibk⊗γ^i+1i=γ^ibk⊗[112(w^i−bwi)δ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 增量信息。
$α^i+1bk=α^ibk+β^ibkδt+12a^ˉiδt2\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}$ $β^i+1bk=β^ibk+a^ˉiδt\hat{\beta}^{b_{k}}_{i+1} = \hat{\beta}^{b_{k}}_{i} + \bar{\hat{a}}_{i}\delta t$ $γ^i+1bk=γ^ibk⊗γ^i+1i=γ^ibk⊗[112w^ˉiδ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}$ 其中 $a^ˉt=12[γ^ibk(a^i−bi)+γ^i+1bk(a^i+1−bai)]\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})]$ $w^ˉi=12(w^i+w^i+1)−bwi\bar{\hat{w}}_{i} = \frac{1}{2}(\hat w_{i} + \hat w_{i+1})-b_{wi}$
初始状态下 $αbkbk\alpha_{b_{k}}^{b_{k}}$ 、 $βbkbk\beta_{b_{k}}^{b_{k}}$ 为0, $γbkbk\gamma_{b_{k}}^{b_{k}}$ 为单位四元素， $n_{a}$ 、 $n_{w}$ 被视为0， $i$ 为在 $[k, k + 1]$ 中IMU测量值的某一时刻， $δt\delta t$ 为IMU测量值 $i$ 和 $i + 1$ 之间的时间间隔。

5. 非线性方程的误差递推方程

5.1 一段时间内多个IMU数据积分形成的预积分量的协方差计算

一个 IMU 数据作为测量值的噪声方差我们能够标定。现在，一段时间内多个 IMU 数据积分形成的预积分量的方差呢？

要推导预积分量的协方差，需要知道IMU噪声和预积分量之间的线性递推关系。

假设已知了相邻时刻误差的线性传递方程：
$ηik+1=Fkηik+Gknk\eta_{ik+1} = F_{k}\eta_{ik} + G_{k}n_{k}$ 其中 $ηik\eta_{ik}$ 为状态量误差且 $ηik=[δθik,δvik,δpik]\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 基于一阶泰勒展开的误差递推方程

令状态量为 $x^=x+δx\hat{x} = x +\delta x$ ，其中 $x^\hat{x}$ 表示计算得到的值，带有误差，真值为 $x$ ，误差为 $δx\delta x$ 。另外，输入量 $u$ 的噪声为 $n$ 。
应用一阶泰勒展开(EKF的协方差预测也是用了泰勒展开)，非线性系统 $x^k+1=f(x^k,u^k)\hat{x}_{k+1}=f(\hat{x}_{k},\hat{u}_{k})$ 的状态误差的线性递推关系为：
$δxk+1=Fδxk+Gnk\delta x_{k+1} = F\delta x_{k}+Gn_{k}$ 其中，F是状态量 $x_{k+1}$ 对状态量 $x_{k}$ 的雅克比矩阵，G是状态量 $x_{k+1}$ 对输入量 $u_{k}$ 的雅克比矩阵。

证明：对非线性状态方程进行一阶泰勒展开有：
$x^k+1=f(x^k,u^k)\hat{x}_{k+1} = f(\hat{x}_{k}, \hat{u}_{k})$ $xk+1+δxk+1=f(xk+δxk,uk+nk)x_{k+1}+\delta x_{k+1} = f(x_{k} + \delta x_{k}, u_{k} + n_{k})$ $xk+1‾+δxk+1=f(xk,uk)‾+Fδxk+Gnk(3)\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 基于误差随时间变化(误差导数)的递推方程

如果我们能够推导状态误差随时间变化的导数关系，如：
$δ˙x=Aδx+Bn\dot {\delta}_{x} = A\delta x + Bn$ 则误差状态的传递方程为：
$δxk+1=δxk+δ˙xkΔt\delta x_{k+1} = \delta x_{k} + \dot{\delta} x_{k} \Delta t$ $→δxk+1=(I+AΔt)δxk+BΔtnk(4)\rightarrow \delta x_{k+1} = (I+A\Delta t)\delta x_{k} + B\Delta tn_{k} \tag{4}$

由公式 $(3) (4)$ 得 $F=I+AΔtF=I+A\Delta t$ , $G=BΔtG=B\Delta t$

5.2.3 基于泰勒展开和误差随时间变化的方法对比

基于一阶泰勒展开的误差递推方程不香吗，为什么会用误差随时间的变化来进行误差递推方程的构建呢？

在VIO系统中，我们已经知道了状态的导数和状态之间的转移矩阵，如我们已经知道速度和状态量之间的关系：
$v˙w=Rbwab+gw\dot{v}^{w} = R^{w}_{b}a^{b}+g^{w}$ 那我们就可以推导速度的误差导数和状态误差之间的关系，在每一项上都加上各自的误差就有：
$v˙w+δ˙vw=Rbwexp([δθ]×)(ab+δab)+g+δg\dot{v}^{w}+\dot{\delta }v^{w} = R_{b}^{w}exp([\delta \theta]_{\times})(a^{b} + \delta a^{b})+g+\delta g$ $v˙w+δ˙vw=Rbw(I+[δθ]×)(ab+δab)+g+δg\dot{v}^{w}+\dot{\delta }v^{w} = R_{b}^{w}(I+[\delta \theta]_{\times})(a^{b} + \delta a^{b})+g+\delta g$ $v˙‾w+δ˙vw=Rbwab+g‾+Rbwδab+Rbw[δθ]×(ab+δab)+δ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$ $δ˙vw=Rbwδab+Rbw[δθ]×(ab+δab)+δg\dot{\delta}{v}^{w} = R_{b}^{w}\delta a^{b} + R_{b}^{w} [\delta \theta]_{\times} (a^{b} + \delta a^{b}) + \delta g$ $δ˙vw=Rbwδab−Rbw[ab]×δθ+δg(5)\dot{\delta} v^{w} = R_{b}^{w}\delta a^{b} – R_{b}^{w} [a^{b}]_{\times} \delta \theta + \delta g \tag{5}$ 由此就能以此类推，轻易写出整个 A 和 B 其他方程了。

考虑到公式的编辑篇幅，为了对一些求导公式进行简化，对求导公式进行了简化 (下同)：
$∂aw∂δθ=limδθ→0Rbwexp([δθ]×)ab−Rbwabδθ→∂aw∂δθ=∂Rbwexp([δθ]×)ab∂δθ\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部分)，由线性高斯系统的协方差，推到方程协方差矩阵，并纠结对应的雅克比矩阵。
首先由于四元素 $γtbk\gamma_{t}^{b_{k}}$ 被参数化过，我们将其误差定义为围绕其均值的扰动：【论文式(8)】
$γtbk≈γ^tbk⊗[112δθtbk]\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)】
$[δαbk+1bkδβbk+1bkδθbk+1bkδbaδbw]=[Rwbk(pbk+1w−pbkw−vkwΔt+12gwΔt2)−αbk+1bkRwbk(vbk+1w−vbkw+gwΔt)−βbk+1bk2[qwbk⊗qbk+1w⊗γbkbk+1]xyzbabk+1−babkbwbk+1−bwbk]\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)】:
$[δα˙tbkδβ˙tbkδθ˙tbkδb˙atδb˙wt]=[0I00000−Rtbk[a^t−bat]×−Rtbk000−[w^−bwt]×0−I0000000000][δαtbkδβtbkδθtbkδbatδbwt]+[0000−Rtbk0000−I0000I0000I][nanwnbanbw]=Ftδztbk+Gtnt(6)\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}$ 可以简写为：
$δz˙tbk=Ftδztbk+Gtnt\delta \dot{z}_{t}^{b_{k}} = F_{t}\delta z_{t}^{b_{k}} + G_{t}n_{t}$ 其中 $F_{t}$ 是15×15， $G_{t}$ 是12×12， $δztbk\delta z_{t}^{b_{k}}$ 是15×1， $n_{t}$ 是12×1。
根据导数的定义有：
$δz˙tbk=limδt→0δzt+δtbk−δztbkδt\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}$ 即： $δzt+δtbk=δztbk+δz˙tbkδt=δztbk+(Ftδztbk+Gtnt)δt=(I+Ftδt)δztbk+Gtδtnt\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)不谋而合，一般连续形式下的微小时间用 $δt\delta t$ 表示，离散形式下的时间区间用 $Δt\Delta t$ 表示，其实都是一个意思。进一步的，令 $F=(I+Ftδt),V=GtδtF=(I+F_{t} \delta t), V=G_{t}\delta t$ , 有：
$δzt+δtbk=Fδztbk+Vnt(7)\delta z_{t+\delta t}^{b_{k}} = F \delta z_{t}^{b_{k}} + Vn_{t} \tag{7}$ 由此我们已经知道了该非线性系统的线性化递推方程。

至于下一时刻的协方差，首先对于一个高斯分布的变量线性变换后得到新的变量的协方差计算，我们有以下推导：
已知一个变量 $y = A x$ ， $\sim(0, \Sigma_{x})$ ，则有 $Σy=AΣxAT\Sigma_{y} = A \Sigma_{x} A^{T}$ ，即：
$Σy=E((Ax)(Ax)T)=E(AxxTAT)=AΣxAT\Sigma_{y} = E((Ax)(Ax)^{T}) = E(Axx^{T}A^{T}) = A \Sigma_{x} A^{T}$

得到了递推方程 $(7)$ 之后，根据高斯分布线性变换协方差的传递规律，我们可以预测下一时刻的协方差：【论文式(10)】
$Pt+δtbk=(I+Ftδt)Ptbk(I+Ftδt)T+(Gδt)Q(Gtδt)TP_{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$ 代表噪声项的对角协方差矩阵：
$Q12×12=[σa20000σw20000σba20000σbw2]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]】
$Jt+δt=(I+Ftδt)JtJ_{t+\delta t} = (I+F_{t} \delta t) J_{t}$ 其中初始雅克比矩阵为： $J_{b_{k}} = I$ 。

关于公式 $(6)$ 的推导：
先把之前的预积分再写一遍：
$αbk+1bk=∫∫t∈[tk,tk+1]Rtbk(a^t−bat−na)dt2\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}$ $βbk+1bk=∫t∈[tk,tk+1]Rtbk(a^t−bat−na)dt\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$ $γbk+1=∫t∈[tk,tk+1]12Ω(w^t−bwt−nw)γtbkdt\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）首先对于 $δα˙tbk\delta \dot{\alpha}_{t}^{b_{k}}$ 、 $δb˙at\delta \dot {b}_{at}$ 、 $δb˙wt\delta \dot{b}_{wt}$ , 显然根据定义有：
$δα˙tbk=α˙^tbk−α˙tbk=β^tbk−βtbk=δβtbk\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}}$ $δb˙at=b˙at−0=nba\delta \dot{b}_{at} = \dot{b}_{at} – 0 = n_{b_{a}}$ $δb˙wt=b˙wt−0=nbw\delta \dot{b}_{wt} = \dot{b}_{wt} – 0 = n_{b_{w}}$ 其中上标" ^ " (如 $α˙^tbk\hat{\dot {\alpha}}_{t}^{b_{k}}$ ) 代表真实测量值，包含了噪声， $α˙tbk\dot {\alpha}_{t}^{b_{k}}$ 代表无噪声的理论值，偏置被建立成随机游走模型，其导数服从0均值的高斯分布。
2）然后是 $δβ˙tbk\delta \dot{\beta}_{t}^{b_{k}}$ :
$β˙tbk\dot{\beta}_{t}^{b_{k}}$ 为理论值，即不考虑存在噪声 $n_{a}$ 和 $b_{ba}$ 时应为： $β˙tbk=Rtbk(a^t−bat)\dot{\beta}_{t}^{b_{k}}=R_{t}^{b_{k}}(\hat a_{t} – b_{at})$
$β˙tbk\dot \beta _{t}^{b_{k}}$ 的实际测量值，即考虑噪声时应为：
$β˙^tbk=R^tbk(a^t−b^at−na)=Rtbkexp([δθ]×)(a^t−na−bat−δbat)=Rtbk(1+[δθ]×)(a^t−na−bat−δbat)=Rtbk(a^t−na−bat−δbat+[δθ]×(a^t−bat))=Rtbk(a^t−na−bat−δbat−[a^t−bat]×δθ)(8)\hat{\dot{\beta}}_{t}^{b_{k}} = \hat{R}_{t}^{b_{k}} (\hat a_{t}-\hat b_{at} – n_{a}) = R_{t}^{b_{k}}exp([\delta \theta]_{\times})(\hat a_{t} – n_{a} – b_{at} – \delta b_{at}) \\ =R_{t}^{b_{k}}(1+[\delta \theta]_{\times})(\hat a_{t} – n_{a} – b_{at} – \delta b_{at}) \\ = R_{t}^{b_{k}} (\hat a_{t} – n_{a} – b_{at} – \delta b_{at} + [\delta \theta]_{\times} (\hat a_{t} – b_{at})) \\ = R_{t}^{b_{k}} (\hat a_{t} – n_{a} – b_{at} – \delta b_{at} – [\hat a_{t} – b_{at}]_{\times} \delta \theta ) \tag{8}$ 则有 $δβ˙tbk=β˙^tbk−β˙tbk=−Rtbk[a^t−bat]×δθ−Rtbkδbat−Rtbkna\delta \dot{\beta}_{t}^{b_{k}} = \hat{\dot{\beta}}_{t}^{b_{k}} – \dot{\beta}_{t}^{b_{k}} = -R_{t}^{b_{k}} [\hat a_{t} – b_{at}]_{\times} \delta \theta – R_{t}^{b_{k}}\delta b_{at} – R_{t}^{b_{k}}n_{a}$ 其中公式 $(8)$ 的推导原因可借鉴公式 $(5)$ ，证毕！
3）接下来是 $δθ˙tbk\delta \dot{\theta} _{t}^{b_{k}}$ ，先推个 $δq˙tbk\delta \dot{q}_{t}^{b_{k}}$
$q˙tbk\dot q_{t}^{b_{k}}$ 的理论值，即考虑噪声 $n_{w}$ 和 $n_{b_{w}}$ : $q˙tbk=12qtbk⊗[(w^t−bwt)0]=12Ω(w^t−bwt)qtbk\dot q_{t}^{b_{k}} = \frac{1}{2} q_{t}^{b_{k}} \otimes \begin{bmatrix} (\hat{w}_{t} – b_{wt}) \\ 0 \end{bmatrix}= \frac{1}{2} \Omega(\hat{w}_{t} – b_{wt}) q_{t}^{b_{k}}$ $q˙tbk\dot q_{t}^{b_{k}}$ 的真实测量值： $q˙t^bk=12qt^bk⊗[(w^t−nw−bwt−δbwt)0]=12qtbk⊗δqtbk⊗[(w^t−nw−bwt−δbwt)0]\hat{\dot q_{t}}^{b_{k}} = \frac{1}{2} \hat{q_{t}}^{b_{k}} \otimes \begin{bmatrix} (\hat{w}_{t} – n_{w} – b_{wt} – \delta b_{wt}) \\ 0 \end{bmatrix} \\ = \frac{1}{2} q_{t}^{b_{k}} \otimes \delta q_{t}^{b_{k}}\otimes \begin{bmatrix} (\hat{w}_{t} – n_{w} – b_{wt} – \delta b_{wt}) \\ 0 \end{bmatrix}$ 根据导数性质，有：
$q˙t^bk=(qtbk⊗˙δqtbk)=q˙tbk⊗δqtbk+qtbk⊗δq˙tbk=12qtbk⊗[(w^t−bwt)0]⊗δqtbk+qtbk⊗δq˙tbk\hat{\dot q_{t}}^{b_{k}} = (q_{t}^{b_{k}} \dot \otimes \delta q_{t}^{b_{k}}) = \dot q_{t}^{b_{k}} \otimes \delta q_{t}^{b_{k}} + q_{t}^{b_{k}} \otimes \delta \dot q_{t}^{b_{k}} = \frac{1}{2} q_{t}^{b_{k}} \otimes \begin{bmatrix} (\hat {w}_{t} – b_{wt}) \\ 0 \end{bmatrix} \otimes \delta q_{t}^{b_{k}} + q_{t}^{b_{k}} \otimes \delta \dot q_{t}^{b_{k}}$ 即得到等式：
$12qtbk⊗δqtbk⊗[(w^t−nw−bwt−δbwt)0]=12qtbk⊗[(w^t−bwt)0]⊗δqtbk+qtbk⊗δq˙tbk\frac{1}{2} q_{t}^{b_{k}} \otimes \delta q_{t}^{b_{k}}\otimes \begin{bmatrix} (\hat{w}_{t} – n_{w} – b_{wt} – \delta b_{wt}) \\ 0 \end{bmatrix} = \frac{1}{2} q_{t}^{b_{k}} \otimes \begin{bmatrix} (\hat {w}_{t} – b_{wt}) \\ 0 \end{bmatrix} \otimes \delta q_{t}^{b_{k}} + q_{t}^{b_{k}} \otimes \delta \dot q_{t}^{b_{k}}$ $2δq˙tbk=qtbk⊗[(w^t−nw−bwt−δbwt)0]−[(w^t−bwt)0]⊗δqtbk2 \delta \dot q_{t}^{b_{k}} = q_{t}^{b_{k}}\otimes \begin{bmatrix} (\hat{w}_{t} – n_{w} – b_{wt} – \delta b_{wt}) \\ 0 \end{bmatrix} – \begin{bmatrix} (\hat {w}_{t} – b_{wt}) \\ 0 \end{bmatrix} \otimes \delta q_{t}^{b_{k}}$ $2δq˙tbk=R([(w^t−nw−bwt−δbwt)0])δqtbk−L([(w^t−bwt)0])δqtbk2 \delta \dot q_{t}^{b_{k}} = R(\begin{bmatrix} (\hat{w}_{t} – n_{w} – b_{wt} – \delta b_{wt}) \\ 0 \end{bmatrix} ) \delta q_{t}^{b_{k}}- L(\begin{bmatrix} (\hat {w}_{t} – b_{wt}) \\ 0 \end{bmatrix}) \delta q_{t}^{b_{k}}$ $2δq˙tbk=[δθ˙tbk0]=[−[2w^t−2bwt−nw−δbwt]×−nw−δbwt(nw+δbwt)T0][12δθtbk1]2 \delta \dot q_{t}^{b_{k}} = \begin{bmatrix} \delta \dot{\theta}_{t}^{b_{k}} \\ 0\end{bmatrix}= \begin{bmatrix} -[2\hat{w}_{t} – 2b_{wt} – n_{w} – \delta b_{wt}]_{\times} & -n_{w}-\delta b_{wt}\\ (n_{w}+\delta b_{wt})^{T} & 0\end{bmatrix} \begin{bmatrix} \frac{1}{2} \delta \theta _{t}^{b_{k}} \\ 1 \end{bmatrix}$ $δθ˙tbk=−[2w^t−2bwt−nw−δbwt]×12δθtbk−nw−δbwt≈−[w^t−bwt]×δθtbk−nw−δbwt\delta \dot{\theta}_{t}^{b_{k}} =-[2\hat{w}_{t} – 2b_{wt} – n_{w} – \delta b_{wt}]_{\times} \frac{1}{2} \delta \theta _{t}^{b_{k}} – n_{w} – \delta b_{wt} \\ \approx -[\hat{w}_{t} – b_{wt} ]_{\times} \delta \theta _{t}^{b_{k}} – n_{w} – \delta b_{wt}$ 证毕！

6.2 离散形式下(中值法)增量误差、协方差及雅克比矩阵的递推方程

根据连续形式的增量误差递推方程，同理我们可以推导出离散形式的方程，首先直接给出 PVQ 增量误差在离散形式下的矩阵形式，这在vins_estimator/src/factor/integration_base.h中的void integrationBase::midPointIntegration()函数中得以实现 (这里的矩阵V几个变量与VINS中的代码差个负号，体现在前四个噪声项，应该是推导过程中与VINS添加噪声的方式不同，即是减去一个噪声还是加上一个噪声，问题不是特别大)
$[δαk+1δθk+1δβk+1δbak+1δbwk+1]=[If01δtf03f040f1100−δt0f21If23f24000I00000I][δαkδθkδβkδbakδbwk]+[v00v01v02v03000−12δt0−12δt00−12Rkδtv21−12Rk+1δtv23000000δt000000δt][naknwknak+1nwk+1nbanbw](9)\begin{bmatrix} \delta \alpha_{k+1} \\ \delta \theta_{k+1} \\ \delta \beta_{k+1} \\ \delta b_{ak+1} \\ \delta b_{wk+1} \end{bmatrix} = \begin{bmatrix} I & f_{01} & \delta t & f_{03} & f_{04} \\ 0 & f_{11} & 0 & 0 & -\delta t \\ 0 & f_{21} & I & f_{23} & f_{24} \\ 0 & 0& 0 & I & 0 \\ 0 & 0& 0& 0 & I\end{bmatrix} \begin{bmatrix} \delta \alpha_{k} \\ \delta \theta_{k} \\ \delta \beta_{k} \\ \delta b_{ak} \\ \delta b_{wk} \end{bmatrix} + \begin{bmatrix} v_{00} & v_{01} &v_{02} & v_{03} & 0 & 0 \\ 0 & -\frac{1}{2} \delta t & 0 & -\frac{1}{2} \delta t& 0 & 0 \\ -\frac{1}{2}R_{k}\delta t & v_{21} & -\frac{1}{2}R_{k+1}\delta t & v_{23} & 0 & 0\\ 0 & 0& 0 & 0 & \delta t&0 \\ 0 & 0& 0& 0 & 0 & \delta t\end{bmatrix} \begin{bmatrix} n_{ak} \\ n_{wk} \\ n_{ak+1} \\ n_{wk+1} \\ n_{ba} \\ n_{bw}\end{bmatrix} \tag{9}$ 其中： $f01=12f21δt,f03=−14(Rk+Rk+1)δt2,f04=12f24δtf_{01} = \frac{1}{2} f_{21}\delta t, \ f_{03} = -\frac{1}{4}(R_{k}+R_{k+1})\delta t^{2}, \ f_{04}=\frac{1}{2} f_{24} \delta {t}$ $f11=I−[w^k+w^k+12−bwk]×δtf_{11}=I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} – b_{wk}]_{\times} \delta t$ $f21=−12Rk[a^k−bak]×δt−12Rk+1[a^k+1−bak]×(I−[w^k+w^k+12−bwk]×δt)δtf_{21} = -\frac{1}{2}R_{k} [\hat a_{k} – b_{ak}]_{\times} \delta t -\frac{1}{2} R_{k+1} [\hat a_{k+1} – b_{ak}]_{\times} (I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} – b_{wk}]_{\times} \delta t) \delta t$ $f23=−12(Rk+Rk+1)δt,f24=12Rk+1[a^k+1−bak]×δt2f_{23} = -\frac{1}{2}(R_{k} + R_{k+1})\delta t, \ f_{24} = \frac{1}{2} R_{k+1}[\hat {a}_{k+1} – b_{a{k}}]_{\times} \delta t^ {2}$ $v00=−14Rkδt2,v01=v03=12v21δt,v02=−14Rk+1δt2v_{00}=-\frac{1}{4}R_{k}\delta t^{2}, \ v_{01} = v_{03} = \frac{1}{2} v_{21}\delta t, \ v_{02} = -\frac{1}{4}R_{k+1}\delta t^{2}$ $v21=v23=14Rk+1[a^k+1−bak]×δt2v_{21}=v_{23} = \frac{1}{4}R_{k+1}[\hat{a}_{k+1}-b_{ak}]_{\times} \delta t^{2}$
————————————
其中上述符号中，为了与预积分(4.2.2)的推导对应，应有：
$Rk==γ^ibk.toRotationMatrix()R_{k}==\hat{\gamma}_{i}^{b_{k}}.toRotationMatrix()$ , $a^k==ai^\hat{a}_{k}==\hat{a_{i}}$ , $b_{ak}==b_{i}$ , $Rk+1=γ^i+1bk.toRotationMatrix()R_{k+1}=\hat{\gamma}_{i+1}^{b_{k}}.toRotationMatrix()$ , $a^k+1==a^i+1\hat{a}_{k+1}==\hat{a}_{i+1}$ , $b_{ak} = b_{ai}$ , $w^k==w^i\hat{w}_{k}==\hat{w}_{i}$ , $w^k+1==w^i+1\hat{w}_{k+1}==\hat{w}_{i+1}$ , $b_{wk}==b_{wi}$

$. t o R o t a t i o n M a t r i x ()$ 表示将单位四元素转成旋转矩阵
————————————

简写为：
$δzk+115×1=F15×15δzk15×1+V15×18n18×1(10)\delta z_{k+1}^{15 \times 1} = F^{15 \times 15} \delta z_{k}^{15 \times 1} + V^{15 \times 18} n^{18 \times 1} \tag{10}$ 协方差的迭代公式为（协方差的初始值 $P_{0} = 0$ ）：
$P_{k+1} = FP_{k}F^{T} + VQV^{T}$ 其中噪声项的对角协方差矩阵 $Q$ 为： $Q18×18=(σa2,σw2,σa2,σw2,σba2,σbw2)Q^{18 \times 18} = (\sigma_{a}^{2}, \sigma_{w}^{2},\sigma_{a}^{2},\sigma_{w}^{2},\sigma_{ba}^{2},\sigma_{bw}^{2})$ 。
雅克比矩阵的迭代公式为（雅克比矩阵的初始值为 $J_{0} =I$ ）： $J_{k+1} = FJ_{k}$ 这里计算出来的 $J_{k+1}$ 只是为了给后面提供对 bias 的雅克比矩阵。

关于公式(9)的推导：
1）对于 $δbak+1\delta b_{ak+1}$ 、 $δbwk+1\delta b_{wk+1}$ ，根据偏置模型定义显然有：
$δbak+1=δbak+nbaδt,δbwk+1=δbwk+nbwδt\delta b_{ak+1} = \delta b_{ak} + n_{b_{a}} \delta t, \ \delta b_{wk+1} = \delta b_{wk} + n_{b_{w}} \delta t$ 2) 对于 $δθk+1\delta \theta_{k+1}$ :
之前已经得到了角度误差导数的连续形式： $δθ˙tbk=−[w^t−bwt]×δθtbk−nw−δbwt\delta \dot{\theta}_{t}^{b_{k}} = -[\hat{w}_{t} – b_{wt}]_{\times} \delta \theta_{t}^{b_{k}} – n_{w} – \delta b_{wt}$ 那么其中值积分的离散形式为：
$δθ˙k=−[w^k+w^k+12−bwk]×δθkbk−nwk+nwk+12−δbwk\delta \dot{\theta}_{k} = -[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} – b_{wk}]_{\times} \delta \theta_{k}^{b_{k}} – \frac{n_{wk}+n_{wk+1}}{2} – \delta b_{wk}$ 根据导数定义可得下一时刻的误差为：
$δθk+1=δθk+δθ˙kδt=(I−[w^k+w^k+12−bwk]×δt)δθk−nwk+nwk+12δt−δbwkδt\delta \theta_{k+1} = \delta \theta_{k} + \delta \dot{\theta}_{k} \delta t = (I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} – b_{wk}]_{\times} \delta t) \delta \theta_{k} – \frac{n_{wk}+n_{wk+1}}{2} \delta t- \delta b_{wk} \delta t$ 整理得
$δθk+1=(I−[w^k+w^k+12−bwk]×δt)δθk−δtδbwk−δt2nwk−δt2nwk+1=f11δθk−δtδbwk−δt2nwk−δt2nwk+1\delta \theta_{k+1} = (I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} – b_{wk}]_{\times} \delta t) \delta \theta_{k} – \delta t \delta b_{wk} – \frac{\delta t}{2} n_{wk} – \frac{\delta t}{2} n_{wk+1} = f_{11} \delta \theta_{k} – \delta t \delta b_{wk} – \frac{\delta t}{2} n_{wk} – \frac{\delta t}{2} n_{wk+1}$ 故有：
$f11=I−[w^k+w^k+12−bwk]×δtf_{11}=I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} – b_{wk}]_{\times} \delta t$ 3）对于 $δβk+1:\delta \beta_{k+1}:$
之前已经得到了速度误差导数的连续形式： $δβ˙tbk=−Rtbk[a^t−bat]×δθ−Rtbkδbat−Rtbkna\delta \dot{\beta}_{t}^{b_{k}} = -R_{t}^{b_{k}} [\hat a_{t} – b_{at}]_{\times} \delta \theta – R_{t}^{b_{k}}\delta b_{at} – R_{t}^{b_{k}}n_{a}$ 那么其中值积分的离散形式为：
$δβ˙k=−12Rk[a^k−bak]×δθk−12Rk+1[a^k+1−bak+1]×δθk+1−12(Rk+Rk+1)δbak−12Rknak−12Rk+1nak+1\delta \dot{\beta}_{k} = -\frac{1}{2}R_{k} [\hat a_{k} – b_{ak}]_{\times} \delta \theta_{k} -\frac{1}{2} R_{k+1} [\hat a_{k+1} – b_{ak+1}]_{\times}\delta \theta_{k+1} – \frac{1}{2}(R_{k}+R_{k+1}) \delta b_{ak} -\frac{1}{2} R_{k}n_{ak} – \frac{1}{2} R_{k+1}n_{ak+1}$ 其中假设在 $k$ 时刻和 $k + 1$ 时刻下的加速度计偏置 $b_{a}$ 相等(下同)，把之前的 $δθk+1\delta \theta_{k+1}$ 代进去有:
$δβ˙k={−12Rk[a^k−bak]×−12Rk+1[a^k+1−bak]×(I−[w^k+w^k+12−bwk]×δt)}δθk+14Rk+1[a^k+1−bak]×δt(nwk+nwk+1)+12Rk+1[a^k+1−bak]×δtδbwk−12(Rk+Rk+1)δbak−12Rknak−12Rk+1nak+1\delta \dot{\beta}_{k} = \begin{Bmatrix} -\frac{1}{2}R_{k} [\hat a_{k} – b_{ak}]_{\times} -\frac{1}{2} R_{k+1} [\hat a_{k+1} – b_{ak}]_{\times} (I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} – b_{wk}]_{\times} \delta t)\end{Bmatrix} \delta \theta_{k} \\ + \frac{1}{4} R_{k+1} [\hat a_{k+1} – b_{ak}]_{\times}\delta t (n_{wk}+n_{wk+1}) + \frac{1}{2} R_{k+1} [\hat a_{k+1} – b_{ak}]_{\times} \delta t \delta b_{wk} \\ -\frac{1}{2}(R_{k} + R_{k+1})\delta b_{ak} – \frac{1}{2} R_{k}{n_{ak}} – \frac{1}{2}R_{k+1}{n_{ak+1}}$ 根据导数定义可得到下一时刻的误差为： $δβk+1=δβk+δβ˙kδt={−12Rk[a^k−bak]×−12Rk+1[a^k+1−bak]×(I−[w^k+w^k+12−bwk]×δt)}δtδθk+δβk+{14Rk+1[a^k+1−bak]×δt(nwk+nwk+1)+12Rk+1[a^k+1−bak]×δtδbwk−12(Rk+Rk+1)δbak−12Rknak−12Rk+1nak+1}δt\delta \beta_{k+1} = \delta \beta_{k} + \delta \dot {\beta}_{k} \delta t \\ = \begin{Bmatrix} -\frac{1}{2}R_{k} [\hat a_{k} – b_{ak}]_{\times} -\frac{1}{2} R_{k+1} [\hat a_{k+1} – b_{ak}]_{\times} (I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} – b_{wk}]_{\times} \delta t)\end{Bmatrix} \delta t \delta \theta_{k} + \delta \beta _{k} + \\ \begin{Bmatrix} \frac{1}{4} R_{k+1} [\hat a_{k+1} – b_{ak}]_{\times}\delta t (n_{wk}+n_{wk+1}) + \frac{1}{2} R_{k+1} [\hat a_{k+1} – b_{ak}]_{\times} \delta t \delta b_{wk} -\frac{1}{2}(R_{k} + R_{k+1})\delta b_{ak} – \frac{1}{2} R_{k}{n_{ak}} – \frac{1}{2}R_{k+1}{n_{ak+1}} \end{Bmatrix} \delta t$ 整理后得到：
$δβk+1=f21δθk+δβk+f23δbak+f24δbwk−12Rkδtnak−12Rk+1δtnak+1+v21nwk+v23nwk+1\delta \beta_{k+1} = f_{21} \delta \theta_{k} + \delta \beta _{k} + f_{23}\delta b_{ak} + f_{24}\delta b_{wk} – \frac{1}{2} R_{k}\delta tn_{ak} \\ – \frac{1}{2}R_{k+1}\delta tn_{ak+1} + v_{21}n_{wk}+v_{23}n_{wk+1}$ 其中
$f21=−12Rk[a^k−bak]×δt−12Rk+1[a^k+1−bak]×(I−[w^k+w^k+12−bwk]×δt)δtf_{21} = -\frac{1}{2}R_{k} [\hat a_{k} – b_{ak}]_{\times} \delta t -\frac{1}{2} R_{k+1} [\hat a_{k+1} – b_{ak}]_{\times} (I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} – b_{wk}]_{\times} \delta t) \delta t$ $f23=−12(Rk+Rk+1)δtf_{23} = -\frac{1}{2}(R_{k} + R_{k+1})\delta t$ $f24=12Rk+1[a^k+1−bak]×δt2f_{24} = \frac{1}{2} R_{k+1}[\hat {a}_{k+1} – b_{a_{k}}]_{\times} \delta t^ {2}$ $v21=v23=14Rk+1[a^k+1−bak]×δt2v_{21}=v_{23} = \frac{1}{4}R_{k+1}[\hat{a}_{k+1}-b_{ak}]_{\times} \delta t^{2}$ 证毕！
4）对于 $δαk+1:\delta \alpha_{k+1}:$
之前已经得到了速度误差导数的连续形式：
$δα˙tbk=α˙^tbk−α˙tbk=β^tbk−βtbk=δβtbk\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}}$ 则离散形式为： $δα˙tbk=12δβk+12δβk+1=δβk+12f21δθk−14(Rk+Rk+1)δtδbak+12f24δbwk−14Rkδtnak−14Rk+1δtnak+1+12v21nwk+12v23nwk+1\delta \dot{\alpha}_{t}^{b_{k}} = \frac{1}{2}\delta \beta_{k} + \frac{1}{2} \delta \beta_{k+1} \\ = \delta \beta_{k} + \frac{1}{2} f_{21}\delta \theta_{k} – \frac{1}{4}(R_{k}+R_{k+1})\delta t \delta b_{ak} + \frac{1}{2} f_{24} \delta b_{wk} \\ – \frac{1}{4} R_{k}\delta t n_{ak} – \frac{1}{4}R_{k+1}\delta t n_{ak+1} + \frac{1}{2} v_{21}n_{wk} + \frac{1}{2} v_{23}n_{wk+1}$ 根据导数定义可得下一时刻的误差为：
$δαk+1=δαk+δα˙δt=δαk+f01δθk+δtδβk+f03δbak+f04δbwk+v00nak+v01nwk+v02nak+1+v03nwk+1\delta \alpha_{k+1} = \delta{\alpha}_{k} + \delta \dot {\alpha} \delta t \\ = \delta \alpha _{k} + f_{01} \delta \theta_{k} + \delta t \delta \beta _{k} + f_{03}\delta b_{ak} + f_{04} \delta b_{wk} + v_{00}n_{ak} + v_{01}n_{wk}+v_{02}n_{ak+1}+v_{03}n_{wk+1}$ 其中
$f01=12f21δt,f03=−14(Rk+Rk+1)δt2,f04=12f24δtf_{01} = \frac{1}{2} f_{21} \delta t, \ f_{03} = -\frac{1}{4}(R_{k}+R_{k+1})\delta t^{2}, \ f_{04}=\frac{1}{2} f_{24} \delta {t}$ $v00=−14Rkδt2,v01=v03=12v21δt,v02=−14Rk+1δt2v_{00}=-\frac{1}{4}R_{k}\delta t^{2}, \ v_{01} = v_{03} = \frac{1}{2} v_{21}\delta t, \ v_{02} = -\frac{1}{4}R_{k+1}\delta t^{2}$