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