insfilterErrorState

Оценка положения по данным БИНС, GPS и монокулярной визуальной одометрии (MVO)

Описание

The insfilterErrorState реализует комплексирование датчиков данных БИНС, GPS и монокулярной визуальной одометрии (MVO) для оценки положения в опорной системе координат NED (или ENU). Для отслеживания ориентации фильтр использует вектор состояния с 17 элементами quaternion, скорость, положение, смещения датчика БИНС и масштабный коэффициент MVO. The insfilterErrorState объект использует фильтр Калмана состояния ошибки, чтобы оценить эти величины.

Создание

Синтаксис

filter = insfilterErrorState

filter = insfilterErrorState('ReferenceFrame',RF)

filter = insfilterErrorState(___,Name,Value)

Описание

пример

filter = insfilterErrorState создает insfilterErrorState объект со значениями свойств по умолчанию.

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

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

Свойства

расширить все

`IMUSampleRate` - Частота дискретизации БИНС (Гц)
`100` (по умолчанию) | положительная скалярная величина

Частота дискретизации инерциального измерительного блока (IMU) в Гц, заданная как положительная скалярная величина.

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

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

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

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

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

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

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

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

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

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

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

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

`AccelerometerNoise` - Мультипликативное отклонение шума процесса от акселерометра ((м/с²)²)
`[1e-4 1e-4 1e-4]` (по умолчанию) | скаляром | 3-элементным вектором-строкой

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

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

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

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

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

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

`State` - Вектор состояния фильтра Калмана
`[1;zeros(15,1);1]` (по умолчанию) | вектор-столбец с 17 элементами

Вектор состояния расширенного фильтра Калмана, заданный как 17-элементный вектор-столбец. Значения состояния представляют:

Государство	Модули	Индекс
Ориентация (кватернионные части)	Н/Д	1:4
Положение (NED или ENU)	m	5:7
Скорость (NED или ENU)	м/с	8:10
Смещение гироскопа (XYZ)	рад/с	11:13
Смещение акселерометра (XYZ)	м/с²	14:16
Шкала визуальной одометрии (XYZ)	Н/Д	17

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

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

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

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

Ковариация государства	Индекс строка/столбец
α Вектор Вращения (XYZ)	1:3
β Положение (NED или ENU)	4:6
β Скорость (NED или ENU)	7:9
β Смещение гироскопа (XYZ)	10:12
β Смещение акселерометра (XYZ)	13:15
β Шкала визуальной одометрии (XYZ)	16

Обратите внимание, что поскольку это фильтр Калмана с состоянием ошибки, он отслеживает ошибки в состояниях. .rпредставляет ошибку в соответствующем состоянии.

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

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

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

Примеры

свернуть все

Оценка положения наземного транспортного средства

Открыть Live Script

Загрузка записанных данных наземного транспортного средства по круговой траектории. The .mat файл содержит измерения БИНС и GPS-датчика и ориентацию и положение основной истины.

load('loggedGroundVehicleCircle.mat', ...
    'imuFs','localOrigin', ...
    'initialStateCovariance', ...
    'accelData','gyroData', ...
    'gpsFs','gpsLLA','Rpos','gpsVel','Rvel', ...
    'trueOrient','truePos');

Создайте фильтр INS, чтобы сплавить данные IMU и GPS с помощью фильтра Калмана с ошибкой.

initialState = [compact(trueOrient(1)),truePos(1,:),-6.8e-3,2.5002,0,zeros(1,6),1].';
filt = insfilterErrorState;
filt.IMUSampleRate = imuFs;
filt.ReferenceLocation = localOrigin;
filt.State = initialState;
filt.StateCovariance = initialStateCovariance;

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

numIMUSamples = size(accelData,1);
estOrient = ones(numIMUSamples,1,'quaternion');
estPos = zeros(numIMUSamples,3);

gpsIdx = 1;

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

