insfilterErrorState

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

Описание

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

Создание

Синтаксис

filter = insfilterErrorState

filter = insfilterErrorState('ReferenceFrame',RF)

filter = insfilterErrorState(___,Name,Value)

Описание

пример

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` — Мультипликативное отклонение шума процесса от гироскопа ((rad/s) ²)
`[1e-6 1e-6 1e-6]` (значение по умолчанию) | скаляр | вектор-строка с 3 элементами

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

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

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

`GyroscopeBiasNoise` — Отклонение шума аддитивного процесса от смещения гироскопа ((rad/s) ²)
`[1e-9 1e-9 1e-9]` (значение по умолчанию) | скаляр | вектор-строка с 3 элементами

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

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

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

`AccelerometerNoise` — Мультипликативное отклонение шума процесса от акселерометра (^(m/s2) ²)
`[1e-4 1e-4 1e-4]` (значение по умолчанию) | скаляр | вектор-строка с 3 элементами

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

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

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

`AccelerometerBiasNoise` — Отклонение шума аддитивного процесса от смещения акселерометра (^(m/s2) ²)
`[1e-4 1e-4 1e-4]` (значение по умолчанию) | скаляр | вектор-строка с 3 элементами

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

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

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

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

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

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

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

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

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

Ковариация состояния	Строка/Индекс столбца
δ Вектор вращения (XYZ)	1:3
δ Положение (NED или ENU)	4:6
δ Скорость (NED или ENU)	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`
`copy`	Создайте копию `insfilterErrorState`

Примеры

свернуть все

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

Скрипт Open Live Script

Загрузите записанные данные наземного транспортного средства после круговой траектории. .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

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

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')

Алгоритмы

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

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

$x = [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \\ p o s i t i o n_{N} \\ p o s i t i o n_{E} \\ p o s i t i o n_{D} \\ v_{N} \\ v_{E} \\ v_{D} \\ g y r o b i a s_{X} \\ g y r o b i a s_{Y} \\ g y r o b i a s_{Z} \\ a c c e l b i a s_{X} \\ a c c e l b i a s_{Y} \\ \begin{array}{l} a c c e l b i a s_{Z} \\ \begin{matrix} s c a l e F a c t o r \end{matrix} \end{array} \end{matrix}]$

где

q ₀, q ₁, q ₂, q ₃ – Части кватерниона ориентации. Кватернион ориентации представляет вращение системы координат от текущей ориентации платформы до локальной системы координат NED.
position _N, position _E, position _D – Положение платформы в локальной системе координат NED.
gyrobias _X, gyrobias _Y, gyrobias _Z – Сместите в чтении гироскопа.
accelbias _X, accelbias _Y, accelbias _Z – Сместите в чтении акселерометра.
scaleFactor Масштабный коэффициент оценки положения.

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

$x_{k | k - 1} = f ({\hat{x}}_{k - 1 | k - 1})$

предсказанная оценка состояния:

$x_{k | k - 1} = [\begin{matrix} q_{0} + Δ t * q_{1} (g y r o b i a s_{X} / 2 - g y r o_{X} / 2) + Δ t * q_{2} * (g y r o b i a s_{Y} / 2 - g y r o_{Y} / 2) + Δ t * q_{3} * (g y r o b i a s_{Z} / 2 - g y r o_{Z} / 2) \\ q_{1} - Δ t * q_{0} (g y r o b i a s_{X} / 2 - g y r o_{X} / 2) + Δ t * q_{3} * (g y r o b i a s_{Y} / 2 - g y r o_{Y} / 2) - Δ t * q_{2} * (g y r o b i a s_{Z} / 2 - g y r o_{Z} / 2) \\ q_{2} - Δ t * q_{3} (g y r o b i a s_{X} / 2 - g y r o_{X} / 2) - Δ t * q_{0} * (g y r o b i a s_{Y} / 2 - g y r o_{Y} / 2) + Δ t * q_{1} * (g y r o b i a s_{Z} / 2 - g y r o_{Z} / 2) \\ q_{3} + Δ t * q_{2} (g y r o b i a s_{X} / 2 - g y r o_{X} / 2) - Δ t * q_{1} * (g y r o b i a s_{Y} / 2 - g y r o_{Y} / 2) - Δ t * q_{0} * (g y r o b i a s_{Z} / 2 - g y r o_{Z} / 2) \\ p o s i t i o n_{N} + Δ t * v_{N} \\ p o s i t i o n_{E} + Δ t * v_{E} \\ p o s i t i o n_{D} + Δ t * v_{D} \\ v_{N} - Δ t * {\begin{cases} q_{0} * (q_{0} * (a c c e l b i a s_{X} - a c c e l_{X}) - q_{3} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{2} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + g_{N} + \\ q_{2} * (q_{1} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{2} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + \\ q_{1} * (q_{1} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{2} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{3} * (a c c e l b i a s_{Z} - a c c e l_{Z})) - \\ q_{3} * (q_{3} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{1} * (a c c e l b i a s_{Z} - a c c e l_{Z})) \end{cases}} \\ v_{E} - Δ t * {\begin{cases} q_{0} * (q_{3} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{1} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + g_{E} - \\ q_{1} * (q_{1} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{2} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + \\ q_{2} * (q_{1} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{2} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{3} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + \\ q_{3} * (q_{0} * (a c c e l b i a s_{X} - a c c e l_{X}) - q_{3} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{2} * (a c c e l b i a s_{Z} - a c c e l_{Z})) \end{cases}} \\ v_{D} - Δ t * {\begin{cases} q_{0} * (q_{1} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{2} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + g_{D} + \\ q_{1} * (q_{3} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{1} * (a c c e l b i a s_{Z} - a c c e l_{Z})) - \\ q_{2} * (q_{0} * (a c c e l b i a s_{X} - a c c e l_{X}) - q_{3} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{2} * (a c c e l b i a s_{Z} - a c c e l_{Z})) - \\ q_{3} * (q_{1} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{2} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{3} * (a c c e l b i a s_{Z} - a c c e l_{Z})) \end{cases}} \\ g y r o b i a s_{X} \\ g y r o b i a s_{Y} \\ g y r o b i a s_{Z} \\ a c c e l b i a s_{X} \\ a c c e l b i a s_{Y} \\ a c c e l b i a s_{Z} \\ s c a l e F a c t o r \end{matrix}]$

где

Δt – Шаг расчета IMU.
g _N, g _E, g _D – Постоянный вектор силы тяжести в системе координат NED.

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

Генерация кода C/C++
Генерация кода C и C++ с помощью MATLAB® Coder™.

Введенный в R2019a

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

insfilterErrorState

Описание

Создание

Синтаксис

Описание

Свойства

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

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

`GyroscopeNoise` — Мультипликативное отклонение шума процесса от гироскопа ((rad/s) ²)
`[1e-6 1e-6 1e-6]` (значение по умолчанию) | скаляр | вектор-строка с 3 элементами

`GyroscopeBiasNoise` — Отклонение шума аддитивного процесса от смещения гироскопа ((rad/s) ²)
`[1e-9 1e-9 1e-9]` (значение по умолчанию) | скаляр | вектор-строка с 3 элементами

`AccelerometerNoise` — Мультипликативное отклонение шума процесса от акселерометра (^(m/s2) ²)
`[1e-4 1e-4 1e-4]` (значение по умолчанию) | скаляр | вектор-строка с 3 элементами

`AccelerometerBiasNoise` — Отклонение шума аддитивного процесса от смещения акселерометра (^(m/s2) ²)
`[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™.

Смотрите также

Документация Sensor Fusion and Tracking Toolbox

Поддержка

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

insfilterErrorState

Описание

Создание

Синтаксис

Описание

Свойства

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

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

GyroscopeNoise — Мультипликативное отклонение шума процесса от гироскопа ((rad/s) 2) [1e-6 1e-6 1e-6] (значение по умолчанию) | скаляр | вектор-строка с 3 элементами

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

AccelerometerNoise — Мультипликативное отклонение шума процесса от акселерометра ((m/s2) 2) [1e-4 1e-4 1e-4] (значение по умолчанию) | скаляр | вектор-строка с 3 элементами

AccelerometerBiasNoise — Отклонение шума аддитивного процесса от смещения акселерометра ((m/s2) 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™.

Смотрите также

Документация Sensor Fusion and Tracking Toolbox

Поддержка

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

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

`GyroscopeNoise` — Мультипликативное отклонение шума процесса от гироскопа ((rad/s) ²)
`[1e-6 1e-6 1e-6]` (значение по умолчанию) | скаляр | вектор-строка с 3 элементами

`GyroscopeBiasNoise` — Отклонение шума аддитивного процесса от смещения гироскопа ((rad/s) ²)
`[1e-9 1e-9 1e-9]` (значение по умолчанию) | скаляр | вектор-строка с 3 элементами

`AccelerometerNoise` — Мультипликативное отклонение шума процесса от акселерометра (^(m/s2) ²)
`[1e-4 1e-4 1e-4]` (значение по умолчанию) | скаляр | вектор-строка с 3 элементами

`AccelerometerBiasNoise` — Отклонение шума аддитивного процесса от смещения акселерометра (^(m/s2) ²)
`[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™.