预积分记录

22 Nov 2020

预积分原理及公式，写大论文复制粘贴用

传统传统捷联惯性导航的递推算法，是在已知上一时刻的 IMU 状态量（姿态和速度、位移）情况下，利用 IMU 测量得到的线加速度和角速度，做积分运算得到当前时刻的状态量，然而在基于非线性优化的视觉惯性里程计中，各个节点的状态量都是估计值。当这些状态量在优化算法中，会不断调整，为了传递调整后的 IMU 测量值，每次都需要在它们之间重新积分。因此提出了 IMU 预积分，希望对 IMU 的相对测量进行处理，使它与绝对位姿解耦，或者只要线性运算就可以进行矫正，从而避免绝对位姿被优化时进行重复积分。

• 符号定义规则
• IMU 模型
  • 当前时刻 PVQ 的连续形式
  • 当前时刻 PVQ 的中值法离散形式
  • 当前时刻 PVQ 的欧拉法离散形式
• IMU 预积分
  • 两帧之间 PVQ 增量的连续形式
  • 两帧之间的 PVQ 的中值法离散形式
  • 两帧之间的 PVQ 的欧拉法离散形式
• PVQ 增量的误差、协方差和 Jacobian
  • 连续形式下 PVQ 增量的误差、协方差和 Jacobian
  • 离散形式的 PVQ 增量误差分析、协方差和 Jacobian

符号定义规则

和 vins 论文中的符号保持一致：

\[(\cdot)^{\omega} \quad (\cdot)^{b} \quad (\cdot)^{c}\]

分别表示世界坐标系(world frame)，IMU 坐标系，也作为体坐标系(body frame)，以及相机坐标系

旋转矩阵和四元数（实部在后，虚部在后）都被用来表示旋转

\[\mathbf{q}_{b}^{w}, \mathbf{p}_{b}^{w}\]

表示从体坐标系到世界坐标系的旋转

\[b_{k}\]

表示第 k 帧图像对应的体坐标系

\[g^{\omega} = \left[ 0,0,g \right]^{\mathrm{T}}\]

表示世界坐标系中的重力向量

\[\hat{(\cdot)}\]

表示带有噪声的测量值或计算得到的估计值

IMU 模型

IMU 测量值包括加速度计得到的线加速度和陀螺仪得到的角速度，被建模为：

\[\begin{array}{c} \hat{a}_{t}=a_{t}+b_{a_{t}}+R_{w}^{t} g^{w}+n_{a} \\ \hat{w}_{t}=w_{t}+b_{w_{t}}+n_{w} \end{array}\]

a 表示加速度 acceleration, w 表示角速度

这两个测量值均在以 IMU 坐标系作为体坐标系(body frame)中测得。其中 t 下标表示时间，并受到加速度偏置 b_a, 陀螺仪偏置 b_w 以及附加噪声 n_a 和 n_w 的影响，R^t_w 表示 t 时刻从世界坐标系(world frame)到体坐标系的旋转矩阵。

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

\[n_{a} \sim N\left(0, \sigma_{a}^{2}\right), \quad n_{w} \sim N\left(0, \sigma_{w}^{2}\right)\]

加速度计偏置和陀螺仪偏置被建模为随机游走，其导数为高斯噪声：

\[\begin{array}{c} \dot{b}_{a_{t}}=n_{b_{a}}, \quad \dot{b}_{w_{t}}=n_{b_{w}} \\ n_{b_{a}} \sim N\left(0, \sigma_{b_{a}}^{2}\right), \quad n_{b_{w}} \sim N\left(0, \sigma_{b_{w}}^{2}\right) \end{array}\]

当前时刻 PVQ 的连续形式

对于图像帧 k 和 k+1，体坐标系对应为 b_k 和 b_k+1, 位置，速度和方向三个状态值(PVQ)可以根据 [t_k, t_k+1] 时间间隔内的 IMU 测量值，在世界坐标系下进行传递，即对图像帧 k 和 k+1 所有的 IMU 进行积分：

\[p_{b_{k+1}}^{w}=p_{b_{k}}^{w}+v_{b_{k}}^{w} \Delta t_{k}+\iint_{t \in\left[t_{k}, t_{k+1}\right]}\left(R_{t}^{w}\left(\hat{a}_{t}-b_{a_{t}}-n_{a}\right)-g^{w}\right) d t^{2} \\ v_{b_{k+1}}^{w}=v_{b_{k}}^{w}+\int_{\left.t \in \mid t_{k}, t_{k+1}\right]}\left(R_{t}^{w}\left(\hat{a}_{t}-b_{a_{t}}-n_{a}\right)-g^{w}\right) dt \\ \begin{aligned} q_{b_{k+1}}^{w} &= q_{b_{k}}^{w} \otimes \int_{t \in\left[t_{k}, t_{k+1}\right]} \frac{1}{2} q_{t}^{b_{k}} \otimes\left[\begin{array}{c} \left(\hat{w}_{t}-b_{w_{t}}-n_{w}\right) \\ 0 \end{array}\right] d t \\ &= q_{b_{k}}^{w} \otimes \int_{t \in\left|t_{k}, t_{k+1}\right|} \frac{1}{2} \Omega\left(\hat{w}_{t}-b_{w_{t}}-n_{w}\right) q_{t}^{b_{k}} d t \end{aligned}\]

其中，△t 是 [tk, tk+1] 之间的时间间隔，Rwt 表示 t 时刻从体坐标系到世界坐标系的转换，qbkt 为四元数表示的 t 时刻体坐标系到 bk 位置的旋转。

关于式：

\[\begin{aligned} q_{b_{k+1}}^{w} &= q_{b_{k}}^{w} \otimes \int_{t \in\left[t_{k}, t_{k+1}\right]} \frac{1}{2} q_{t}^{b_{k}} \otimes\left[\begin{array}{c} \left(\hat{w}_{t}-b_{w_{t}}-n_{w}\right) \\ 0 \end{array}\right] d t \\ &= q_{b_{k}}^{w} \otimes \int_{t \in\left|t_{k}, t_{k+1}\right|} \frac{1}{2} \Omega\left(\hat{w}_{t}-b_{w_{t}}-n_{w}\right) q_{t}^{b_{k}} d t \end{aligned}\]

可以参考补充的四元数求导内容，并且这里的四元数虚部在前，实部在后，这里的 \Omega 的含义是用矩阵表示四元数的右乘，即：

\[\boldsymbol{\Omega}(\boldsymbol{\omega})=\left[\begin{array}{cc} -\lfloor\boldsymbol{\omega}\rfloor_{\times} & \boldsymbol{\omega} \\ -\boldsymbol{\omega}^{T} & 0 \end{array}\right],\lfloor\boldsymbol{\omega}\rfloor_{\times}=\left[\begin{array}{ccc} 0 & -\omega_{z} & \omega_{y} \\ \omega_{z} & 0 & -\omega_{x} \\ -\omega_{y} & \omega_{x} & 0 \end{array}\right]\]

当前时刻 PVQ 的中值法离散形式

使用 mid-point 表示，为了和相机帧（k 和 k+1）作区分，下面的式子表示从第 i 个 IMU 时刻到其相邻时刻第 i + 1 个 IMU 时刻的积分过程，相邻时刻的位姿是用两个加速度和角速度测量值的平均值来计算，这与 Estimator::processIMU() 函数中的 Ps[j] Rs[j] 和 Vs[j] 是一致的（代码中的 j 即为此处的 i+1），IMU 积分出来的第 j 时刻的 PVQ 可以作为第 j 帧图像的初始值。

\[\begin{array}{l} p_{b_{i+1}}^{w}=p_{b_{i}}^{w}+v_{b_{i}}^{w} \delta t+\frac{1}{2} \overline{\hat{a}}_{i} \delta t^{2} \\ v_{b_{i+1}}^{w}=v_{b_{i}}^{w}+\overline{\hat{a}}_{i} \delta t \\ q_{b_{i+1}}^{w}=q_{b_{i}}^{w} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \bar{\hat{\omega}}_{i} \delta t \end{array}\right] \end{array}\]

其中

\[\begin{array}{c} \overline{\hat{a}}_{i}=\frac{1}{2}\left[q_{i}\left(\hat{a}_{i}-b_{a_{i}} -n_{a}\right)-g^{w}+q_{i+1}\left(\hat{a}_{i+1}-b_{a_{i+1}} - n_{a}\right)-g^{w}\right] \\ \overline{\widehat{\omega}}_{i}=\frac{1}{2}\left(\widehat{\omega}_{i}+\widehat{\omega}_{i+1}\right)-b_{\omega_{i}} - n_{\omega} \end{array}\]

对式

\[q_{b_{i+1}}^{w}=q_{b_{i}}^{w} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \bar{\hat{\omega}}_{i} \delta t \end{array}\right]\]

的推导说明

\[\begin{aligned} q_{b_{i+1}}^{w} &= q_{b_{i}}^{w} + \dot{q_{b_{}}^{w}} \delta t \\ &= q_{b_{i}}^{w} + q_{b_{i}}^{w} \otimes \left[\begin{array}{c} 0 \\ \frac{1}{2} \bar{\hat{\omega}}_{i} \end{array}\right] \delta t \\ &= q_{b_{i}}^{w} \otimes \left[\begin{array}{c} 1 \\ 0 \end{array}\right] + q_{b_{i}}^{w} \otimes \left[\begin{array}{c} 0 \\ \frac{1}{2} \bar{\hat{\omega}}_{i} \delta t \end{array}\right] \\ &= q_{b_{i}}^{w} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \bar{\hat{\omega}}_{i} \delta t \end{array}\right] \end{aligned}\]

当前时刻 PVQ 的欧拉法离散形式

为了和相机帧（k 和 k+1)作区分，下面的式子表示从第 i 个 IMU 时刻到其相邻时刻第 i + 1 个 IMU 时刻的积分过程，相邻时刻的位姿是用第 i 时刻的加速度和角速度测量值来计算的。

\[\begin{array}{l} p_{b_{i+1}}^{w}=p_{b_{i}}^{w}+v_{b_{i}}^{w} \delta t+\frac{1}{2} \overline{\hat{a}}_{i} \delta t^{2} \\ v_{b_{i+1}}^{w}=v_{b_{i}}^{w}+\overline{\hat{a}}_{i} \delta t \\ q_{b_{i+1}}^{w}=q_{b_{i}}^{w} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \bar{\hat{\omega}}_{i} \delta t \end{array}\right] \end{array}\]

