这里主要说明预积分做了意见什么事情
-
用IMU进行位姿等状态推算。原本是将加速度信息和陀螺仪信息转换到世界坐标系下,然后积分,得到速度、位置和旋转。如下
-
这样就存在一个问题,在优化过程中, q w b t q_{wb_t} qwbt等是状态量,会在迭代过程中变化。这样每一次迭代就需要重新计算积分,计算量较大。
-
每次加速度和角速度的积分可以不基于世界坐标系,而基于上次帧位姿。这样积分的结果就不随着状态量改变而改变了。如下
-
这样还有一个问题,在积分过程中加速度和角速度的bias是变化的,而我们在同一次积分过程中,用的是同一个值。这样不合理。但是我们可以认为在同一个bias下积分的结果,是真实结果的一阶泰勒展开。这样我们就可以反推真实结果(虽然也是近似的)。如下所示。其中 ∗ ‾ \overline{*} ∗是积分得到的结果, ∗ * ∗是反推得到的真是值。
-
这样就得到了imu得到的位姿等状态量。这样就可以构建残差,如下
-
对于优化而言,需要三个要素。残差、雅克比和协方差。根据残差,可得雅克比。
-
那么最后一个问题,协方差如何得到呢?
我们只知道加速度和陀螺仪的方差和bias的方差,但是残差是多个加速度计和陀螺仪积分的结果,他们协方差是什么呢?
可以得到本次积分结果与上一次积分结果、本次imu方差之间的递推关系。
一个测量量由两部分组成,真值+误差。而这里讨论方差,和真值没有关系。测量值的方差=误差的方差
这样,积分结果就有这样一个递推关系。这里 δ x k \delta x_k δxk是本次的积分误差, δ x k − 1 \delta x_{k-1} δxk−1是上次的积分误差, n k − 1 n_{k-1} nk−1是本次测量值的误差。
根据
所以,知道误差的递推方程,就可以得到协方差的递推方程。即要确定 F 、 G F、G F、G
又所以 F 、 G F、G F、G就是 x k x_k xk对 x k − 1 x_{k-1} xk−1和 u k − 1 u_{k-1} uk−1的雅克比。
-
这样就求得了协方差的递推公式