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

insfilterMARG

Оцените положение из данных о GPS и MARG

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

Описание

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

Создание

Синтаксис

filter = insfilterMARG

filter = insfilterMARG('ReferenceFrame',RF)

filter = insfilterMARG(___,Name,Value)

Описание

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` — Мультипликативное отклонение шума процесса от гироскопа (rad/s) ²
`1e-9` (значение по умолчанию) | скаляр | вектор-строка с 3 элементами

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

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

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

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

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

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

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

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

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

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

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

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

Мультипликативное отклонение шума процесса от акселерометра смещает в ^(m/s2) ² в виде скалярного или вектора-строки с 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 или ENU)	m	5:7
Скорость (NED или ENU)	m/s	8:10
Угловое смещение Delta (XYZ)	рад	11:13
Скоростное смещение Delta (XYZ)	m/s	14:16
Геомагнитный полевой вектор (NED или ENU)	µT	17:19
Смещение магнитометра (XYZ)	µT	20:22

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

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

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

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

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

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

Примеры

свернуть все

Оцените положение UAV

Скрипт Open Live Script

В этом примере показано, как оценить положение беспилотного воздушного транспортного средства (UAV) из регистрируемых данных о датчике и положение основной истины.

Загрузите регистрируемые данные о датчике и положение основной истины UAV.

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

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

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} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \\ 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} \\ ν_{N} \\ ν_{E} \\ ν_{D} \\ Δ θ b i a s_{X} \\ Δ θ b i a s_{Y} \\ Δ θ b i a s_{Z} \\ Δ ν b i a s_{X} \\ Δ ν b i a s_{Y} \\ Δ ν b i a s_{Z} \\ g e o m a g n e t i c F i e l d V e c t o r_{N} \\ g e o m a g n e t i c F i e l d V e c t o r_{E} \\ g e o m a g n e t i c F i e l d V e c t o r_{D} \\ m a g b i a s_{X} \\ m a g b i a s_{Y} \\ m a g b i a s_{Z} \end{matrix}]$

где

q ₀, q ₁, q ₂, q ₃ – Части кватерниона ориентации. Кватернион ориентации представляет вращение системы координат от текущей ориентации платформы до локальной системы координат NED.
position _N, position _E, position _D – Положение платформы в локальной системе координат NED.
ν _N, ν _E, ν _D – Скорость платформы в локальной системе координат NED.
ΔθbiasX, ΔθbiasY, ΔθbiasZ – Сместите в интегрированном чтении гироскопа.
ΔνbiasX, ΔνbiasY, ΔνbiasZ – Сместите в интегрированном чтении акселерометра.
geomagneticFieldVector _N, geomagneticFieldVector _E, geomagneticFieldVector _D – Оценка геомагнитного полевого вектора в ссылочном местоположении.
magbias _X, magbias _Y, magbias _Z – Сместите в показаниях магнитометра.

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

$x_{k | k - 1} = f ({\hat{x}}_{k - 1 | k - 1}, u_{k})$

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

$x_{k | k - 1} = [\begin{matrix} q_{0} - q_{1} (\frac{Δ θ_{X} - Δ θ b i a s_{X}}{2}) - q_{2} (\frac{Δ θ_{Y} - Δ θ b i a s_{Y}}{2}) - q_{3} (\frac{Δ θ_{Z} - Δ θ b i a s_{Z}}{2}) \\ q_{1} + q_{0} (\frac{Δ θ_{X} - Δ θ b i a s_{X}}{2}) - q_{3} (\frac{Δ θ_{Y} - Δ θ b i a s_{Y}}{2}) + q_{2} (\frac{Δ θ_{Z} - Δ θ b i a s_{Z}}{2}) \\ q_{2} + q_{3} (\frac{Δ θ_{X} - Δ θ b i a s_{X}}{2}) + q_{0} (\frac{Δ θ_{Y} - Δ θ b i a s_{Y}}{2}) - q_{1} (\frac{Δ θ_{Z} - Δ θ b i a s_{Z}}{2}) \\ q_{3} - q_{2} (\frac{Δ θ_{X} - Δ θ b i a s_{X}}{2}) + q_{1} (\frac{Δ θ_{Y} - Δ θ b i a s_{Y}}{2}) + q_{0} (\frac{Δ θ_{Z} - Δ θ b i a s_{Z}}{2}) \\ p o s i t i o n_{N} + (Δ t) (ν_{N}) \\ p o s i t i o n_{E} + (Δ t) (ν_{E}) \\ p o s i t i o n_{D} + (Δ t) (ν_{D}) \\ ν_{N} + (Δ t) (g_{N}) + (Δ ν_{X} - Δ ν b i a s_{X}) (q_{0}^{2} + q_{_{1}}^{2} - q_{2}^{2} - q_{3}^{2}) - 2 (Δ ν_{Y} - Δ ν b i a s_{Y}) (q_{0} q_{3} - q_{1} q_{2}) + 2 (Δ ν_{Z} - Δ ν b i a s_{Z}) (q_{0} q_{2} + q_{1} q_{3}) \\ ν_{E} + (Δ t) (g_{E}) + (Δ ν_{Y} - Δ ν b i a s_{Y}) (q_{0}^{2} - q_{_{1}}^{2} + q_{2}^{2} - q_{3}^{2}) + 2 (Δ ν_{X} - Δ ν b i a s_{X}) (q_{0} q_{3} + q_{1} q_{2}) - 2 (Δ ν_{Z} - Δ ν b i a s_{Z}) (q_{0} q_{1} - q_{2} q_{3}) \\ ν_{D} + (Δ t) (g_{D}) + (Δ ν_{Z} - Δ ν b i a s_{Z}) (q_{0}^{2} - q_{_{1}}^{2} - q_{2}^{2} + q_{3}^{2}) - 2 (Δ ν_{X} - Δ ν b i a s_{X}) (q_{0} q_{2} - q_{1} q_{3}) + 2 (Δ ν_{Y} - Δ ν b i a s_{Y}) (q_{0} q_{1} + q_{2} q_{3}) \\ Δ θ b i a s_{X} \\ Δ θ b i a s_{Y} \\ Δ θ b i a s_{Z} \\ Δ ν b i a s_{X} \\ Δ ν b i a s_{Y} \\ Δ ν b i a s_{Z} \\ g e o m a g n e t i c F i e l d V e c t o r_{N} \\ g e o m a g n e t i c F i e l d V e c t o r_{E} \\ g e o m a g n e t i c F i e l d V e c t o r_{D} \\ m a g b i a s_{X} \\ m a g b i a s_{Y} \\ m a g b i a s_{Z} \end{matrix}]$

где

ΔθX, ΔθY, ΔθZ – Интегрированное чтение гироскопа.
ΔνX, ΔνY, ΔνZ – Интегрированные показания акселерометра.
Δt – Шаг расчета IMU.
g _N, g _E, g _D – Постоянный вектор силы тяжести в системе координат NED.

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

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

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

insfilterAsync | insfilterErrorState | insfilterNonholonomic

Введенный в R2018b

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

Поддержка

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