其中

\[\begin{array}{c} \overline{\hat{a}}_{i}=q_{i}\left(\hat{a}_{i}-b_{a_{i}} - n_a\right)-g^{w} \\ \overline{\widehat{\omega}}_{i}=\widehat{\omega}_{i}-b_{\omega_{i}} - n_{\omega} \end{array}\]

IMU 预积分

两帧之间 PVQ 增量的连续形式

为了避免非线性优化过程中的重复积分，当前时刻的参考坐标系从世界坐标系 w 调整为第 k 帧的体坐标系 b_k，即对 PVQ 乘上一个旋转

\[\begin{array}{c} R_{w}^{b_{k}} p_{b_{k+1}}^{w}=R_{w}^{b_{k}}\left(p_{b_{k}}^{w}+v_{b_{k}}^{w} \Delta t_{k}-\frac{1}{2} g^{w} \Delta t_{k}^{2}\right)+\alpha_{b_{k+1}}^{b_{k}} \\ R_{w}^{b_{k}} v_{b_{k+1}}^{w}=R_{w}^{b_{k}}\left(v_{b_{k}}^{w}-g^{w} \Delta t_{k}\right)+\beta_{b_{k+1}}^{b_{k}} \\ q_{w}^{b_{k}} \otimes q_{b_{k+1}}^{w}=\gamma_{b_{k+1}}^{b_{k}} \end{array}\]

其中

\[\begin{aligned} \alpha_{b_{k+1}}^{b_{k}} &=\iint_{\left.t \in \mid t_{k}, t_{k+1}\right]} R_{t}^{b_{k}}\left(\hat{a}_{t}-b_{a_{t}}-n_{a}\right) d t^{2} \\ \beta_{b_{k+1}}^{b_{k}} &=\int_{t \in\left[t_{k}, t_{k+1}\right]} R_{t}^{b_{k}}\left(\hat{a}_{t}-b_{a_{t}}-n_{a}\right) d t \\ \gamma_{b_{k+1}}^{b_{k}} &=\int_{t \in\left|t_{k}, t_{k+1}\right|} \frac{1}{2} \Omega\left(\hat{w}_{t}-b_{w_{t}}-n_{w}\right) \gamma_{t}^{b_{k}} dt \end{aligned}\]

上式中的积分结果可以理解为 b_k+1 相对于 b_k 的相对运动， b_k 的状态改变并不会对其产生影响，因此将其作为优化变量，可以避免状态的重复传递。

并且，这是假设 IMU 的偏置 b_a 和 b_w 已经确定的情况下，实际上偏置也是需要优化的变量，那么每次迭代，b_a 和 b_w 发生改变，得重新根据公式求得所有帧之间的 IMU 预积分，非常耗时。所以，对其偏置(高斯噪声被设置为 0)做一阶的泰勒展开，可以得到一个近似的线性关系(来替代原来的积分这样的耗时操作)。

\[\begin{aligned} \boldsymbol{\alpha}_{b_{k+1}}^{b_{k}} & \approx \hat{\boldsymbol{\alpha}}_{b_{k+1}}^{b_{k}}+\mathbf{J}_{b_{a}}^{\alpha} \delta \mathbf{b}_{a_{k}}+\mathbf{J}_{b_{w}}^{\alpha} \delta \mathbf{b}_{w_{k}} \\ \boldsymbol{\beta}_{b_{k+1}}^{b_{k}} & \approx \hat{\boldsymbol{\beta}}_{b_{k+1}}^{b_{k}}+\mathbf{J}_{b_{a}}^{\beta} \delta \mathbf{b}_{a_{k}}+\mathbf{J}_{b_{w}}^{\beta} \delta \mathbf{b}_{w_{k}} \\ \boldsymbol{\gamma}_{b_{k+1}}^{b_{k}} & \approx \hat{\boldsymbol{\gamma}}_{b_{k+1}}^{b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \mathbf{J}_{b_{w}}^{\gamma} \delta \mathbf{b}_{w_{k}} \end{array}\right] \end{aligned}\]

用离散形式来表示，那么预积分各项在 b_k 和 b_k+1 图像帧之间的 IMU 测量时刻之间传播。

两帧之间的 PVQ 的中值法离散形式

需要注意，预积分各项的初始值

\[\alpha_{b_{k},}^{b_{k}} \quad \beta_{b_{k}}^{b_{k}} \quad \gamma_{b_{k}}^{b_{k}}\]

分别为 0 向量，0 向量以及 identity quaternion，然后预积分各项在 b_k 和 b_k+1 图像帧之间的传播 i, i+1 …。

中值法为 vins 代码中采用的方法，和 Estimator::processIMU() 函数中的 ` IntegrationBase::push_back()` 上是一致的，并且由于加性噪声 n_a 和 n_w 是未知的，在实际的代码中被设置为 0。第 i 个 IMU 时刻与第 i+1 个 IMU 时刻的变量关系为

\[\begin{aligned} \hat{\alpha}_{i+1}^{b_{k}} &=\hat{\alpha}_{i}^{b_{k}}+\hat{\beta}_{i}^{b_{k}} \delta t+\frac{1}{2} \bar{a}_{i} \delta t^{2} \\ \hat{\beta}_{i+1}^{b_{k}} &=\hat{\beta}_{i}^{b_{k}}+\bar{a}_{i} \delta t \\ \hat{\gamma}_{i+1}^{b_{k}} &=\hat{\gamma}_{i}^{b_{k}} \otimes \hat{\gamma}_{i+1}^{i}=\hat{\gamma}_{i}^{b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \overline{\hat{\omega}}_{i} \delta t \end{array}\right] \end{aligned}\]

其中

\[\overline{\hat{a}}_{i}=\frac{1}{2}\left[q_{i}\left(\hat{a}_{i}-b_{a_{i}}\right)+q_{i+1}\left(\hat{a}_{i+1}-b_{a_{i}}\right)\right] \\ \overline{\widehat{\omega}}_{i}=\frac{1}{2}\left(\widehat{\omega}_{i}+\widehat{\omega}_{i+1}\right)-b_{\omega_{i}}\]

两帧之间的 PVQ 的欧拉法离散形式

vins 论文中采用的方法，含义是第 i 个 IMU 时刻与第 i+1 个 IMU 时刻的变量关系为：

\[\begin{array}{l} \hat{\alpha}_{i+1}^{b_{k}}=\hat{\alpha}_{i}^{b_{k}}+\hat{\beta}_{i}^{b_{k}} \delta t+\frac{1}{2} R\left(\hat{\gamma}_{i}^{b_{k}}\right)\left(\hat{a}_{i}-b_{a_{i}}\right) \delta t^{2} \\ \hat{\beta}_{i+1}^{b_{k}}=\hat{\beta}_{i}^{b_{k}}+R\left(\hat{\gamma}_{i}^{b_{k}}\right)\left(\hat{a}_{i}-b_{a_{i}}\right) \delta t \\ \hat{\gamma}_{i+1}^{b_{k}}=\hat{\gamma}_{i}^{b_{k}} \otimes \left[\begin{array}{c} 1 \\ \frac{1}{2}\left(\widehat{\omega}_{i}-b_{\omega_{i}}\right) \delta t \end{array} \right] \end{array}\]

其中 \delta t 表示第 i 和第 i+1 个 IMU 测量时刻的差值。

PVQ 增量的误差、协方差和 Jacobian

上述的过程为 IMU 预积分测量值的推导，而要将 IMU 预积分运用到非线性优化中，需要建立线性高斯误差状态递推方程，由线性高斯系统的协方差，推导方程协方差矩阵，并求解对应的雅可比矩阵

连续形式下 PVQ 增量的误差、协方差和 Jacobian

将四元数的误差项定义为围绕其均值的扰动（此四元数实数在前）

\[\gamma_{t}^{b_{k}} \approx \hat{\gamma}_{t}^{b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \theta_{t}^{b_{k}} \end{array}\right] \\ = \hat{\gamma}_{t}^{b_{k}} \otimes \delta \gamma_{t}^{b_{k}}\]

IMU 在每一个时刻积分出来的值是有误差的,对误差进行分析。首先直接给出在 t 时刻误差项的导数为:

\[\left[\begin{array}{c} \delta \dot{\boldsymbol{\alpha}}_{t}^{b_{k}} \\ \delta \dot{\boldsymbol{\beta}}_{t}^{b_{k}} \\ \delta \dot{\boldsymbol{\theta}}_{t}^{b_{k}} \\ \delta \dot{\mathbf{b}}_{a_{t}} \\ \delta \dot{\mathbf{b}}_{w_{t}} \end{array}\right]=\left[\begin{array}{ccccc} 0 & \mathbf{I} & 0 & 0 & 0 \\ 0 & 0 & -\mathbf{R}_{t}^{b_{k}}\left\lfloor\hat{\mathbf{a}}_{t}-\mathbf{b}_{a_{t}}\right\rfloor_{\times} & -\mathbf{R}_{t}^{b_{k}} & 0 \\ 0 & 0 & -\left\lfloor\hat{\boldsymbol{\omega}}_{t}-\mathbf{b}_{w_{t}}\right\rfloor_{\times} & 0 & -\mathbf{I} \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \end{array}\right]\left[\begin{array}{l} \delta \boldsymbol{\alpha}_{t}^{b_{k}} \\ \delta \boldsymbol{\beta}_{t}^{b_{k}} \\ \delta \boldsymbol{\theta}_{t}^{b_{k}} \\ \delta \mathbf{b}_{a_{t}} \\ \delta \mathbf{b}_{w_{t}} \end{array}\right] \\ +\left[\begin{array}{cccc} 0 & 0 & 0 & 0 \\ -\mathbf{R}_{t}^{b_{k}} & 0 & 0 & 0 \\ 0 & -\mathbf{I} & 0 & 0 \\ 0 & 0 & \mathbf{I} & 0 \\ 0 & 0 & 0 & \mathbf{I} \end{array}\right]\left[\begin{array}{c} \mathbf{n}_{a} \\ \mathbf{n}_{w} \\ \mathbf{n}_{b_{a}} \\ \mathbf{n}_{b_{w}} \end{array}\right]=\mathbf{F}_{t} \delta \mathbf{z}_{t}^{b_{k}}+\mathbf{G}_{t} \mathbf{n}_{t}\]

