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

insfilterNonholonomic

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

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

Описание

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

Создание

Синтаксис

filter = insfilterNonholonomic

filter = insfilterNonholonomic('ReferenceFrame',RF)

filter = insfilterNonholonomic(___,Name,Value)

Описание

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

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

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

Свойства

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

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

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

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

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

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

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

`DecimationFactor` — Фактор децимации для кинематической ограничительной коррекции
2 (значение по умолчанию) | положительный целочисленный скаляр

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

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

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

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

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

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

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

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

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

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

`GyroscopeBiasDecayFactor` — Фактор затухания для смещения гироскопа
0.999 (значение по умолчанию) | скаляр в области значений [0,1]

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

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

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

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

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

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

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

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

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

`AccelerometerBiasDecayFactor` — Фактор затухания для смещения акселерометра
0.9999 (значение по умолчанию) | скаляр в области значений [0,1]

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

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

`State` — Вектор состояния расширенного Фильтра Калмана
`[1;zeros(15,1)]` | Вектор-столбец с 16 элементами

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

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

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

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

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

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

`ZeroVelocityConstraintNoise` — Скоростной ограничительный шум (m/s) ²
`1e-2` (значение по умолчанию) | неотрицательный скаляр

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

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

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

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

Примеры

свернуть все

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

Скрипт Open Live Script

В этом примере показано, как оценить положение наземного транспортного средства от регистрируемого 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 = [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \\ 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} \\ 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} \\ 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} \end{matrix}]$

где

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

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

$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) \\ - g r y o b i a s_{X} * (Δ t * λ_{g y r o} - 1) \\ - g r y o b i a s_{Y} * (Δ t * λ_{g y r o} - 1) \\ - g r y o b i a s_{Z} * (Δ t * λ_{g y r o} - 1) \\ 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}} \\ - a c c e l b i a s_{X} * (Δ t * λ_{a c c e l} - 1) \\ - a c c e l b i a s_{Y} * (Δ t * λ_{a c c e l} - 1) \\ - a c c e l b i a s_{Z} * (Δ t * λ_{a c c e l} - 1) \end{matrix}]$

где

Δ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™.

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

insfilterAsync | insfilterErrorState | insfilterMARG

Введенный в R2018b

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

Поддержка