Исправьте состояния, используя данные GPS для insfilterAsync
[ предохраняет данные о положении GPS, чтобы исправить оценку состояния.res,resCov] = fusegps(FUSE,position,positionCovariance)
[ предохраняет данные о положении и скорости GPS, чтобы исправить оценку состояния.res,resCov] = fusegps(FUSE,position,positionCovariance,velocity,velocityCovariance)
insfilter | insfilterAsync | insfilterMARG (Sensor Fusion and Tracking Toolbox)