可以简写为

\[\delta \dot{\mathbf{z}}_{t}^{b_{k}} = \mathbf{F}_{t} \delta \mathbf{z}_{t}^{b_{k}}+\mathbf{G}_{t} \mathbf{n}_{t}\]

其中各项的维数为

\[\begin{aligned} \mathbf{F}_{t} \quad &15 \times 15 \\ \mathbf{G}_{t} \quad &15 \times 12 \\ \delta \mathbf{z}_{t}^{b_{k}} \quad &15 \times 1 \\ \mathbf{n}_{t} \quad &12 \times 1 \end{aligned}\]

下面是关于上式的推导过程

首先可以确定的三项为

\[\begin{array}{c} \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}_{a_{t}}=\dot{b}_{a_{t}}-0=n_{b_{a}} \\ \delta \dot{b}_{w_{t}}=\dot{b}_{w_{t}}-0=n_{b_{w}} \end{array}\]

考虑噪声时

\[\begin{array}{c} \hat{\dot{\beta}}_{t}^{b_k}=\hat{R}_{t}^{b_{k}}\left(\hat{a}_{t}-\hat{b}_{a_{t}}-n_{a}\right)=R_{t}^{b_{k}} \exp \left([\delta \theta]_{\times}\right)\left(\hat{a}_{t}-n_{a}-b_{a_{t}}-\delta b_{a_{t}}\right) \\ =R_{t}^{b_{k}}\left(1+[\delta \theta]_{\times}\right)\left(\hat{a}_{t}-n_{a}-b_{a_{t}}-\delta b_{a_{t}}\right) \\ =R_{t}^{b_{k}}\left(\hat{a}_{t}-n_{a}-b_{a_{t}}-\delta b_{a_{t}}+[\delta \theta]_{\times}\left(\hat{a}_{t}-b_{a_{t}}\right)\right) \\ =R_{t}^{b_{k}}\left(\hat{a}_{t}-n_{a}-b_{a_{t}}-\delta b_{a_{t}}-\left[\hat{a}_{t}-b_{a_{t}}\right]_{\times} \delta \theta\right) \end{array}\]

不考虑噪声 n_a 的情况下

\[\dot{\beta}_{t}^{b_{k}}=R_{t}^{b_{k}}\left(\hat{a}_{t}-b_{a_{t}}\right)\]

所以

\[\delta \dot{\beta}_{t}^{b_{k}}=\hat{\dot{\beta}}_{t}^{b_{k}}-\dot{\beta}_{t}^{b_{k}}=-R_{t}^{b_{k}}\left[\hat{a}_{t}-b_{a_{t}}\right]_{\times} \delta \theta-R_{t}^{b_{k}} \delta b_{a_{t}}-R_{t}^{b_{k}} n_{a}\]

为了求角度误差的导数，首先给出四元数误差的导数的计算过程，其理论值和真实测量值的导数分别为

\[\dot{q}_{t}^{b_{k}}=\frac{1}{2} \Omega\left(\hat{w}_{t}-b_{w_{t}}\right) q_{t}^{b_{k}}=\frac{1}{2} q_{t}^{b_{k}} \otimes\left[\begin{array}{c} \left(\hat{w}_{t}-b_{w_{t}}\right) \\ 0 \end{array}\right] \\ \hat{\dot{q}}_{t}^{b_{k}}=\frac{1}{2} \hat{q}_{t}^{b_{k}} \otimes\left[\begin{array}{c} \hat{w}_{t}-b_{w_{t}}-n_{w}-\delta b_{w_{t}} \\ 0 \end{array}\right] \\ =\frac{1}{2} q_{t}^{b_{k}} \otimes \delta q_{t}^{b_{k}} \otimes\left[\begin{array}{c} \hat{w}_{t}-b_{w_{t}}-n_{w}-\delta b_{w_{t}} \\ 0 \end{array}\right]\]

并且根据两函数相乘的导数的性质，有

\[\hat{\dot{q}}_{t}^{b_{k}}=\left(q_{t}^{b_{k}} \dot{\otimes} \delta \dot{}q_{t}^{b_{k}}\right)=\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\left[\begin{array}{c} \left(\hat{w}_{t}-b_{w_{t}}\right) \\ 0 \end{array}\right] \otimes \delta q_{t}^{b_{k}}+q_{t}^{b_{k}} \otimes \dot{\delta q_{t}^{b_{k}}}\]

那么可以根据两个四元数真实测量值的导数得到等式

\[\frac{1}{2} q_{t}^{b_{k}} \otimes \delta q_{t}^{b_{k}} \otimes\left[\begin{array}{c} \hat{w}_{t}-b_{w_{t}}-n_{w}-\delta b_{w_{t}} \\ 0 \end{array}\right]=\frac{1}{2} q_{t}^{b_{k}} \otimes\left[\begin{array}{c} \left(\hat{w}_{t}-b_{w_{t}}\right) \\ 0 \end{array}\right] \otimes \delta q_{t}^{b_{k}}+q_{t}^{b_{k}} \otimes \dot{\delta q_{t}^{b_{k}}}\] \[2 \delta \dot{q_{t}^{b_{k}}}=\delta q_{t}^{b_{k}} \otimes\left[\begin{array}{c} \hat{w}_{t}-b_{w_{t}}-n_{w}-\delta b_{w_{t}} \\ 0 \end{array}\right]-\left[\begin{array}{c} \left(\hat{w}_{t}-b_{w_{t}}\right) \\ 0 \end{array}\right] \otimes \delta q_{t}^{b_{k}}\] \[2 \delta \dot{q_{t}^{b_{k}}}=\mathcal{R}\left(\left[\begin{array}{c} \hat{w}_{t}-b_{w_{t}}-n_{w}-\delta b_{w_{t}} \\ 0 \end{array}\right]\right) \delta q_{t}^{b_{k}}-\mathcal{L}\left(\left[\begin{array}{c} \left(\hat{w}_{t}-b_{w_{t}}\right) \\ 0 \end{array}\right]\right) \delta q_{t}^{b_{k}}\]

其中 R[] 和 L[] 分别代表左乘和右乘矩阵，并且根据四元数误差项的定义(围绕其均值的扰动，见本小节开头)

\[2 \delta \dot{q_{t}^{b_{k}}}=\left[\begin{array}{c} \delta \dot{\theta}_{t}^{b_{k}} \\ 0 \end{array}\right]=\left[\begin{array}{cc} -\left[2 \hat{w}_{t}-2 b_{w_{t}}-n_{w}-\delta b_{w_{t}}\right]_{\times} & -n_{w}-\delta b_{w_{t}} \\ \left(n_{w}+\delta b_{w_{t}}\right)^{T} & 0 \end{array}\right]\left[\begin{array}{c} \frac{1}{2} \delta \theta_{t}^{b_{k}} \\ 1 \end{array}\right]\] \[\begin{array}{c} \delta \dot{\theta_{t}^{b_{k}}}=-\left[2 \hat{w}_{t}-2 b_{w_{t}}-n_{w}-\delta b_{w_{t}}\right]_{\times} \frac{1}{2} \delta \theta_{t}^{b_{k}}-n_{w}-\delta b_{w_{t}} \\ \approx-\left[\hat{w}_{t}-b_{w_{t}}\right]_{\times} \delta \theta_{t}^{b_{k}}-n_{w}-\delta b_{w_{t}} \end{array}\]

到此为止，式

\[\delta \dot{\mathbf{z}}_{t}^{b_{k}} = \mathbf{F}_{t} \delta \mathbf{z}_{t}^{b_{k}}+\mathbf{G}_{t} \mathbf{n}_{t}\]

已推导完毕，并且根据导数定义有

\[\begin{aligned} \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}}+\left(F_{t} \delta z_{t}^{b_{k}}+G_{t} n_{t}\right) \delta t \\ &= \left(I+F_{t} \delta t\right) \delta z_{t}^{b_{k}}+G_{t} \delta t n_{t} \end{aligned}\]

这个式子的意义是下一个时刻的 IMU 测量值误差和上一时刻的误差为线性关系，这样可以根据当前时刻的值预测下一时刻的均值和协方差，根据误差传播公式，协方差为:

\[\begin{array}{c} \mathbf{P}_{t+\delta t}^{b_{k}}=\left(\mathbf{I}+\mathbf{F}_{t} \delta t\right) \mathbf{P}_{t}^{b_{k}}\left(\mathbf{I}+\mathbf{F}_{t} \delta t\right)^{T}+\left(\mathbf{G}_{t} \delta t\right) \mathbf{Q}\left(\mathbf{G}_{t} \delta t\right)^{T} \quad t \in[k, k+1] \\ \mathbf{P}_{b_{k}}^{b_{k}}=\mathbf{0}， Q^{12 \times 12}=\left[\begin{array}{cccc} \sigma_{a}^{2} & 0 & 0 & 0 \\ 0 & \sigma_{w}^{2} & 0 & 0 \\ 0 & 0 & \sigma_{b_{a}}^{2} & 0 \\ 0 & 0 & 0 & \sigma_{b_{w}}^{2} \end{array}\right] \end{array}\]

P 和 Q 分别表示 k 帧时的初始协方差以及噪声项的协方差矩阵。

同时可以根据递推方程得到 Jacobian 迭代公式，表示

\[\mathbf{J}_{t+\delta t}=\left(\mathbf{I}+\mathbf{F}_{t} \delta t\right) \mathbf{J}_{t}, \quad t \in[k, k+1], \quad \mathbf{J}_{b_{k}}=\mathbf{I}\]

离散形式的 PVQ 增量误差分析、协方差和 Jacobian

离散形式的方程，在 vins_estimator/src/factor/integration_base.h 中的 void IntegrationBase::midPointIntegration() 函数实现，为了和代码一致，修改了变量的顺序(并且，既然是为了和第 k 帧与第 k+1 帧图像帧之间的 IMU 帧相对应，下式中的下角标应改为 i 和 i + 1 更加合适？)