for idx = 1:numIMUSamples

    % Use predict to estimate the filter state based on the accelData and
    % gyroData arrays.
    predict(filt,accelData(idx,:),gyroData(idx,:));
    
    % GPS data is collected at a lower sample rate than IMU data. Fuse GPS
    % data at the lower rate.
    if mod(idx, imuFs / gpsFs) == 0
        % Correct the filter states based on the GPS data.
        fusegps(filt,gpsLLA(gpsIdx,:),Rpos,gpsVel(gpsIdx,:),Rvel);
        gpsIdx = gpsIdx + 1;
    end
    
    % Log the current pose estimate
    [estPos(idx,:), estOrient(idx,:)] = pose(filt);
end

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

pErr = truePos - estPos;
qErr = rad2deg(dist(estOrient,trueOrient));

pRMS = sqrt(mean(pErr.^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.40, Y: 0.24, Z: 0.05 (meters)

fprintf('Quaternion Distance RMS Error\n');

Quaternion Distance RMS Error

fprintf('\t%.2f (degrees)\n\n',qRMS);

	0.30 (degrees)

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

plot(truePos(:,1),truePos(:,2),estPos(:,1),estPos(:,2),'r:','LineWidth',2)
grid on
axis square
xlabel('N (m)')
ylabel('E (m)')
legend('Ground Truth','Estimation')

Figure contains an axes. The axes contains 2 objects of type line. These objects represent Ground Truth, Estimation.

Алгоритмы

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

insfilterErrorState использует 17-осевое состояние ошибки структуры фильтра Калмана, чтобы оценить положение в опорной системе координат NED. Состояние определяется как:

$x = [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \\ 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} \\ v_{N} \\ v_{E} \\ v_{D} \\ 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} \\ a c c e l b i a s_{X} \\ a c c e l b i a s_{Y} \\ \begin{array}{l} a c c e l b i a s_{Z} \\ \begin{matrix} s c a l e F a c t o r \end{matrix} \end{array} \end{matrix}]$

где

q 0, _q 1, q 2_, q 3 -- Части ориентационного кватерниона. Кватернион ориентации представляет поворот системы координат от текущей ориентации платформы к локальной системе координат NED.
position N, position E, position D -- Положение платформы в локальной системе координат NED.
gyrobias X, gyrobias Y gyrobias Z -- Смещение в считывании гироскопа.
accelbias X, accelbias Y accelbias Z -- Смещение в показаниях акселерометра.
scaleFactor - Масштабный коэффициент оценки положения.

Учитывая обычную формулировку функции перехода состояния,

$x_{k | k - 1} = f ({\hat{x}}_{k - 1 | k - 1})$

прогнозируемая оценка состояния:

