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