\[\left[\begin{array}{c} \delta \alpha_{k+1} \\ \delta \theta_{k+1} \\ \delta \beta_{k+1} \\ \delta b_{a_{k+1}} \\ \delta b_{w_{k+1}} \end{array}\right]=\left[\begin{array}{ccccc} 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{array}\right]\left[\begin{array}{c} \delta \alpha_{k} \\ \delta \theta_{k} \\ \delta \beta_{k} \\ \delta b_{a_{k}} \\ \delta b_{w_{k}} \end{array}\right]+ \\ \left[\begin{array}{ccccc} 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{array}\right]\left[\begin{array}{c} n_{a_{k}} \\ n_{w_{k}} \\ n_{a_{k+1}} \\ n_{w_{k+1}} \\ n_{b_{a}} \\ n_{b_{w}} \end{array}\right]\]

可以简写为

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

下面是推导过程

由于偏置的导数符合高斯分布，所以

\[\delta b_{a_{k+1}}=\delta b_{a_{k}}+n_{b_{a}} \delta t, \quad \delta b_{w_{k+1}}=\delta b_{w_{k}}+n_{b_{w}} \delta t\]

之前已经求出了角度误差导数的连续形式

\[\delta \dot{\theta_{t}^{b_{k}}}=-\left[\hat{w}_{t}-b_{w_{t}}\right]_{\times} \delta \theta_{t}^{b_{k}}-n_{w}-\delta b_{w_{t}}\]

那么其中值积分的离散形式为

\[\delta \dot{\theta}_{k}=-\left[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2}-b_{w_{k}}\right]_{\times} \delta \theta_{k}-\frac{n_{w_{k}}+n_{w_{k+1}}}{2}-\delta b_{w_{k}}\]

根据导数定义下一时刻的误差为

\[\delta \theta_{k+1}=\delta \theta_{k}+\delta \dot{\theta}_{k} \delta t=\left(I-\left[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2}-b_{w_{k}}\right]_{\times} \delta t\right) \delta \theta_{k}-\frac{n_{w_{k}}+n_{w_{k+1}}}{2} \delta t-\delta b_{w_{k}} \delta t\]

整理可以得到

\[\begin{aligned} \delta \theta_{k+1}&=\left(I-\left[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2}-b_{w_{k}}\right]_{\times} \delta t\right) \delta \theta_{k}-\delta t \delta b_{w_{k}}-\frac{\delta t}{2} n_{w_{k}}-\frac{\delta t}{2} n_{w_{k+1}} \\ &= f_{11} \delta \theta_{k}-\delta t \delta b_{w_{k}}-\frac{\delta t}{2} n_{w_{k}}-\frac{\delta t}{2} n_{w_{k+1}} \\ \end{aligned}\]

得

\[f_{11}=\left(I-\left[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2}-b_{w_{k}}\right]_{\times} \delta t\right)\]

之前已经得到速度误差的导数的连续形式

\[\delta \dot{\beta}_{t}^{b_{k}}=-R_{t}^{b_{k}}\left[\hat{a}_{t}-b_{a_{t}}\right]_{\times} \delta \theta-R_{t}^{b_{k}} \delta b_{a_{t}}-R_{t}^{b_{k}} n_{a}\]

那么其中值积分的离散形式为

\[\delta \dot{\beta}_{k}=-\frac{1}{2} R_{k}\left[\hat{a}_{k}-b_{a_{k}}\right]_{\times} \delta \theta_{k}- \frac{1}{2} R_{k+1}\left[\hat{a}_{k+1}-b_{a_{k}}\right]_{\times} \delta \theta_{k+1}-\\ \frac{1}{2}\left(R_{k}+R_{k+1}\right) \delta b_{a_{k}}-\frac{1}{2} R_{k} n_{a_{k}}-\frac{1}{2} R_{k+1} n_{a_{k+1}}\]

把之前 k+1 时刻角度的误差代入可得

\[\delta \dot{\beta}_{k}=\left\{-\frac{1}{2} R_{k}\left[\hat{a}_{k}-b_{a_{k}}\right]_{\times}-\frac{1}{2} R_{k+1}\left[\hat{a}_{k+1}-b_{a_{k}}\right]_{\times}\left(I-\left[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2}-b_{w_{k}}\right]_{\times} \delta t\right)\right\} \delta \theta_{k} \\ +\delta \beta_{k}+\left\{\frac{1}{4} R_{k+1}\left[\hat{a}_{k+1}-b_{a_{k}}\right]_{\times} \delta t\left(n_{w_{k}}+n_{w_{k+1}}\right)+\frac{1}{2} R_{k+1}\left[\hat{a}_{k+1}-b_{a_{k}}\right]_{\times} \delta t \delta b_{w_{k}}\right. \\ \left.-\frac{1}{2}\left(R_{k}+R_{k+1}\right) \delta b_{a_{k}}-\frac{1}{2} R_{k} n_{a_{k}}-\frac{1}{2} R_{k+1} n_{a_{k+1}}\right\} \delta t\]

根据导数定义可知下一时刻的误差为

