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

insfilterMARG

Оценка позы на основе данных MARG и GPS

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

Описание

insfilterMARG объект реализует сенсорное слияние данных MARG и GPS для оценки позы в опорном кадре NED (или ENU). Данные MARG (магнитная, угловая скорость, сила тяжести) обычно получают из датчиков магнитометра, гироскопа и акселерометра. Фильтр использует 22-элементный вектор состояния для отслеживания кватерниона ориентации, скорости, положения, смещений датчика MARG и геомагнитного вектора. insfilterMARG объект использует расширенный фильтр Калмана для оценки этих величин.

Создание

Синтаксис

фильтр = insfilterMARG

filter = insfilterMARG («Ссылочный кадр», RF)

filter = insfilterMARG (___, имя, значение)

Описание

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

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

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

Свойства

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

`AccelerometerNoise` - Мультипликативная дисперсия технологического шума от акселерометра (м/с2) 2
`1e-4` (по умолчанию) | скаляр | 3-элементный вектор строки

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

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

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

`AccelerometerBiasNoise` - Мультипликативная дисперсия технологического шума от смещения акселерометра (м/с2) 2
`1e-4` (по умолчанию) | положительный скаляр | 3-элементный вектор строки

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

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

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

`GeomagneticVectorNoise` - Аддитивный технологический шум для геомагнитного вектора (µT2)
`1e-6` (по умолчанию) | положительный скаляр | 3-элементный вектор строки

Аддитивный шум процесса для геомагнитного вектора в ^µT2, определяемый как скалярный или 3-элементный вектор строки положительных вещественных чисел.

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

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

`MagnetometerBiasNoise` - Аддитивный технологический шум для смещения магнитометра (µT2)
`0.1` (по умолчанию) | положительный скаляр | 3-элементный вектор строки

Аддитивный шум процесса для смещения магнитометра в ^µT2, определяемый как скалярный или 3-элементный вектор строки.

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

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

`State` - Вектор состояния расширенного фильтра Кальмана
22-элементный вектор столбца

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

Государство	Единицы	Индекс
Ориентация (части кватерниона)	Н/Д	1:4
Положение (NED или RUS)	m	5:7
Скорость (NED или RUS)	м/с	8:10
Смещение по дельта-углу (XYZ)	радиус	11:13
Смещение дельта-скорости (XYZ)	м/с	14:16
Вектор геомагнитного поля (NED или RUS)	µT	17:19
Смещение магнитометра (XYZ)	µT	20:22

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

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

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

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

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

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

Примеры

свернуть все

Оценка положения БПЛА

Открыть сценарий в реальном времени

Этот пример показывает, как оценить позу беспилотного летательного аппарата (БПЛА) по записанным данным датчиков и позе истинности земли.

Загрузите записанные данные датчика и позу истинности земли БПЛА.

load uavshort.mat

Инициализируйте insfilterMARG объект фильтра.

f = insfilterMARG;
f.IMUSampleRate = imuFs;
f.ReferenceLocation = refloc;
f.AccelerometerBiasNoise = 2e-4;
f.AccelerometerNoise = 2;
f.GyroscopeBiasNoise = 1e-16;
f.GyroscopeNoise = 1e-5;
f.MagnetometerBiasNoise = 1e-10;
f.GeomagneticVectorNoise = 1e-12;
f.StateCovariance = 1e-9*ones(22);
f.State = initstate;
 
gpsidx = 1;
N = size(accel,1);
p = zeros(N,3);
q = zeros(N,1,'quaternion');

Плавкий акселерометр, гироскоп, магнитометр и данные GPS.

for ii = 1:size(accel,1)               % Fuse IMU
   f.predict(accel(ii,:), gyro(ii,:));
        
   if ~mod(ii,fix(imuFs/2))            % Fuse magnetometer at 1/2 the IMU rate
       f.fusemag(mag(ii,:),Rmag);
   end
  
   if ~mod(ii,imuFs)                   % Fuse GPS once per second
       f.fusegps(lla(gpsidx,:),Rpos,gpsvel(gpsidx,:),Rvel);
       gpsidx = gpsidx + 1;
   end
 
   [p(ii,:),q(ii)] = pose(f);           %Log estimated pose
end

Расчет и отображение среднеквадратичных ошибок.

posErr = truePos - p;
qErr = rad2deg(dist(trueOrient,q));
pRMS = sqrt(mean(posErr.^2));
qRMS = sqrt(mean(qErr.^2));
fprintf('Position RMS Error\n\tX: %.2f, Y: %.2f, Z: %.2f (meters)\n\n',pRMS(1),pRMS(2),pRMS(3));

Position RMS Error
	X: 0.57, Y: 0.53, Z: 0.68 (meters)

    
fprintf('Quaternion Distance RMS Error\n\t%.2f (degrees)\n\n',qRMS);

Quaternion Distance RMS Error
	0.28 (degrees)

Алгоритмы

Примечание.Следующий алгоритм применяется только к опорному кадру NED.

insfilterMARG использует 22-осевую расширенную структуру фильтра Калмана для оценки позы в опорном кадре NED. Состояние определяется как:

$x = \begin{matrix} _{[} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \end{matrix}$ q0q1q2q3posityNposityEposityDstartNstartEü DΔstartbiasXΔstartbiasZΔObjectstartbiasXΔ

где

q0, q1, q2, q3 -- Части ориентации кватерниона. Кватернион ориентации представляет поворот кадра от текущей ориентации платформы к локальной системе координат NED.
posityN, posityE, posityD - положение платформы в локальной системе координат NED.
startN, startE, startD - скорость платформы в локальной системе координат NED.
ΔstartbiasX, ΔobjectbiasY, ΔstartbiasZ - смещение в интегрированном считывании гироскопа.
ΔstartbiasX, ΔstartbiasY, ΔstartbiasZ - Смещение в интегрированном показании акселерометра.
geompienStartVectorN, geompyTVectorE, geompytperVectorD - оценка вектора геомагнитного поля в опорном местоположении.
magbiasX, magbiasY, magbiasZ - смещение в показаниях магнитометра.

Учитывая традиционное формирование прогнозируемой оценки состояния,

$_{xk 'k −} 1 {\overset{}{=}}_{f (x^} k_{-}$ 1 | k − 1, uk)

uk управляется данными акселерометра и гироскопа, которые были преобразованы в дельта-скорость и дельта-угол посредством трапециевидного интегрирования. Прогнозная оценка состояния:

$_{xk'k−1} = [\begin{matrix} _{}_{q0−q1} \frac{_{}_{}}{} (ΔθX-ΔθbiasX2)_{} \frac{−q2_{}_{}}{}_{(ΔθY−ΔθbiasY2)} \frac{_{−q3}_{}}{} \\ _{}_{(ΔθZ−ΔθbiasZ2)} \frac{_{} q1+q0_{}}{}_{} \frac{_{(ΔθX−ΔθbiasX2)} −q3_{}}{}_{} \frac{_{} (ΔθY−ΔθbiasY2) +q2_{}}{} \\ _{}_{} \frac{_{} (ΔθZ−ΔθbiasZ2)_{q2+q3}}{}_{} \frac{_{} (ΔθX−ΔθbiasX2)_{}}{} +q0_{} \frac{_{}_{(ΔθY−ΔθbiasY2)}}{} \\ _{−q1}_{} \frac{_{}_{(ΔθZ−ΔθbiasZ2)}}{}_{} q3-q2 \frac{_{}_{}}{} {(ΔθX−ΔθbiasX2)}_{} \frac{{+q1}_{}_{}}{} \\ (ΔθY−ΔθbiasY2) {+q0}_{}_{} \\ (ΔθZ−ΔθbiasZ2)_{} положение +_{} \\ (Δt)_{}_{} (νN) positionE \\ +_{} (Δt)_{}_{} (νE) помещаемый +_{} {(Δt)}_{}^{}_{_{}}^{} {(νD) νN}_{+}^{}_{}^{(Δt)} (gN)_{+}_{}_{}_{(ΔνX−ΔνbiasX)}_{}_{}_{} (q02+q12−q22−q32)_{} -2_{}_{}_{}_{} \\ _{} (ΔνY-ΔνbiasY)_{} {(q0q3−q1q2)}_{} +2_{}_{}^{}_{_{(ΔνZ−ΔνbiasZ)}}^{}_{}^{}_{}^{} (q0q2+q1q3) {νE}_{+} (Δt)_{} (GE)_{+}_{}_{}_{}_{(ΔνY−ΔνbiasY)}_{}_{}_{}_{} {(q02−q12+q22−q32)}_{} \\ {+2}_{}_{} {(ΔνX−ΔνbiasX)}_{}_{} {(q0q3+q1q2)}_{}^{} -2_{_{}}^{}_{}^{}_{}^{} (ΔνZ−ΔνbiasZ)_{}_{(q0q1−q2q3)} {νD}_{+}_{}_{(Δt)}_{} (gD) +_{}_{}_{(ΔνZ−ΔνbiasZ)}_{}_{}_{} \\ _{} \\ (q02−q12−q22+q32) {−2}_{} \\ _{} \\ {(ΔνX−ΔνbiasX)}_{} \\ _{} \\ (q0q2−q1q3)_{+2} \\ (ΔνY−ΔνbiasY)_{} \\ (q0q1+q2q3)_{} \\ _{} \\ _{} \\ _{} \\ _{} \end{matrix}$ ΔθbiasXΔθbiasYΔθbiasZΔνbiasXΔνbiasYΔνbiasZ geomagneticFieldVectorNgeomagneticFieldVectorEgeomagneticFieldVectorDmagbiasXmagbiasYmagbiasZ]

где

ΔstartX, ΔstartY, ΔstartZ - Интегрированное считывание гироскопа.
ΔstartX, ΔstartY, ΔstartZ - показания интегрированного акселерометра.
Δt - время выборки IMU.
gN, gE, gD - вектор постоянной гравитации в кадре NED.

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

Создание кода C/C + +
Создайте код C и C++ с помощью MATLAB ® Coder™

.

См. также

insfilterAsync | insfilterErrorState | insfilterNonholonomic

Темы

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

Представлен в R2018b

Документация по комплекту инструментов для слияния и отслеживания датчиков

Поддержка