insfilterErrorState

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

Описание

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

Создание

Описание

пример

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

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

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

Свойства

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

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

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

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

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

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

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

  • Если GyroscopeNoise задан как скаляр, один элемент применяется к каждой оси.

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

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

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

  • Если GyroscopeBiasNoise задан как скаляр, один элемент применяется к каждой оси

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

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

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

  • Если AccelerometerNoise задан как скаляр, один элемент применяется к каждой оси.

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

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

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

  • Если AccelerometerBiasNoise задан как скаляр, один элемент применяется к каждой оси.

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

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

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

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

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

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

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

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

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

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

Примеры

свернуть все

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

где

  • 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 Масштабный коэффициент оценки положения.

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

xk|k1=f(x^k1|k1)

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

xk|k1=[q0+Δtq1(gyrobiasX/2gyroX/2)+Δtq2(gyrobiasY/2gyroY/2)+Δtq3(gyrobiasZ/2gyroZ/2)q1Δtq0(gyrobiasX/2gyroX/2)+Δtq3(gyrobiasY/2gyroY/2)Δtq2(gyrobiasZ/2gyroZ/2)q2Δtq3(gyrobiasX/2gyroX/2)Δtq0(gyrobiasY/2gyroY/2)+Δtq1(gyrobiasZ/2gyroZ/2)q3+Δtq2(gyrobiasX/2gyroX/2)Δtq1(gyrobiasY/2gyroY/2)Δtq0(gyrobiasZ/2gyroZ/2)positionN+ΔtvNpositionE+ΔtvEpositionD+ΔtvDvNΔt{q0(q0(accelbiasXaccelX)q3(accelbiasYaccelY)+q2(accelbiasZaccelZ))+gN+q2(q1(accelbiasYaccelY)q2(accelbiasXaccelX)+q0(accelbiasZaccelZ))+q1(q1(accelbiasXaccelX)+q2(accelbiasYaccelY)+q3(accelbiasZaccelZ))q3(q3(accelbiasXaccelX)+q0(accelbiasYaccelY)q1(accelbiasZaccelZ))}vEΔt{q0(q3(accelbiasXaccelX)+q0(accelbiasYaccelY)q1(accelbiasZaccelZ))+gEq1(q1(accelbiasYaccelY)q2(accelbiasXaccelX)+q0(accelbiasZaccelZ))+q2(q1(accelbiasXaccelX)+q2(accelbiasYaccelY)+q3(accelbiasZaccelZ))+q3(q0(accelbiasXaccelX)q3(accelbiasYaccelY)+q2(accelbiasZaccelZ))}vDΔt{q0(q1(accelbiasYaccelY)q2(accelbiasXaccelX)+q0(accelbiasZaccelZ))+gD+q1(q3(accelbiasXaccelX)+q0(accelbiasYaccelY)q1(accelbiasZaccelZ))q2(q0(accelbiasXaccelX)q3(accelbiasYaccelY)+q2(accelbiasZaccelZ))q3(q1(accelbiasXaccelX)+q2(accelbiasYaccelY)+q3(accelbiasZaccelZ))}gyrobiasXgyrobiasYgyrobiasZaccelbiasXaccelbiasYaccelbiasZscaleFactor]

где

  • Δt – Шаг расчета IMU.

  • g N, g E, g D – Постоянный вектор силы тяжести в системе координат NED.

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

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

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

| |

Введенный в R2019a

Для просмотра документации необходимо авторизоваться на сайте