insfilterAsync

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

Описание

insfilterAsync возразите cочетанию датчиков реализаций MARG и данных о GPS, чтобы оценить положение в NED (или ENU) система координат. MARG (магнитный, угловой уровень, сила тяжести) данные обычно выводятся из магнитометра, гироскопа и данных об акселерометре, соответственно. Фильтр использует вектор состояния с 28 элементами, чтобы отследить ориентацию quaternion, скорость, положение, смещения датчика MARG и геомагнитный вектор. 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` — Аддитивный шум процесса скорости вращения в локальной системе координат навигации ((rad/s)²)
[0.005 0.005 0.005] (значение по умолчанию) | скаляр | трехэлементный вектор-строка

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Состояние	Модули	Индекс
Ориентация (части кватерниона)	N/A	1:4
Скорость вращения (XYZ)	рад/с	5:7
Положение (NED или ENU)	m	8:10
Скорость (NED или ENU)	m/s	11:13
Ускорение (NED или ENU)	m/s²	14:16
Смещение акселерометра (XYZ)	m/s²	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 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`	Шумовая структура фильтра сплава

Примеры

свернуть все

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

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

Figure contains an axes object. The axes object 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 ₀, 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} \\ 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++ с помощью MATLAB® Coder™.

Введенный в R2019a

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

insfilterAsync

Описание

Создание

Синтаксис

Описание

Свойства

`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

Алгоритмы

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

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

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

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

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

Поддержка

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

insfilterAsync

Описание

Создание

Синтаксис

Описание

Свойства

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

Алгоритмы

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

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

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

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

Документация Navigation 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 матрица

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

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