Оцените положение от IMU, GPS и данных о монокулярной визуальной одометрии (MVO)
insfilterErrorState
возразите сплаву датчика реализаций IMU, GPS и данных о монокулярной визуальной одометрии (MVO), чтобы оценить положение в NED (или ENU) система координат. Фильтр использует вектор состояния с 17 элементами, чтобы отследить ориентацию quaternion
, скорость, положение, смещения датчика IMU и масштабный коэффициент MVO. insfilterErrorState
возразите использует Фильтр Калмана состояния ошибки, чтобы оценить эти количества.
создает filter
= insfilterErrorStateinsfilterErrorState
объект со значениями свойств по умолчанию.
позволяет вам задавать систему координат, filter
= insfilterErrorState('ReferenceFrame'
,RF
)RF
, из filter
. Задайте RF
как 'NED'
(Северо-восток вниз) или 'ENU'
(Восточный Север). Значением по умолчанию является 'NED'
.
также позволяет вам свойства набора созданного filter
= insfilterErrorState(___,Name,Value
)filter
использование одной или нескольких пар "имя-значение". Заключите каждое имя свойства в одинарные кавычки.
predict | Обновите состояния с помощью данных о гироскопе и акселерометра |
fusegps | Правильные состояния с помощью данных о GPS |
fusemvo | Правильные состояния с помощью монокулярной визуальной одометрии |
correct | Правильные состояния с помощью прямых измерений состояния |
pose | Текущая ориентация и оценка положения |
reset | Сбросьте внутренние состояния |
stateinfo | Отобразите информацию о векторе состояния |
Примечание: следующий алгоритм только применяется к системе координат NED.
insfilterErrorState
использует структуру Фильтра Калмана состояния ошибки с 17 осями, чтобы оценить положение в системе координат NED. Состояние задано как:
где
q 0, q 1, q 2, q 3 – Части кватерниона ориентации. Кватернион ориентации представляет вращение системы координат от текущей ориентации платформы до локальной системы координат NED.
position N, position E, position D – Положение платформы в локальной системе координат NED.
gyrobias X, gyrobias Y, gyrobias Z – Сместите в чтении гироскопа.
accelbias X, accelbias Y, accelbias Z – Сместите в чтении акселерометра.
scaleFactor Масштабный коэффициент оценки положения.
Учитывая обычную формулировку функции изменения состояния,
предсказанная оценка состояния:
где
Δt – Шаг расчета IMU.
g N, g E, g D – Постоянный вектор силы тяжести в системе координат NED.