$x_{k | k - 1} = [\begin{matrix} q_{0} + Δ t * q_{1} (g y r o b i a s_{X} / 2 - g y r o_{X} / 2) + Δ t * q_{2} * (g y r o b i a s_{Y} / 2 - g y r o_{Y} / 2) + Δ t * q_{3} * (g y r o b i a s_{Z} / 2 - g y r o_{Z} / 2) \\ q_{1} - Δ t * q_{0} (g y r o b i a s_{X} / 2 - g y r o_{X} / 2) + Δ t * q_{3} * (g y r o b i a s_{Y} / 2 - g y r o_{Y} / 2) - Δ t * q_{2} * (g y r o b i a s_{Z} / 2 - g y r o_{Z} / 2) \\ q_{2} - Δ t * q_{3} (g y r o b i a s_{X} / 2 - g y r o_{X} / 2) - Δ t * q_{0} * (g y r o b i a s_{Y} / 2 - g y r o_{Y} / 2) + Δ t * q_{1} * (g y r o b i a s_{Z} / 2 - g y r o_{Z} / 2) \\ q_{3} + Δ t * q_{2} (g y r o b i a s_{X} / 2 - g y r o_{X} / 2) - Δ t * q_{1} * (g y r o b i a s_{Y} / 2 - g y r o_{Y} / 2) - Δ t * q_{0} * (g y r o b i a s_{Z} / 2 - g y r o_{Z} / 2) \\ p o s i t i o n_{N} + Δ t * v_{N} \\ p o s i t i o n_{E} + Δ t * v_{E} \\ p o s i t i o n_{D} + Δ t * v_{D} \\ v_{N} - Δ t * {\begin{cases} q_{0} * (q_{0} * (a c c e l b i a s_{X} - a c c e l_{X}) - q_{3} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{2} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + g_{N} + \\ q_{2} * (q_{1} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{2} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + \\ q_{1} * (q_{1} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{2} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{3} * (a c c e l b i a s_{Z} - a c c e l_{Z})) - \\ q_{3} * (q_{3} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{1} * (a c c e l b i a s_{Z} - a c c e l_{Z})) \end{cases}} \\ v_{E} - Δ t * {\begin{cases} q_{0} * (q_{3} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{1} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + g_{E} - \\ q_{1} * (q_{1} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{2} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + \\ q_{2} * (q_{1} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{2} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{3} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + \\ q_{3} * (q_{0} * (a c c e l b i a s_{X} - a c c e l_{X}) - q_{3} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{2} * (a c c e l b i a s_{Z} - a c c e l_{Z})) \end{cases}} \\ v_{D} - Δ t * {\begin{cases} q_{0} * (q_{1} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{2} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Z} - a c c e l_{Z})) + g_{D} + \\ q_{1} * (q_{3} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{0} * (a c c e l b i a s_{Y} - a c c e l_{Y}) - q_{1} * (a c c e l b i a s_{Z} - a c c e l_{Z})) - \\ q_{2} * (q_{0} * (a c c e l b i a s_{X} - a c c e l_{X}) - q_{3} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{2} * (a c c e l b i a s_{Z} - a c c e l_{Z})) - \\ q_{3} * (q_{1} * (a c c e l b i a s_{X} - a c c e l_{X}) + q_{2} * (a c c e l b i a s_{Y} - a c c e l_{Y}) + q_{3} * (a c c e l b i a s_{Z} - a c c e l_{Z})) \end{cases}} \\ 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} \\ 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} \\ s c a l e F a c t o r \end{matrix}]$

где

И t -- шаг расчета БИНС.
g N, _g E, g D - Вектор постоянной гравитации в системе координат.

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

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

См. также

insfilterAsync | insfilterMARG | insfilterNonholonomic

Введенный в R2019a

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

insfilterErrorState

Описание

Создание

Синтаксис

Описание

Свойства

`IMUSampleRate` - Частота дискретизации БИНС (Гц)
`100` (по умолчанию) | положительная скалярная величина

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

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

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

`AccelerometerNoise` - Мультипликативное отклонение шума процесса от акселерометра ((м/с²)²)
`[1e-4 1e-4 1e-4]` (по умолчанию) | скаляром | 3-элементным вектором-строкой

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

`State` - Вектор состояния фильтра Калмана
`[1;zeros(15,1);1]` (по умолчанию) | вектор-столбец с 17 элементами

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

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

Примеры

Оценка положения наземного транспортного средства

Алгоритмы

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

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

См. также

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

Поддержка

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

insfilterErrorState

Описание

Создание

Синтаксис

Описание

Свойства

IMUSampleRate - Частота дискретизации БИНС (Гц) 100 (по умолчанию) | положительная скалярная величина

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

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

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

AccelerometerNoise - Мультипликативное отклонение шума процесса от акселерометра ((м/с2)2) [1e-4 1e-4 1e-4] (по умолчанию) | скаляром | 3-элементным вектором-строкой

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

State - Вектор состояния фильтра Калмана [1;zeros(15,1);1] (по умолчанию) | вектор-столбец с 17 элементами

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

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

Примеры

Оценка положения наземного транспортного средства

Алгоритмы

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

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

См. также

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

Поддержка

`IMUSampleRate` - Частота дискретизации БИНС (Гц)
`100` (по умолчанию) | положительная скалярная величина

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

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

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

`AccelerometerNoise` - Мультипликативное отклонение шума процесса от акселерометра ((м/с²)²)
`[1e-4 1e-4 1e-4]` (по умолчанию) | скаляром | 3-элементным вектором-строкой

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

`State` - Вектор состояния фильтра Калмана
`[1;zeros(15,1);1]` (по умолчанию) | вектор-столбец с 17 элементами

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

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