Документация

insfilterNonholonomic

Оценка позы с неголономными ограничениями

развернуть все на странице

Описание

insfilterNonholonomic объект реализует сенсорное слияние блока инерциальных измерений (IMU) и данных GPS для оценки позы в опорном кадре NED (или RUS). Данные IMU получают из данных гироскопа и акселерометра. Фильтр использует 16-элементный вектор состояния для отслеживания кватерниона ориентации, скорости, положения и смещения датчика IMU. insfilterNonholonomic объект использует расширенный фильтр Калмана для оценки этих величин.

Создание

Синтаксис

filter = insfilterНеголономный

filter = insfilterNonholonomic («Ссылочный кадр», RF)

filter = insfilterNonholonomic (___, имя, значение)

Описание

filter = insfilterNonholonomic создает insfilterErrorState со значениями свойств по умолчанию.

filter = insfilterNonholonomic('ReferenceFrame',RF) позволяет указать опорную рамку, RF, из filter. Определить RF как 'NED' (Север-Восток-вниз) или 'ENU' (Восток-Север-Вверх). Значение по умолчанию: 'NED'.

filter = insfilterNonholonomic(___,Name,Value) также позволяет задать свойства созданного filter с использованием одной или нескольких пар имя-значение. Заключите каждое имя свойства в отдельные кавычки.

Свойства

развернуть все

`IMUSampleRate` - Частота дискретизации IMU (Гц)
`100` (по умолчанию) | положительный скаляр

Частота дискретизации IMU в Гц, заданная как положительный скаляр.

Типы данных: single | double

`ReferenceLocation` - Опорное местоположение (град., град., метров)
`[0 0 0]` (по умолчанию) | 3-элементный положительный вектор строки

Опорное местоположение, определяемое как 3-элементный вектор строки в геодезических координатах (широта, долгота и высота). Высота - высота над эталонной эллипсоидной моделью, WGS84. Ссылочные единицы местоположения - [градусы метра].

Типы данных: single | double

`DecimationFactor` - Коэффициент прореживания для коррекции кинематической зависимости
`2` (по умолчанию) | целочисленный скаляр

Коэффициент прореживания для кинематической коррекции ограничения, заданный как положительный целочисленный скаляр.

Типы данных: single | double

`GyroscopeNoise` - Мультипликативная дисперсия шума процесса от гироскопа (рад/с) 2
`[4.8e-6 4.8e-6 4.8e-6]` (по умолчанию) | скаляр | 3-элементный вектор строки

Мультипликативная дисперсия шума процесса от гироскопа в (рад/с) 2, заданная как скалярный или 3-элементный вектор строк положительных вещественных конечных чисел .

Если GyroscopeNoise задается как вектор строки, элементы соответствуют шуму в осях x, y и z гироскопа соответственно.
Если GyroscopeNoise задается как скаляр, один элемент применяется к осям x, y и z гироскопа.

Типы данных: single | double

`GyroscopeBiasNoise` - Мультипликативная дисперсия шума процесса от смещения гироскопа (рад/с) 2
`[4e-14 4e-14 4e-14]` (по умолчанию) | скаляр | 3-элементный вектор строки

Мультипликативная дисперсия шума процесса от смещения гироскопа в (рад/с) 2, заданная как скалярный или 3-элементный вектор строки положительных вещественных конечных чисел. Смещение гироскопа моделируется как процесс белого шума с фильтрацией нижних частот.

Если GyroscopeBiasNoise задается как вектор строки, элементы соответствуют шуму в осях x, y и z гироскопа соответственно.
Если GyroscopeBiasNoise задается как скаляр, один элемент применяется к осям x, y и z гироскопа.

Типы данных: single | double

`GyroscopeBiasDecayFactor` - Коэффициент затухания для смещения гироскопа
`0.999` (по умолчанию) | скаляр в диапазоне [0,1]

Коэффициент затухания для смещения гироскопа, заданный как скаляр в диапазоне [0,1]. Коэффициент распада 0 моделирует смещение гироскопа как процесс белого шума. Коэффициент распада 1 моделирует смещение гироскопа как случайный процесс ходьбы.

Типы данных: single | double

`AccelerometerNoise` - Мультипликативная дисперсия технологического шума от акселерометра (м/с2) 2
`[4.8e-2 4.8e-2 4.8e-2]` (по умолчанию) | скаляр | 3-элементный вектор строки

