无论是严恭敏老师的开源程序PSINS,还是牛小骥老师组开源的KF-GINS,在进行SINS机械编排时都是先更新速度和位置,再更新姿态,这是为什么呢?
可以从姿态、速度和位置更新的具体公式中看出其中缘由:
进行姿态更新时需要通过平均位置和平均速度计算n系相对于i系的平均角速率(计算平均速度和平均位置需要前一时刻和当前时刻的速度和位置),
但是进行速度更新时不需要当前历元的姿态(只需要上一历元的姿态),进行位置更新时需要平均速度,所以先进行速度更新(得到精度较高的速度),
在进行位置更新(得到位置较高的位置),然后计算出精度较高的平均角速率,再计算出精度较高的Cn2n,最后进行姿态更新(得到精度较高的姿态)。
注:进行速度更新时也需要计算n系相对于i系的平均角速率,为了提高速度的精度,KF-GINS中进行速度更新时,先用预测的平均速度和位置(精度较低)
计算当前时刻的速度,然后用此时的速度再重新计算平均速度和位置(精度较高),然后重新计算n系相对于i系的平均角速率,最后重新得到较高的精度的速度。
姿态更新:
速度更新:
位置更新: