insfilterAsync

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

Описание

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

Создание

Синтаксис

filter = insfilterAsync

filter = insfilterAsync('ReferenceFrame',RF)

filter = insfilterAsync(___,Name,Value)

Описание

пример

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` - Аддитивный технологический шум скорости вращения в локальной навигационной системе координат ((рад/с)²)
`[0.005 0.005 0.005]` (по умолчанию) | скаляром | трехэлементным вектором-строкой

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Примеры

свернуть все

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

Открыть Live Script

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

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. Внешний контур предсказывает фильтр вперед на один временной шаг и запирает данные акселерометра и гироскопа со скоростью дискретизации БИНС.

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 между известным истинным положением и ориентацией и выходом от асинхронного фильтра БИНС.

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-осевой непрерывно-дискретный расширенный фильтр Калмана с помощью последовательного слияния. Фильтр основывается на допущении, что отдельные измерения датчика являются некоррелированными. Фильтр использует всенаправленную модель движения и принимает постоянную скорость вращения и постоянное ускорение. Состояние определяется как:

$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 0, _q 1, q 2_, q 3 -- Части ориентационного кватерниона. Кватернион ориентации представляет поворот системы координат от текущей ориентации платформы к локальной системе координат 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} \\ 0 \\ 0 \\ 0 \\ ν_{N} \\ ν_{E} \\ ν_{D} \\ a c c e l_{N} \\ a c c e l_{E} \\ a c c e l_{D} \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \end{matrix}]$

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

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

См. также

insfilterErrorState | insfilterMARG | insfilterNonholonomic

Введенный в R2019a

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

insfilterAsync

Описание

Создание

Синтаксис

Описание

Свойства

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

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

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

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

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

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

Примеры

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

Алгоритмы

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

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

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

См. также

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

Поддержка

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

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++ с помощью Coder™ MATLAB ®

См. также

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

Поддержка

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

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

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

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

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

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

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