insfilterNonholonomic

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

Описание

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

Создание

Описание

пример

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

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

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

Свойства

расширить все

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Примеры

свернуть все

Этот пример показывает, как оценить положение наземного транспортного средства из зарегистрированных измерений БИНС и 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 -- шаг расчета БИНС.

  • g N, g E, g D - Вектор постоянной гравитации в системе координат.

  • accel X, accel Y accel Z -- Вектор ускорения в каркасе кузова.

  • λ accel -- Коэффициент распада смещения акселерометра.

  • λ gyro -- коэффициент распада гироскопа.

Ссылки

[1] Munguia, R. «A GPS-Aided Inertial Navigation System in Direct Configuration». Журнал прикладных исследований и технологий. Том 12, № 4, 2014, с. 803 - 814.

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

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

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