在IMU在slam系统中的应用(二)中给出了IMU预积分计算推导以及代码,这里给出IMU误差递推方程的推导。
(一)一段时间多个IMU数据预积分量协方差
单个IMU数据作为测量值,其噪声方差我们可以标定,但是一段时间内多个IMU数据积分形成的预积分量的方差应该怎么求解呢?
- 协方差传递公式
当,并且服从分布,那么的协方差为:
- 相邻时刻误差线性传递方程
误差包括两部分,上一时刻的误差传递给当前时刻,上一时刻的噪声传递给当前时刻。
假设上一时刻的状态量误差为,上一时刻得测量噪声为,那么,传递给当前时刻误差线性传递方程为:
- 协方差传递公式
所以,协方差传递方程为:
(二)非线性方程的误差递推方程推导
通常,状态量之间的递推关系是非线性的方程,如,其中为状态量,为观测量。可以用两种方式来推导状态误差传递的线性递推关系。
- 基于一阶泰勒展开的误差传递方程
令状态量为,其中为计算得到的预测状态量,为实际真值,为误差,观测量的噪声为,所以,对非线性状态方程进行一阶泰勒展开
代入误差和噪声得:
所以展开后:
其中为状态量对状态量的雅可比矩阵,为状态量对观测量的雅可比矩阵。
因此状态误差的递推方程为:
- 基于误差随时间变化的递推方程(论文中的推导)
如果可以推导状态误差随时间变化的导数关系,如:
那么误差状态的传递方程为:
代入到一阶泰勒展开的误差递推方程中,可以推导:
,
- 基于泰勒展开和基于误差随时间变化方法对比
为什么要用误差随时间的变化来进行误差递推方程的构建呢?
在应用(一)(二)的推导中,我们已经知道状态的导数和状态之间的关系,比如已经知道速度和状态量之间的关系:
因此可以推导速度的误差导数和状态误差之间的关系,在上式中加上各自误差则有
由此,就可以推导写出整个和和其他方程了。
(三)IMU的PVQ增量误差、协方差以及雅可比矩阵的递推方程
我们已经建立起IMU预积分测量值的推导,在前置知识下,需要将IMU预积分运用到非线性优化中,需要建立线性高斯误差状态递推方程,由线性高斯系统的协方差,推导协方差矩阵,并求解对应的雅可比矩阵。
首先,由于四元数被参数化过,所以将其误差定义为围绕其均值的扰动:
由预积分的连续形式建立的运动模型,以一段时间内IMU构建的预积分量作为测量值,对两时刻之间的状态量进行约束,可以得到时刻和时刻下的误差项。
之前已经推导过世界坐标系中的预积分和PVQ状态的关系,从世界坐标系转换到本体坐标系,该关系可以写成:
以预积分作为测量量,可以得到误差项为:
因此根据误差递推方程的推导,可以给出时刻误差项的线性化递推方程:
简写为:
进一步的,根据导数的定义,
即可得到:
得到与误差随时间的变化来进行误差递推方程类似的方程,令,可以得到非线性系统的线性化递推误差方程:
再,对于下一时刻的协方差,根据协方差递推公式,由高斯分布线性变换协方差的传递规律,可以预测下一时刻的协方差:
其中表示时刻的协方差,表示噪声项的对角协方差矩阵,初始协方差为:
同时也可以根据递推方程得到对应的雅可比矩阵迭代公式:
其中初始化雅可比矩阵为单位阵。