insfilterAsync

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

Описание

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

Создание

Описание

пример

filter = insfilterAsync создает insiflterAsync возразите, чтобы плавить асинхронный MARG и данные о GPS со значениями свойств по умолчанию.

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

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

Свойства

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

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

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

Аддитивное отклонение шума процесса кватерниона в виде скалярного или четырехэлементного вектора частей кватерниона.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Отклонение шума аддитивного процесса геомагнитного вектора в μT2 в виде скалярного или трехэлементного вектора-строки из положительных вещественных чисел.

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

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

Отклонение шума аддитивного процесса от магнитометра смещает в μT2 в виде скалярного или трехэлементного вектора-строки из положительных вещественных чисел.

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

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

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

СостояниеМодулиИндекс
Ориентация (части кватерниона)Нет данных1:4
Скорость вращения (XYZ)rad/s5:7
Положение (NED или ENU)m8:10
Скорость (NED или ENU)m/s11:13
Ускорение (NED или ENU)m/s214:16
Смещение акселерометра (XYZ)m/s217:19
Смещение гироскопа (XYZ)rad/s20:22
Геомагнитный полевой вектор (NED или ENU)μT23:25
Смещение магнитометра (XYZ)μT26:28

Начальное состояние по умолчанию соответствует объекту, в покое расположенному в [0 0 0] в геодезических координатах LLA.

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

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

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

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

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

Примеры

свернуть все

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

load('uavshort.mat','refloc','initstate','imuFs', ...
    'accel','gyro','mag','lla','gpsvel', ...
    'trueOrient','truePos')

Создайте фильтр INS, чтобы плавить асинхронный MARG и данные о GPS, чтобы оценить положение.

filt = insfilterAsync;
filt.ReferenceLocation = refloc;
filt.State = [initstate(1:4);0;0;0;initstate(5:10);0;0;0;initstate(11:end)];

Задайте шумы измерения датчика. Шумы были определены из таблиц данных и экспериментирования.

Rmag  = 80;
Rvel  = 0.0464;
Racc  = 800;
Rgyro = 1e-4;
Rpos  = 34;

Предварительно выделите переменные для положения и ориентации. Выделите переменную для индексации в данные о GPS.

N = size(accel,1);
p = zeros(N,3);
q = zeros(N,1,'quaternion');

gpsIdx = 1;

Плавьте акселерометр, гироскоп, магнитометр и данные о GPS. Внешний цикл предсказывает, что фильтр передает один временной шаг и акселерометр предохранителей и данные о гироскопе на уровне частоты дискретизации IMU.

for ii = 1:N
    
    % Predict the filter forward one time step
    predict(filt,1./imuFs);
    
    % Fuse accelerometer and gyroscope readings
    fuseaccel(filt,accel(ii,:),Racc);
    fusegyro(filt,gyro(ii,:),Rgyro);
    
    % Fuse magnetometer at 1/2 the IMU rate
    if ~mod(ii, fix(imuFs/2))
        fusemag(filt,mag(ii,:),Rmag);
    end
    
    % Fuse GPS once per second
    if ~mod(ii,imuFs)
        fusegps(filt,lla(gpsIdx,:),Rpos,gpsvel(gpsIdx,:),Rvel);
        gpsIdx = gpsIdx + 1;
    end
    
    % Log the current pose estimate
    [p(ii,:),q(ii)] = pose(filt);
    
end

Вычислите ошибки RMS между известным истинным положением и ориентацией и выходом от асинхронного фильтра IMU.

posErr = truePos - p;
qErr = rad2deg(dist(trueOrient,q));

pRMS = sqrt(mean(posErr.^2));
qRMS = sqrt(mean(qErr.^2));

fprintf('Position RMS Error\n');
Position RMS Error
fprintf('\tX: %.2f, Y: %.2f, Z: %.2f (meters)\n\n',pRMS(1),pRMS(2),pRMS(3));
	X: 0.55, Y: 0.71, Z: 0.74 (meters)
fprintf('Quaternion Distance RMS Error\n');
Quaternion Distance RMS Error
fprintf('\t%.2f (degrees)\n\n', qRMS);
	4.72 (degrees)

Визуализируйте истинное положение и предполагаемое положение.

plot3(truePos(:,1),truePos(:,2),truePos(:,3),'LineWidth',2)
hold on
plot3(p(:,1),p(:,2),p(:,3),'r:','LineWidth',2)
grid on
xlabel('N (m)')
ylabel('E (m)')
zlabel('D (m)')

Алгоритмы

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

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

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

Введенный в R2019a

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