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

insfilterMARG

Оценка положения по данным MARG и GPS

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

Описание

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Аддитивный технологический шум для смещения магнитометра в мкТ², заданный как скалярный или 3-элементный вектор-строка.

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

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

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

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

Государство	Модули	Индекс
Ориентация (кватернионные части)	Н/Д	1:4
Положение (NED или ENU)	m	5:7
Скорость (NED или ENU)	м/с	8:10
Смещение угла дельты (XYZ)	рад	11:13
Смещение скорости в дельте (XYZ)	м/с	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`	Исправьте состояния, используя прямые измерения состояния для `insfilterMARG`
`residual`	Невязки и остаточные ковариации от прямых измерений состояния для `insfilterMARG`
`fusegps`	Исправьте состояния, используя данные GPS для `insfilterMARG`
`residualgps`	Невязки и остаточная ковариация от измерений для `insfilterMARG`
`fusemag`	Исправьте состояния, используя данные магнитометра для `insfilterMARG`
`residualmag`	Невязки и остаточная ковариация от измерений магнитометра для `insfilterMARG`
`pose`	Текущая ориентация и оценка положения для `insfilterMARG`
`predict`	Обновите состояния, используя данные акселерометра и гироскопа для `insfilterMARG`
`reset`	Сбросьте внутренние состояния для `insfilterMARG`
`stateinfo`	Отобразите информацию о векторе состояния для `insfilterMARG`
`tune`	Настройка `insfilterMARG` параметры для уменьшения ошибки расчета
`copy`	Создание копии `insfitlerMARG`

Примеры

свернуть все

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

Открыть Live Script

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

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

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 0, _q 1, q 2_, q 3 -- Части ориентационного кватерниона. Кватернион ориентации представляет поворот системы координат от текущей ориентации платформы к локальной системе координат NED.
position N, position E, position D -- Положение платформы в локальной системе координат NED.
ν N, _ν E, ν D - Скорость платформы в локальной системе координат NED.
.R. θbias X, И θbias Y, И _θbias Z - Смещение во встроенном показании гироскопа.
.R. νbias X, И νbias Y, И _νbias Z - Смещение в интегрированных показаниях акселерометра.
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}]$

где

.R. _θ X, _В θ Y_, В θ Z - Интегрированное чтение гироскопа.
.R. _ν X, _В ν Y_, В ν Z - Интегрированные показания акселерометра.
И t -- шаг расчета БИНС.
g N, _g E, g D - Вектор постоянной гравитации в системе координат.

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

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

.

См. также

insfilterAsync | insfilterErrorState | insfilterNonholonomic

Темы

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

Введенный в R2018b

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

Поддержка

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