MARGGPSFuser

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

Описание

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

Создание

Создайте MARGGPSFuser с помощью insfilter:

filt = insfilter('NonholonomicHeading',false,'Magnetometer',true);

Свойства

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

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

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

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

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

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

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

  • Если GyroscopeNoise задан как скаляр, один элемент применяется к x, y и осям z гироскопа.

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

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

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

  • Если GyroscopeBiasNoise задан как скаляр, один элемент применяется к каждой оси.

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

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

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

  • Если AccelerometerNoise задан как скаляр, один элемент применяется к каждой оси.

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

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

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

  • Если AccelerometerBiasNoise задан как скаляр, один элемент применяется к каждой оси.

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

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

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

  • Если GeomagneticVectorNoise задан как скаляр, один элемент применяется к каждой оси.

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

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

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

  • Если MagnetometerBiasNoise задан как скаляр, один элемент применяется к каждой оси.

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

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

СостояниеМодулиИндекс
Ориентация (части кватерниона)Нет данных1:4
Положение (NED)m5:7
Скорость (NED)m/s8:10
Угловое смещение Delta (XYZ)рад11:13
Скоростное смещение Delta (XYZ)m/s14:16
Геомагнитный полевой вектор (NED)µT17:19
Смещение магнитометра (XYZ)µT20:22

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

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

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

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

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

Примеры

свернуть все

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

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

load uavshort.mat

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

f = insfilter;
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)

Алгоритмы

MARGGPSFuser использует расширенную структуру Фильтра Калмана с 22 осями, чтобы оценить положение в ссылочном кадре NED. Состояние задано как:

x=[q0q1q2q3positionNpositionEpositionDνNνEνDΔθbiasXΔθbiasYΔθbiasZΔνbiasXΔνbiasYΔνbiasZgeomagneticFieldVectorNgeomagneticFieldVectorEgeomagneticFieldVectorDmagbiasXmagbiasYmagbiasZ]

где

  • q 0, q 1, q 2, q 3 – Части кватерниона ориентации. Кватернион ориентации представляет вращение кадра от текущей ориентации платформы до локальной системы координат 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 – Сместите в показаниях магнитометра.

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

xk|k1=f(x^k1|k1,uk)

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

xk|k1=[q0q1(ΔθXΔθbiasX2)q2(ΔθYΔθbiasY2)q3(ΔθZΔθbiasZ2)q1+q0(ΔθXΔθbiasX2)q3(ΔθYΔθbiasY2)+q2(ΔθZΔθbiasZ2)q2+q3(ΔθXΔθbiasX2)+q0(ΔθYΔθbiasY2)q1(ΔθZΔθbiasZ2)q3q2(ΔθXΔθbiasX2)+q1(ΔθYΔθbiasY2)+q0(ΔθZΔθbiasZ2)positionN+(Δt)(νN)positionE+(Δt)(νE)positionD+(Δt)(νD)νN+(Δt)(gN)+(ΔνXΔνbiasX)(q02+q12q22q32)2(ΔνYΔνbiasY)(q0q3q1q2)+2(ΔνZΔνbiasZ)(q0q2+q1q3)νE+(Δt)(gE)+(ΔνYΔνbiasY)(q02q12+q22q32)+2(ΔνXΔνbiasX)(q0q3+q1q2)2(ΔνZΔνbiasZ)(q0q1q2q3)νD+(Δt)(gD)+(ΔνZΔνbiasZ)(q02q12q22+q32)2(ΔνXΔνbiasX)(q0q2q1q3)+2(ΔνYΔνbiasY)(q0q2+q1q3)ΔθbiasXΔθbiasYΔθbiasZΔνbiasXΔνbiasYΔνbiasZgeomagneticFieldVectorNgeomagneticFieldVectorEgeomagneticFieldVectorDmagbiasXmagbiasYmagbiasZ]

где

  • ΔθX, ΔθY, ΔθZ – Интегрированное чтение гироскопа.

  • ΔνX, ΔνY, ΔνZ – Интегрированные показания акселерометра.

  • Δt – Шаг расчета IMU.

  • g N, g E, g D – Постоянный вектор силы тяжести в кадре NED.

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

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

Введенный в R2018b

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