insfilterNonholonomic

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

Описание

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

Создание

Описание

пример

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

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

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

Свойства

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

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

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

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

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

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

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

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

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

  • Если GyroscopeNoise задан как скаляр, один элемент применяется к x, y и осям z гироскопа.

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

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

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

  • Если GyroscopeBiasNoise задан как скаляр, один элемент применяется к x, y и осям z гироскопа.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Примеры

свернуть все

В этом примере показано, как оценить положение наземного транспортного средства от регистрируемого 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

Вычислите и отобразите ошибки RMS.

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=[q0q1q2q3gyrobiasXgyrobiasYgyrobiasZpositionNpositionEpositionDvNvEvDaccelbiasXaccelbiasYaccelbiasZ]

где

  • q 0, q 1, q 2, q 3 – Части кватерниона ориентации. Кватернион ориентации представляет вращение системы координат от текущей ориентации платформы до локальной системы координат NED.

  • gyrobias X, gyrobias Y, gyrobias Z – Сместите в чтении гироскопа.

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

  • ν N, ν E, ν D – Скорость платформы в локальной системе координат NED.

  • accelbias X, accelbias Y, accelbias Z – Сместите в чтении акселерометра.

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

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)gryobiasX(Δtλgyro1)gryobiasY(Δtλgyro1)gryobiasZ(Δtλgyro1)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))}accelbiasX(Δtλaccel1)accelbiasY(Δtλaccel1)accelbiasZ(Δtλaccel1)]

где

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

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

  • accel X, accel Y, accel Z – Ускоряющий вектор в системе координат тела.

  • λ accel – Смещение акселерометра затухает фактор.

  • λ gyro – Смещение гироскопа затухает фактор.

Ссылки

[1] Munguía, R. "Помогшая GPS Инерционная Система навигации в Прямой Настройке". Журнал прикладного исследования и технологии. Издание 12, Номер 4, 2014, стр 803 – 814.

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

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

Введенный в R2018b

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