insfilterErrorState

Оценка позы на основе данных IMU, GPS и монокулярной визуальной одометрии (MVO)

Описание

insfilterErrorState объект реализует сенсорное слияние данных IMU, GPS и монокулярной визуальной одометрии (МВО) для оценки позы в опорном кадре NED (или RUS). Фильтр использует 17-элементный вектор состояния для отслеживания ориентации quaternion, скорость, положение, смещения датчика IMU и коэффициент масштабирования MVO. insfilterErrorState объект использует фильтр Калмана состояния ошибки для оценки этих величин.

Создание

Синтаксис

filter = insfilterErrorState

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

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

Описание

пример

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

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

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

Свойства

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Вектор состояния расширенного фильтра Калмана, заданный как 17-элементный вектор столбца. Значения состояния представляют:

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

Исходное состояние по умолчанию соответствует объекту в состоянии покоя, расположенному в [0 0 0] в геодезических координатах ЛЛА.

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

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

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

Ковариация штата	Индекс строки/столбца
δ Вектор вращения (XYZ)	1:3
δ Положение (NED или RUS)	4:6
δ Скорость (NED или RUS)	7:9
δ Смещение гироскопа (XYZ)	10:12
δ Смещение акселерометра (XYZ)	13:15
δ Визуальная шкала одометрии (XYZ)	16

Обратите внимание, что поскольку это фильтр Калмана состояния ошибки, он отслеживает ошибки в состояниях. δ представляет ошибку в соответствующем состоянии.

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

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

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

Примеры

свернуть все

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

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

Загрузка зарегистрированных данных наземного транспортного средства, идущего по круговой траектории. .mat файл содержит измерения датчиков IMU и GPS, а также ориентацию и положение на земле.

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

Создайте фильтр INS для предохранения IMU и данных GPS с помощью фильтра Калмана в состоянии ошибки.

initialState = [compact(trueOrient(1)),truePos(1,:),-6.8e-3,2.5002,0,zeros(1,6),1].';
filt = insfilterErrorState;
filt.IMUSampleRate = imuFs;
filt.ReferenceLocation = localOrigin;
filt.State = initialState;
filt.StateCovariance = initialStateCovariance;

Предварительно распределить переменные для положения и ориентации. Выделение переменной для индексирования в данные GPS.

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

gpsIdx = 1;

Плавкий акселерометр, гироскоп и данные GPS. Внешняя петля предсказывает фильтр вперед на уровне самой быстрой частоты дискретизации (частота дискретизации IMU).

for idx = 1:numIMUSamples

    % Use predict to estimate the filter state based on the accelData and
    % gyroData arrays.
    predict(filt,accelData(idx,:),gyroData(idx,:));
    
    % GPS data is collected at a lower sample rate than IMU data. Fuse GPS
    % data at the lower rate.
    if mod(idx, imuFs / gpsFs) == 0
        % Correct the filter states based on the GPS data.
        fusegps(filt,gpsLLA(gpsIdx,:),Rpos,gpsVel(gpsIdx,:),Rvel);
        gpsIdx = gpsIdx + 1;
    end
    
    % Log the current pose estimate
    [estPos(idx,:), estOrient(idx,:)] = pose(filt);
end

Вычислите среднеквадратичные ошибки между известной истинной позицией и ориентацией и выходными данными фильтра состояния ошибки.

pErr = truePos - estPos;
qErr = rad2deg(dist(estOrient,trueOrient));

pRMS = sqrt(mean(pErr.^2));
qRMS = sqrt(mean(qErr.^2));

fprintf('Position RMS Error\n');

Position RMS Error

fprintf('\tX: %.2f, Y: %.2f, Z: %.2f (meters)\n\n',pRMS(1),pRMS(2),pRMS(3));

	X: 0.40, Y: 0.24, Z: 0.05 (meters)

fprintf('Quaternion Distance RMS Error\n');

Quaternion Distance RMS Error

fprintf('\t%.2f (degrees)\n\n',qRMS);

	0.30 (degrees)

Визуализация истинного положения и расчетного положения.

plot(truePos(:,1),truePos(:,2),estPos(:,1),estPos(:,2),'r:','LineWidth',2)
grid on
axis square
xlabel('N (m)')
ylabel('E (m)')
legend('Ground Truth','Estimation')

Figure contains an axes. The axes contains 2 objects of type line. These objects represent Ground Truth, Estimation.

Алгоритмы

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

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

$x = \begin{matrix} _{[} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ \begin{array}{l} _{} \\ \begin{matrix} \end{matrix} \end{array} \end{matrix}$ q0q1q2q3posityNposiceDvNvEvDgyrobiasXgyrobiasZaccelbiasXaccelbiasYaccelbiasYaccelbiasZscityFactor]

где

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

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

$_{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)}_{}_{} \\ _{}_{} \\ _{} \begin{array}{l} _{}_{}_{} positionN +Δt∗vNpositionE +ΔtvEpositionD +Δt∗vDvN−Δ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 _{}_{} (accelbiasX-accelX) \end{array} \\ _{} +q0 \begin{array}{l} _{}_{} {(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 _{}_{} {(accelbiasZ−accelZ)}_{)} \end{array} \\ _{+q3 } (q0 \begin{array}{l} _{}_{}_{} (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 _{}_{} (accelbiasY-accelY) \end{array} \\ +q2 _{} \\ _{} \\ (accelbiasZ−accelZ))_{} \\ −q3 (q1 _{} \\ _{} \\ (accelbiasX−accelX)_{} \\ +q2 \end{matrix}$ (accelbiasY−accelY) +q3 (accelbiasZ−accelZ))} gyrobiasXgyrobiasYgyrobiasZaccelbiasXaccelbiasYaccelbiasZscaleFactor]

где

Δt - время выборки IMU.
gN, gE, gD - вектор постоянной гравитации в кадре NED.

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

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

См. также

insfilterAsync | insfilterMARG | insfilterNonholonomic

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

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

insfilterErrorState

Описание

Создание

Синтаксис

Описание

Свойства

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

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

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

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

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

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

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

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

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

Примеры

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

Алгоритмы

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

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

См. также

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

Поддержка

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

insfilterErrorState

Описание

Создание

Синтаксис

Описание

Свойства

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

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

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

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

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

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

State - Вектор состояния фильтра Кальмана [1;zeros(15,1);1] (по умолчанию) | 17-элементный вектор столбца

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

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

Примеры

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

Алгоритмы

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

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

См. также

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

Поддержка

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

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

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

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

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

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

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

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

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