误差状态方程与雅可比矩阵

误差状态方程与雅可比矩阵

1. 误差状态方程

在惯性的优化中包括p、v、q、ba、bg、外参等等优化变量 。
 

预积分量由陀螺、加计的示数得到，以及陀螺、加计的白噪声、偏置。可以先将白噪声从预积分量中剥离出来，作为预积分量测的协方差阵；计算预积分关于偏置的雅可比矩阵，用于预积分的更新。从而能够避免重复计算预积分项。

在后端优化迭代的时候，当对优化变量更新的时候，ba、bg发生了变化，那么预积分量就需要重新进行更新；同时惯性数据计算预积分量产生约束的时候，为了更加契合紧耦合的概念，预积分需要有一个权重，就把预积分协方差的倒数作为信息阵。

所以需要知道预积分的测量噪声，以及关于bias的雅可比矩阵。

这两个变量可以通过推导误差状态方程得到。

即error_state=真实状态(真实测量值，包含噪声，陀螺、加表的白噪声)-nomial_state(无噪声理论值)。

在推导过程中也是把真实值用误差加噪声的形式进行表达。

由此建立关于优化变量的误差加噪声的形式。

2.雅可比矩阵

残差项对两个时刻的位姿、偏置进行求导能够得到雅可比矩阵