insfilterAsync

Оценка позы по асинхронным данным MARG и GPS

Описание

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

Создание

Синтаксис

filter = insfilterAsync

filter = insfilterAsync («Ссылочный кадр», RF)

filter = insfilterAsync (___, имя, значение)

Описание

пример

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

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

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

Свойства

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

`VelocityNoise` - Дисперсия шума процесса аддитивной скорости в локальной навигационной системе координат ((м/с) 2)
`[1e-6 1e-6 1e-6]` (по умолчанию) | скаляр | трехэлементный вектор строки

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

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

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

`AccelerationNoise` - Дисперсия шума процесса аддитивного ускорения в локальной навигационной системе координат ((м/с2) 2)
`[50 50 50]` (по умолчанию) | скаляр | трехэлементный вектор строки

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

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

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

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

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

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

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

`AccelerometerBiasNoise` - Дисперсия аддитивного технологического шума от смещения акселерометра ((м/с2) 2)
`[1e-4 1e-4 1e-4]` (по умолчанию) | положительный скаляр | трехэлементный вектор строки

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Примеры

свернуть все

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

Открыть сценарий в реальном времени

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

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

Вычислите среднеквадратичные ошибки между известной истинной позицией и ориентацией и выходными данными асинхронного фильтра 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)')

Figure contains an axes. The axes contains 2 objects of type line.

Алгоритмы

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

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

Примечание.Следующий алгоритм применяется только к опорному кадру NED.

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

$\begin{matrix} _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{} \\ _{x=[q0q1q2q3angVelXangVelYangVelZpositionNpositionEpositionDνNνEνDaccelNaccelEaccelDaccelbiasXaccelbiasYaccelbiasZgyrobiasXgyrobiasYgyrobiasZgeomagneticFieldVectorNgeomagneticFieldVectorEgeomagneticFieldVectorDmagbiasXmagbiasYmagbiasZ} \end{matrix}]$

где

q0, q1, q2, q3 -- Части ориентации кватерниона. Кватернион ориентации представляет поворот кадра от текущей ориентации платформы к локальной системе координат NED.
angVelX, angVelY, angVelZ - угловая скорость относительно корпуса платформы.
posityN, posityE, posityD - положение платформы в локальной системе координат NED.
startN, startE, startD - скорость платформы в локальной системе координат NED.
accelN, accelE, accelD - ускорение платформы в локальной системе координат NED.
accelbiasX, accelbiasY, accelbiasZ - смещение в показаниях акселерометра.
gyrobiasX, gyrobiasY, gyrobiasZ - смещение в чтении гироскопа.
geompienStartVectorN, geompyTVectorE, geompytperVectorD - оценка вектора геомагнитного поля в опорном местоположении.
magbiasX, magbiasY, magbiasZ - смещение в показаниях магнитометра.

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

$f (x) \begin{matrix} \frac{= [-_{} (q1)_{}_{(angVelX)} - (q2)_{}_{} (angVelY) -_{} (q3)}{} \\ \frac{_{} (angVelZ) 2_{} {(q0)}_{}_{(angVelX)} -_{} (q3)_{}}{} \\ \frac{(angVelY)_{+} (q1)_{}_{} (angVelZ) 2 {(q3)}_{}_{} (angVelX) +_{}}{(q0)} \\ \frac{_{} (angVelY)_{−}_{(q1)}_{} (angVelZ) 2_{} (q1)_{}}{} \\ (angVelX) \\ -_{} \\ _{} \\ _{(q2)} \\ _{} \\ (angVelY)_{+} \\ {(q0)}_{} \\ (angVelZ) \end{matrix}$ 2000νNνEνDaccelNaccelEaccelD000000000000000]

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

Создание кода C/C + +
Создайте код C и C++ с помощью MATLAB ® Coder™

См. также

insfilterErrorState | insfilterMARG | insfilterNonholonomic

Представлен в R2019a

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

insfilterAsync

Описание

Создание

Синтаксис

Описание

Свойства

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

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

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

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

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

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

Примеры

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

Алгоритмы

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

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

Создание кода C/C + +
Создайте код C и C++ с помощью MATLAB ® Coder™

См. также

Документация по комплекту инструментов для слияния и отслеживания датчиков

Поддержка

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

insfilterAsync

Описание

Создание

Синтаксис

Описание

Свойства

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

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

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

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

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

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

Примеры

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

Алгоритмы

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

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

Создание кода C/C + + Создайте код C и C++ с помощью MATLAB ® Coder™

См. также

Документация по комплекту инструментов для слияния и отслеживания датчиков

Поддержка

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

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

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

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

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

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

Создание кода C/C + +
Создайте код C и C++ с помощью MATLAB ® Coder™