Исправьте состояния, используя прямые измерения состояния для insfilterMARG
correct(
корректирует состояние и ошибку расчета состояния на основе ковариации измерения и измерения. Измерение преобразуется непосредственно в состояние, заданное индексами FUSE
,idx
,measurement
,measurementCovariance
)idx
.