如何推导IMU预积分的协方差矩阵？（解决IMU预积分置信度的问题）

上次在最后讨论了IMU观测置信度的问题，VINS-mono中是通过误差卡尔曼滤波理论来计算和估计的。

1 什么是误差卡尔曼滤波？

卡尔曼滤波和误差卡尔曼滤波的区别如上图所示。

卡尔曼滤波通过对测量值和预测值进行滤波得到一个较准确的真实值。
而误差卡尔曼滤波的输入参数为4个，$\delta$$x$是我们认为预测值和真实值的误差（这里设为0，因为如果我们能知道这个量我们就直接把它加到$x$上了），$\delta$$y$是测量值与预测值之间的误差，另外两个参数分别是$\delta$$x$和$\delta$$y$的置信度，最终经过卡尔曼滤波得到一个置信度较高的误差量。我们将此误差量加到预测值上就是我们认为的较为准确的真实值。

因为我们讨论的是一段时间内多个 IMU 数据积分形成的预积分量的问题，我们要解决途中每个时刻对下一时刻的影响是什么？也就是说相邻两时刻，误差是怎么传递的。

通过上面我们对误差卡尔曼的概括，我们可以看出相邻时刻误差的传递是由两部分组成的：
①当前时刻的状态量误差传递给下一时刻；
②当前时刻的测量噪声传递给下一时刻。

2 为什么要用误差卡尔曼滤波？

在误差卡尔曼滤波里，用的是旋转向量表示旋转误差。

①这就避免了像四元数那样过参数化的旋转表示，这样协方差矩阵也是$3\times 3$的形式。虽然旋转向量具有周期性，但是在表示误差的时候相邻两帧旋转误差肯定是在$[-\pi ,\pi ]$之间的。
②通常观测值和预测值是相当接近的，用误差量来进行优化也是避免了万向节死锁等问题。
③误差小意味着其对状态变量的二阶导是可以忽略的，这样后面计算雅可比矩阵就相对容易了。

3 VINS-mono中对误差卡尔曼滤波的应用

这是VINS-mono论文。
VINS-mono中引入误差卡尔曼滤波的主要原因是因为其旋转是通过四元数来表示的，而四元数是过参数化的。
VINS-mono中旋转误差的表示为：

误差状态帮助IMU预积分计算协方差矩阵时避免过参数化。

3.1 IMU误差状态传递的推导

我们想知道上一帧的误差是怎么传导到下一帧的，这个过程可以用下式表示：

如果已知上面的误差状态传递方程，那么一段时间内IMU积分的协方差矩阵就可以轻松递推算出：

其中，${\sum }_n$是测量噪声的协方差矩阵，方差从 $i$ 时刻开始进行递推，${\sum }_{ii}=0$。
所以我们必须推导出IMU误差状态传递方程。

---

如果我们能够推导状态误差随时间变化的导数关系，比如：

则误差状态的传递方程为：

所以我们得先推出$\delta$$x$的导数。

3.1.1 连续时间IMU误差状态传递的推导

这一部分可见论文的附录A。
先推出了$\delta$$x$的导数。

然后给出了协防差递归计算公式。

3.1.2 基于中值积分的离散时间IMU预积分误差状态传递的推导

上面有了连续时间下误差的变化量，论文中没有写，但是可以根据$\delta$$x$的导数反推啊，形式如下：

由于实际的slam过程中，我们收到的IMU信息是离散时刻的，所以推导离散时间IMU预积分误差状态传递形式很重要！写进代码里解决实际问题啊。
推导结果是这，好吧超出我能力范围了。

想看具体推导过程的的，可以私信我，因为深蓝学院的课件没法上传到资源里，涉及侵权。还有对应的课程。

4 非常感谢您的阅读！