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

insfilterNonholonomic

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

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

Описание

The insfilterNonholonomic объект реализует слияние датчиков инерциального измерительного блока (IMU) и данных GPS для оценки положения в опорной системе координат NED (или БИНС). Данные БИНС получают из данных гироскопа и акселерометра. Фильтр использует вектор состояния с 16 элементами, чтобы отслеживать смещения кватерниона ориентации, скорости, положения и датчика БИНС. The 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` - Частота дискретизации БИНС (Гц)
`100` (по умолчанию) | положительная скалярная величина

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

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

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

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

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

`DecimationFactor` - Коэффициент десятикратного уменьшения для кинематической коррекции ограничений
`2` (по умолчанию) | положительный целочисленный скаляр

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Мультипликативное отклонение шума процесса от смещения акселерометра в (м/с²)², заданный как скалярный или 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)	рад/с	5:7
Положение (NED или ENU)	m	8:10
Скорость (NED или ENU)	м/с	11:13
Смещение акселерометра (XYZ)	м/с²	14:16

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

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

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

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

`ZeroVelocityConstraintNoise` - Шум ограничений скорости (м/с)²
`1e-2` (по умолчанию) | неотрицательной скаляром

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

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

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

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

Примеры

свернуть все

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

Открыть Live Script

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

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

$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 -- шаг расчета БИНС.
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 ®

.

См. также

insfilterAsync | insfilterErrorState | insfilterMARG

Темы

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

Введенный в R2018b

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

Поддержка