\[\delta \beta_{k+1}=\delta \beta_{k}+\delta \dot{\beta}_{k} \delta t=\left\{-\frac{1}{2} R_{k}\left[\hat{a}_{k}-b_{a_{k}}\right]_{\times}-\frac{1}{2} R_{k+1}\left[\hat{a}_{k+1}-b_{a_{k}}\right]_{\times}\left(I-\left[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2}-b_{w_{k}}\right]_{\times} \delta t\right)\right\} \delta t \delta \theta_{k} \\ +\delta \beta_{k}+\left\{\frac{1}{4} R_{k+1}\left[\hat{a}_{k+1}-b_{a_{k}}\right]_{\times} \delta t\left(n_{w_{k}}+n_{w_{k+1}}\right)+\frac{1}{2} R_{k+1}\left[\hat{a}_{k+1}-b_{a_{k}}\right]_{\times} \delta t \delta b_{w_{k}}\right. \\ \left.-\frac{1}{2}\left(R_{k}+R_{k+1}\right) \delta b_{a_{k}}-\frac{1}{2} R_{k} n_{a_{k}}-\frac{1}{2} R_{k+1} n_{a_{k+1}}\right\} \delta t\]

整理后可以得到

\[\begin{array}{c} \delta \beta_{k+1}=f_{21} \delta \theta_{k}+\delta \beta_{k}-\frac{1}{2}\left(R_{k}+R_{k+1}\right) \delta t \delta b_{a_{k}}+f_{24} \delta b_{w_{k}} \\ -\frac{1}{2} R_{k} \delta t n_{a_{k}}-\frac{1}{2} R_{k+1} \delta t n_{a_{k+1}}+v_{21} n_{w_{k}}+v_{23} n_{w_{k+1}} \end{array}\]

其中

\[f_{21}=-\frac{1}{2} R_{k}\left[\hat{a}_{k}-b_{a_{k}}\right]_{x} \delta t-\frac{1}{2} R_{k+1}\left[\hat{a}_{k+1}-b_{a_{k}}\right]_{\times}\left(I-\left[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2}-b_{w_{k}}\right]_{x} \delta t\right) \delta t \\ f_{24}=\frac{1}{2} R_{k+1}\left[\hat{a}_{k+1}-b_{a_{k}}\right]_{\times} \delta t^{2} \\ v_{21}=v_{23}=\frac{1}{4} R_{k+1}\left[\hat{a}_{k+1}-b_{a_{k}}\right]_{\times} \delta t^{2}\]

之前已经得到

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

其离散形式为

\[\begin{array}{c} \delta \dot{\alpha}_{k}=\frac{1}{2} \delta \beta_{k}+\frac{1}{2} \delta \beta_{k+1} \\ =\beta_{k}+\frac{1}{2} f_{21} \delta \theta_{k}-\frac{1}{4}\left(R_{k}+R_{k+1}\right) \delta t \delta b_{a_{k}}+\frac{1}{2} f_{24} \delta b_{w_{k}} \\ -\frac{1}{4} R_{k} \delta t n_{a_{k}}-\frac{1}{4} R_{k+1} \delta t n_{a_{k+1}}+\frac{1}{2} v_{21} n_{w_{k}}+\frac{1}{2} v_{23} n_{w_{k+1}} \end{array}\]

根据导数定义可得下一时刻的误差为

\[\begin{array}{c} \delta \alpha_{k+1}=\delta \alpha_{k}+\delta \dot{\alpha}_{k} \delta t \\ =\delta \alpha_{k}+f_{01} \delta \theta_{k}+\delta t \delta \beta_{k}+f_{03} \delta b_{a_{k}}+f_{04} \delta b_{w_{k}}+v_{00} n_{a_{k}}+v_{01} n_{w_{k}}+v_{02} n_{a_{k+1}}+v_{03} n_{w_{k+1}} \end{array}\]

其中

\[\begin{array}{c} f_{01}=\frac{1}{2} \delta t \delta \theta_{k}, \quad f_{03}=-\frac{1}{4}\left(R_{k}+R_{k+1}\right) \delta t^{2}, \quad f_{04}=\frac{1}{2} f_{24} \delta t \\ v_{00}=\frac{1}{4} R_{k} \delta t^{2}, \quad v_{01}=v_{03}=\frac{1}{2} v_{21} \delta t, \quad v_{02}=\frac{1}{4} R_{k+1} \delta t^{2} \end{array}\]

并且可以得到协方差的迭代公式

\[P_{k+1}=F P_{k} F^{T}+V Q V^{T}, \quad P_{0}=0\]

噪声项的对角协方差矩阵为

\[Q^{18 \times 18}=\left[\begin{array}{llllll} \sigma_{a}^{2} & 0 & 0 & 0 & 0 & 0 \\ 0 & \sigma_{w}^{2} & 0 & 0 & 0 & 0 \\ 0 & 0 & \sigma_{a}^{2} & 0 & 0 & 0 \\ 0 & 0 & 0 & \sigma_{w}^{2} & 0 & 0 \\ 0 & 0 & 0 & 0 & \sigma_{b_{a}}^{2} & 0 \\ 0 & 0 & 0 & 0 & 0 & \sigma_{b_{w}}^{2} \end{array}\right]\]

根据协方差的迭代公式，可以得到 Jacobian 的迭代公式为

\[J_{k+1}=F J_{k}, \quad J_{0}=I\]