AsyncMARGGPSFuser

Позируйте от асинхронного MARG и GPS

Описание

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

Создание

Создайте AsyncMARGGPSFuser, чтобы плавить асинхронный MARG и данные о GPS с помощью insfilter:

filt = insfilter('AsyncIMU');

Свойства

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

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

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

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

`QuaternionNoise` — Аддитивное отклонение шума процесса кватерниона
`[1e-6 1e-6 1e-6 1e-6]` (значение по умолчанию) | скаляр | четырехэлементный вектор - строка

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

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

`AngularVelocityNoise` — Аддитивный угловой скоростной шум процесса в локальной системе координат NED ((rad/s) ²)
`[0.005 0.005 0.005]` (значение по умолчанию) | скаляр | трехэлементный вектор - строка

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

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

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

`PositionNoise` — Аддитивное отклонение шума процесса положения в локальной системе координат NED ^(m2)
`[1e-6 1e-6 1e-6]` (значение по умолчанию) | скаляр | трехэлементный вектор - строка

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

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

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

`VelocityNoise` — Аддитивное отклонение шума процесса скорости в локальной системе координат NED ((m/s) ²)
`[1e-6 1e-6 1e-6]` (значение по умолчанию) | скаляр | трехэлементный вектор - строка

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

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

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

`AccelerationNoise` — Аддитивное ускоряющее отклонение шума процесса в локальной системе координат NED (^(m/s2) ²)
`[50 50 50]` (значение по умолчанию) | скаляр | трехэлементный вектор - строка

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

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

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

`GyroscopeBiasNoise` — Отклонение шума аддитивного процесса от смещения гироскопа ((rad/s) ²)
`[1e-10 1e-10 1e-10]` (значение по умолчанию) | скаляр | трехэлементный вектор - строка

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

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

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

`AccelerometerBiasNoise` — Отклонение шума аддитивного процесса от смещения акселерометра (^(m/s2) ²)
`[1e-4 1e-4 1e-4]` (значение по умолчанию) | положительная скалярная величина | трехэлементный вектор - строка

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

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

`GeomagneticVectorNoise` — Отклонение шума аддитивного процесса геомагнитного вектора в локальной системе координат NED ^(μT2)
`[1e-6 1e-6 1e-6]` (значение по умолчанию) | положительная скалярная величина | трехэлементный вектор - строка

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

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

`MagnetometerBiasNoise` — Отклонение шума аддитивного процесса от смещения магнитометра ^(μT2)
`[0.1 0.1 0.1]` (значение по умолчанию) | положительная скалярная величина | трехэлементный вектор - строка

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

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

`State` — Вектор состояния расширенного Фильтра Калмана
Вектор-столбец с 28 элементами

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

Состояние	Модули	Индекс
Ориентация (части кватерниона)	Нет данных	1:4
Угловая скорость (XYZ)	rad/s	5:7
Положение (NED)	m	8:10
Скорость (NED)	m/s	11:13
Ускорение (NED)	^m/s2	14:16
Смещение акселерометра (XYZ)	^m/s2	17:19
Смещение гироскопа (XYZ)	rad/s	20:22
Геомагнитный полевой вектор (NED)	μT	23:25
Смещение магнитометра (XYZ)	μT	26:28

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

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

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

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

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

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

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

Примеры

свернуть все

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

Открыть скрипт

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

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

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

filt = insfilter('AsyncIMU');
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');
fprintf('\tX: %.2f, Y: %.2f, Z: %.2f (meters)\n\n',pRMS(1),pRMS(2),pRMS(3));

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

Position RMS Error
	X: 0.55, Y: 0.71, Z: 0.74 (meters)

Quaternion Distance RMS Error
	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)')

Алгоритмы

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

Динамическая модель, используемая в AsyncMARGGPSFuser

AsyncMARGGPSFuser реализует непрерывно-дискретный расширенный Фильтр Калмана с 28 осями с помощью последовательного сплава. Фильтр полагается при условии, что отдельные измерения датчика являются некоррелироваными. Фильтр использует всенаправленную модель движения и принимает постоянную угловую скорость и постоянное ускорение. Состояние задано как:

$x = [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \\ a n g V e l_{X} \\ a n g V e l_{Y} \\ a n g V e l_{Z} \\ 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} \\ a c c e l_{N} \\ a c c e l_{E} \\ a c c e l_{D} \\ a c c e l b i a s_{X} \\ a c c e l b i a s_{Y} \\ a c c e l b i a s_{Z} \\ g y r o b i a s_{X} \\ g y r o b i a s_{Y} \\ g y r o 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.
angVel _X, angVel _Y, angVel _Z – Угловая скорость относительно каркаса кузова платформы.
position _N, position _E, position _D – Положение платформы в локальной системе координат NED.
ν _N, ν _E, ν _D – Скорость платформы в локальной системе координат NED.
accel _N, accel _E, accel _D – Ускорение платформы в локальной системе координат NED.
accelbias _X, accelbias _Y, accelbias _Z – Сместите в чтении акселерометра.
gyrobias _X, gyrobias _Y, gyrobias _Z – Сместите в чтении гироскопа.
geomagneticFieldVector _N, geomagneticFieldVector _E, geomagneticFieldVector _D – Оценка геомагнитного полевого вектора в ссылочном местоположении.
magbias _X, magbias _Y, magbias _Z – Сместите в показаниях магнитометра.

Учитывая обычное формирование уравнения процесса, $\dot{x} = f (x) + w$ , w является шумом процесса, $\dot{x}$ производная x, и:

$f (x) = [\begin{matrix} \frac{- (q_{1}) (a n g V e l_{X}) - (q_{2}) (a n g V e l_{Y}) - (q_{3}) (a n g V e l_{Z})}{2} \\ \frac{(q_{0}) (a n g V e l_{X}) - (q_{3}) (a n g V e l_{Y}) + (q_{1}) (a n g V e l_{Z})}{2} \\ \frac{(q_{3}) (a n g V e l_{X}) + (q_{0}) (a n g V e l_{Y}) - (q_{1}) (a n g V e l_{Z})}{2} \\ \frac{(q_{1}) (a n g V e l_{X}) - (q_{2}) (a n g V e l_{Y}) + (q_{0}) (a n g V e l_{Z})}{2} \\ 000 \\ ν_{N} \\ ν_{E} \\ ν_{D} \\ a c c e l_{N} \\ a c c e l_{E} \\ a c c e l_{D} \\ 000000000000000 \end{matrix}]$

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

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

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

insfilter

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

AsyncMARGGPSFuser

Описание

Создание

Свойства

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

`QuaternionNoise` — Аддитивное отклонение шума процесса кватерниона
`[1e-6 1e-6 1e-6 1e-6]` (значение по умолчанию) | скаляр | четырехэлементный вектор - строка

`GyroscopeBiasNoise` — Отклонение шума аддитивного процесса от смещения гироскопа ((rad/s) ²)
`[1e-10 1e-10 1e-10]` (значение по умолчанию) | скаляр | трехэлементный вектор - строка

`State` — Вектор состояния расширенного Фильтра Калмана
Вектор-столбец с 28 элементами

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

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

Примеры

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

Алгоритмы

Динамическая модель, используемая в AsyncMARGGPSFuser

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

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

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

Введенный в R2019a

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

Поддержка

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

AsyncMARGGPSFuser

Описание

Создание

Свойства

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

QuaternionNoise — Аддитивное отклонение шума процесса кватерниона [1e-6 1e-6 1e-6 1e-6] (значение по умолчанию) | скаляр | четырехэлементный вектор - строка

GyroscopeBiasNoise — Отклонение шума аддитивного процесса от смещения гироскопа ((rad/s) 2) [1e-10 1e-10 1e-10] (значение по умолчанию) | скаляр | трехэлементный вектор - строка

State — Вектор состояния расширенного Фильтра Калмана Вектор-столбец с 28 элементами

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

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

Примеры

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

Алгоритмы

Динамическая модель, используемая в AsyncMARGGPSFuser

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

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

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

Введенный в R2019a

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

Поддержка

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

`QuaternionNoise` — Аддитивное отклонение шума процесса кватерниона
`[1e-6 1e-6 1e-6 1e-6]` (значение по умолчанию) | скаляр | четырехэлементный вектор - строка

`GyroscopeBiasNoise` — Отклонение шума аддитивного процесса от смещения гироскопа ((rad/s) ²)
`[1e-10 1e-10 1e-10]` (значение по умолчанию) | скаляр | трехэлементный вектор - строка

`State` — Вектор состояния расширенного Фильтра Калмана
Вектор-столбец с 28 элементами

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

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