Мультипликативная дисперсия шума процесса от акселерометра в (^m/s2) 2, заданная как скалярный или 3-элементный вектор строки положительных вещественных конечных чисел.

Если AccelerometerNoise задается как вектор строки, элементы соответствуют шуму в осях x, y и z акселерометра соответственно.
Если AccelerometerNoise задается как скаляр, один элемент применяется к каждой оси.

Типы данных: single | double

`AccelerometerBiasNoise` - Мультипликативная дисперсия технологического шума от смещения акселерометра (м/с2) 2
`[4e-14 4e-14 4e-14]` (по умолчанию) | положительный скаляр | 3-элементный вектор строки

Мультипликативная дисперсия шума процесса от смещения акселерометра в (^m/s2) 2, заданная как скалярный или 3-элементный вектор строки положительных вещественных чисел. Смещение акселерометра моделируется как процесс белого шума с фильтрацией нижних частот.

Если AccelerometerBiasNoise задается как вектор строки, элементы соответствуют шуму в осях x, y и z акселерометра соответственно.
Если AccelerometerBiasNoise задается как скаляр, один элемент применяется к каждой оси.

`AccelerometerBiasDecayFactor` - Коэффициент затухания для смещения акселерометра
`0.9999` (по умолчанию) | скаляр в диапазоне [0,1]

Коэффициент затухания для смещения акселерометра, заданный как скаляр в диапазоне [0,1]. Коэффициент распада 0 моделирует смещение акселерометра как процесс белого шума. Коэффициент распада 1 моделирует смещение акселерометра как случайный процесс ходьбы.

Типы данных: single | double

`State` - Вектор состояния расширенного фильтра Кальмана
`[1;zeros(15,1)]` | 16-элементный вектор столбца

Вектор состояния расширенного фильтра Калмана. Значения состояния представляют:

Государство	Единицы	Индекс
Ориентация (части кватерниона)	Н/Д	1:4
Смещение гироскопа (XYZ)	рад/с	5:7
Положение (NED или RUS)	m	8:10
Скорость (NED или RUS)	м/с	11:13
Смещение акселерометра (XYZ)	^м/с2	14:16

Типы данных: single | double

`StateCovariance` - Ковариация ошибок состояния для расширенного фильтра Калмана
`eye(16)` (по умолчанию) | матрица 16 на 16

Ковариация ошибок состояния для расширенного фильтра Калмана, заданная как матрица 16 на 16 элементов, или вещественные числа.

Типы данных: single | double

`ZeroVelocityConstraintNoise` - Шум ограничения скорости (м/с) 2
`1e-2` (по умолчанию) | неотрицательный скаляр

Шум ограничения скорости в (м/с) 2, заданный как неотрицательный скаляр.

Типы данных: single | double

Функции объекта

`correct`	Корректные состояния с использованием прямых измерений состояния для `insfilterNonholonomic`
`residual`	Остатки и остаточные ковариации от прямых измерений состояния для `insfilterNonholonomic`
`fusegps`	Корректные состояния с использованием данных GPS для `insfilterNonholonomic`
`residualgps`	Остатки и остаточная ковариация измерений GPS для `insfilterNonholonomic`
`pose`	Текущая ориентация и оценка положения для `insfilterNonholonomic`
`predict`	Обновление состояний с использованием данных акселерометра и гироскопа для `insfilterNonholonomic`
`reset`	Сброс внутренних состояний для `insfilterNonholonomic`
`stateinfo`	Отображение информации о векторе состояния для `insfilterNonholonomic`
`tune`	Мелодия `insfilterNonholonomic` параметры для уменьшения ошибки оценки
`copy`	Создать копию `insfitlerNonholonomic`

Примеры

свернуть все

Оценка положения наземного транспортного средства

Открыть сценарий в реальном времени

В этом примере показано, как оценить позу наземного транспортного средства по зарегистрированным измерениям IMU и GPS-датчиков и ориентации и положению истинности земли.

Загрузить зарегистрированные данные наземного транспортного средства по круговой траектории.

load('loggedGroundVehicleCircle.mat','imuFs','localOrigin','initialState','initialStateCovariance','accelData',...
      'gyroData','gpsFs','gpsLLA','Rpos','gpsVel','Rvel','trueOrient','truePos');

Инициализируйте insfilterNonholonomic объект.

filt = insfilterNonholonomic;
filt.IMUSampleRate = imuFs;
filt.ReferenceLocation = localOrigin;
filt.State = initialState;
filt.StateCovariance = initialStateCovariance;
    
imuSamplesPerGPS = imuFs/gpsFs;

Данные журнала для окончательного расчета метрики. Используйте predict функция объекта для оценки состояния фильтра на основе данных акселерометра и гироскопа. Затем исправьте состояние фильтра в соответствии с данными GPS.

numIMUSamples = size(accelData,1);
estOrient = quaternion.ones(numIMUSamples,1);
estPos = zeros(numIMUSamples,3);
    
gpsIdx = 1;

for idx = 1:numIMUSamples
    predict(filt,accelData(idx,:),gyroData(idx,:));       %Predict filter state
    
    if (mod(idx,imuSamplesPerGPS) == 0)                   %Correct filter state
        fusegps(filt,gpsLLA(gpsIdx,:),Rpos,gpsVel(gpsIdx,:),Rvel);
        gpsIdx = gpsIdx + 1;
    end
    
    [estPos(idx,:),estOrient(idx,:)] = pose(filt);        %Log estimated pose
end

Расчет и отображение среднеквадратичных ошибок.

posd = estPos - truePos;
quatd = rad2deg(dist(estOrient,trueOrient));
msep = sqrt(mean(posd.^2));

fprintf('Position RMS Error\n\tX: %.2f, Y: %.2f, Z: %.2f (meters)\n\n',msep(1),msep(2),msep(3));

Position RMS Error
	X: 0.15, Y: 0.11, Z: 0.01 (meters)

    
fprintf('Quaternion Distance RMS Error\n\t%.2f (degrees)\n\n',sqrt(mean(quatd.^2)));

Quaternion Distance RMS Error
	0.26 (degrees)

Алгоритмы

Примечание.Следующий алгоритм применяется только к опорному кадру NED.

insfilterNonholonomic использует 16-осевую структуру фильтра Калмана состояния ошибки для оценки позы в опорном кадре NED. Состояние определяется как:

$x = \begin{matrix} _{[} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \end{matrix}$ q0q1q2q3gyrobiasXgyrobiasYgyrobiasZposityNposityEposityDvEvDaccelbiasXaccelbiasYaccelbiasZ]

где

q0, q1, q2, q3 -- Части ориентации кватерниона. Кватернион ориентации представляет поворот кадра от текущей ориентации платформы к локальной системе координат NED.
gyrobiasX, gyrobiasY, gyrobiasZ - смещение в чтении гироскопа.
posityN, posityE, posityD - положение платформы в локальной системе координат NED.
startN, startE, startD - скорость платформы в локальной системе координат NED.
accelbiasX, accelbiasY, accelbiasZ - смещение в показаниях акселерометра.

Учитывая традиционную формулировку функции перехода состояния,

$_{xk 'k −} 1 {\overset{}{=}}_{f (x^} k$ − 1 | k − 1)

прогнозируемая оценка состояния:

$_{xk'k−1} = [\begin{matrix} _{}_{q0 +Δt∗q1}_{}_{} (gyrobiasX/2-gyroX/2) +_{} Δtq2 _{}_{} (gyrobiasY/2−gyroY/2) +_{} Δtq3 _{}_{} \\ _{} (gyrobiasZ/2−gyroZ/2)_{} q1−Δt∗q0_{}_{}_{} (gyrobiasX/2-gyroX/2) + Δtq3 _{}_{}_{} (gyrobiasY/2−gyroY/2)_{−Δtq2 }_{} \\ _{}_{} (gyrobiasZ/2−gyroZ/2)_{} q2−Δt∗q3_{}_{} (gyrobiasX/2−gyroX/2)_{} −Δtq0 _{}_{} (gyrobiasY/2−gyroY/2)_{+} Δtq1 _{} \\ _{}_{} {(gyrobiasZ/2−gyroZ/2)}_{}_{q3 +Δt∗q2}_{}_{} (gyrobiasX/2−gyroX/2)_{} −Δtq1 _{}_{} (gyrobiasY/2−gyroY/2)_{} \\ -Δtq0 _{}_{} (gyrobiasZ/2-gyroZ/2) \\ _{} -gryobiasX ({Δt}_{} \\ *λgyro-1)_{} −gryobiasY (_{Δt} \\ ∗λgyro−1)_{}_{} \\ −gryobiasZ (Δt_{} *λgyro-1)_{} \\ _{}_{} \\ _{} \begin{array}{l} _{}_{}_{}_{}_{positionN +Δt∗vNpositionE +ΔtvEpositionD +ΔtvDvN +Δt } {q0 (q0 _{}_{}_{} (accelbiasX−accelX) {−q3 }_{}_{}_{} \\ _{}_{(accelbiasY−accelY)} +q2 _{}_{}_{} (accelbiasZ-accelZ)) {−gN+q2 }_{} (q1 _{}_{}_{} (accelbiasY-accelY) {−q2 }_{} \\ _{}_{} (accelbiasX−accelX)_{} +q0 _{}_{}_{(accelbiasZ−accelZ)}) {+q1 }_{} (_{q1 }_{}_{} (accelbiasX-accelX) \\ _{} +q2 _{}_{} (accelbiasY−accelY)_{} {+q3 }_{}_{}_{(accelbiasZ−accelZ)})_{} -q3 (q3 _{}_{} \end{array} \\ _{} (accelbiasX−accelX) \begin{array}{l} _{} +q0 _{}_{} (accelbiasY−accelY)_{} {−q1 }_{}_{}_{(accelbiasZ−accelZ)})}_{} vE +Δt {q0 (_{} q3 _{}_{} \\ _{}_{} (accelbiasX−accelX) +q0 _{}_{}_{} (accelbiasY−accelY) −q1 _{}_{}_{} (accelbiasZ−accelZ))_{} −gE−q1 (_{} q1 \\ _{}_{}_{} (accelbiasY-accelY) {−q2 }_{}_{}_{} (accelbiasX−accelX)_{} +q0 _{}_{} (accelbiasZ−accelZ))_{} \\ {+q2 }_{} (_{q1 }_{}_{} (accelbiasX-accelX)_{} +q2 _{}_{} {(accelbiasY−accelY)}_{} +q3 _{}_{} \end{array} \\ _{(accelbiasZ−accelZ)}) \begin{array}{l} {+q3 }_{} (_{q0 }_{}_{} (accelbiasX-accelX)_{} -q3 _{}_{} {(accelbiasY−accelY)}_{} +q2 _{}_{}_{(accelbiasZ−accelZ)}) \\ _{}}_{} vD +Δt {q0 ({q1 }_{}_{}_{} (accelbiasY−accelY)_{} -q2 _{}_{} (accelbiasX−accelX)_{} +q0 _{} \\ _{}_{} (accelbiasZ−accelZ))_{} −gD+q1 (_{} q3 _{}_{} {(accelbiasX−accelX)}_{}_{+q0 }_{}_{} (accelbiasY-accelY) \\ _{} -q1 _{}_{} (accelbiasZ−accelZ))_{}_{−q2 } (q0 _{}_{}_{} (accelbiasX-accelX) −q3 _{}_{} \end{array} \\ (accelbiasY−accelY) {+q2 }_{}_{} \\ (accelbiasZ−accelZ))_{−q3 } (q1 _{} \\ _{(accelbiasX−accelX)} +q2 _{} \end{matrix}$ (accelbiasY−accelY) +q3 (accelbiasZ−accelZ))} −accelbiasX (Δt ∗λaccel−1) −accelbiasY (Δt ∗λaccel−1) −accelbiasZ (Δt ∗λaccel−1)]

где

Δt - время выборки IMU.
gN, gE, gD - вектор постоянной гравитации в кадре NED.
accelX, accelY, accelZ - вектор ускорения в корпусе.
λ accel -- коэффициент затухания акселерометра.
λ gyro - коэффициент распада смещения гироскопа.

Ссылки

[1] Мунгиа, Р. «Система инерциальной навигации с помощью GPS в прямой конфигурации». Журнал прикладных исследований и технологий. Том 12, номер 4, 2014, стр. 803 - 814.

Расширенные возможности

Создание кода C/C + +
Создайте код C и C++ с помощью MATLAB ® Coder™

.

См. также

insfilterAsync | insfilterErrorState | insfilterMARG

Представлен в R2018b

Документация по панели инструментов навигации

Поддержка