Exponenta Banner
exponenta event banner

мелодия

Мелодия insfilterMARG параметры для уменьшения ошибки оценки

свернуть все на странице

Синтаксис

tunedMeasureNoise = tune (фильтр, measureNoise, sensorData, groundTruth)
tunedMeasureNoise = tune (___, конфигурация)

Описание

tunedMeasureNoise = tune(filter,measureNoise,sensorData,groundTruth) корректирует свойства insfilterMARG объект фильтра, filterи шумы измерения для уменьшения ошибки оценки состояния среднеквадратичного (среднеквадратичного) между конденсированными данными датчика и истинностью земли. Функция также возвращает настроенный шум измерения, tunedMeasureNoise. Функция использует значения свойств в фильтре и помехи измерения, предусмотренные в measureNoise структура в качестве начальной оценки для алгоритма оптимизации.

пример

tunedMeasureNoise = tune(___,config) определяет конфигурацию настройки на основе tunerconfig объект, config.

Примеры

свернуть все

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

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

load('insfilterMARGTuneData.mat');

Создайте таблицы для данных датчика и истинных данных.

sensorData = table(Accelerometer, Gyroscope, ...
    Magnetometer, GPSPosition, GPSVelocity);
groundTruth = table(Orientation, Position);

Создание insfilterMARG объект фильтра, имеющий несколько свойств шума.

filter = insfilterMARG('State',initialState,...
        'StateCovariance',initialStateCovariance,...
        'AccelerometerBiasNoise',1e-7,...
        'GyroscopeBiasNoise',1e-7,...
        'MagnetometerBiasNoise',1e-7,...
        'GeomagneticVectorNoise',1e-7);

Создайте объект конфигурации тюнера для фильтра. Установите максимум итераций равным восьми. Также задайте настраиваемые параметры.

cfg = tunerconfig('insfilterMARG', 'MaxIterations', 8);
cfg.TunableParameters = setdiff(cfg.TunableParameters, ...
    {'GeomagneticFieldVector', 'AccelerometerBiasNoise', ...
    'GyroscopeBiasNoise', 'MagnetometerBiasNoise'});

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

measNoise = tunernoise('insfilterMARG')
measNoise = struct with fields:
    MagnetometerNoise: 1
     GPSPositionNoise: 1
     GPSVelocityNoise: 1

Настройте фильтр и получите настроенные параметры.

tunedParams = tune(filter, measNoise, sensorData, ...
        groundTruth, cfg);
    Iteration    Parameter                 Metric
    _________    _________                 ______
    1            AccelerometerNoise        2.5701
    1            GPSPositionNoise          2.5446
    1            GPSVelocityNoise          2.5279
    1            GeomagneticVectorNoise    2.5268
    1            GyroscopeNoise            2.5268
    1            MagnetometerNoise         2.5204
    2            AccelerometerNoise        2.5203
    2            GPSPositionNoise          2.4908
    2            GPSVelocityNoise          2.4695
    2            GeomagneticVectorNoise    2.4684
    2            GyroscopeNoise            2.4684
    2            MagnetometerNoise         2.4615
    3            AccelerometerNoise        2.4615
    3            GPSPositionNoise          2.4265
    3            GPSVelocityNoise          2.4000
    3            GeomagneticVectorNoise    2.3988
    3            GyroscopeNoise            2.3988
    3            MagnetometerNoise         2.3911
    4            AccelerometerNoise        2.3911
    4            GPSPositionNoise          2.3500
    4            GPSVelocityNoise          2.3164
    4            GeomagneticVectorNoise    2.3153
    4            GyroscopeNoise            2.3153
    4            MagnetometerNoise         2.3068
    5            AccelerometerNoise        2.3068
    5            GPSPositionNoise          2.2587
    5            GPSVelocityNoise          2.2166
    5            GeomagneticVectorNoise    2.2154
    5            GyroscopeNoise            2.2154
    5            MagnetometerNoise         2.2063
    6            AccelerometerNoise        2.2063
    6            GPSPositionNoise          2.1505
    6            GPSVelocityNoise          2.0981
    6            GeomagneticVectorNoise    2.0971
    6            GyroscopeNoise            2.0971
    6            MagnetometerNoise         2.0875
    7            AccelerometerNoise        2.0874
    7            GPSPositionNoise          2.0240
    7            GPSVelocityNoise          1.9601
    7            GeomagneticVectorNoise    1.9594
    7            GyroscopeNoise            1.9594
    7            MagnetometerNoise         1.9499
    8            AccelerometerNoise        1.9499
    8            GPSPositionNoise          1.8802
    8            GPSVelocityNoise          1.8035
    8            GeomagneticVectorNoise    1.8032
    8            GyroscopeNoise            1.8032
    8            MagnetometerNoise         1.7959

Плавите данные датчика с помощью настроенного фильтра.

N = size(sensorData,1);
qEstTuned = quaternion.zeros(N,1);
posEstTuned = zeros(N,3);
for ii=1:N
    predict(filter,Accelerometer(ii,:),Gyroscope(ii,:));
    if all(~isnan(Magnetometer(ii,1)))
        fusemag(filter,Magnetometer(ii,:),...
            tunedParams.MagnetometerNoise);
    end
    if all(~isnan(GPSPosition(ii,1)))
        fusegps(filter,GPSPosition(ii,:),...
            tunedParams.GPSPositionNoise,GPSVelocity(ii,:),...
            tunedParams.GPSVelocityNoise);
    end
    [posEstTuned(ii,:),qEstTuned(ii,:)] = pose(filter);
end

Вычислите ошибки RMS. 

orientationErrorTuned = rad2deg(dist(qEstTuned,Orientation));
rmsOrientationErrorTuned = sqrt(mean(orientationErrorTuned.^2))
rmsOrientationErrorTuned = 0.8580

positionErrorTuned = sqrt(sum((posEstTuned - Position).^2,2));
rmsPositionErrorTuned = sqrt(mean(positionErrorTuned.^2))
rmsPositionErrorTuned = 1.7946

Визуализация результатов.

figure();
t = (0:N-1)./filter.IMUSampleRate;
subplot(2,1,1)
plot(t,positionErrorTuned,'b');
title("Tuned insfilterMARG" + newline + ...
    "Euclidean Distance Position Error")
xlabel('Time (s)');
ylabel('Position Error (meters)')
subplot(2,1,2)
plot(t, orientationErrorTuned,'b');
title("Orientation Error")
xlabel('Time (s)');
ylabel('Orientation Error (degrees)');

Figure contains 2 axes. Axes 1 with title Tuned insfilterMARG Euclidean Distance Position Error contains an object of type line. Axes 2 with title Orientation Error contains an object of type line.

Входные аргументы

свернуть все

Объект фильтра, указанный как insfilterMARG объект.

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

Имя поляОписание
MagnetometerNoiseДисперсия шума магнитометра, заданная как скаляр в (мкТ) 2
GPSPositionNoiseДисперсия позиционного шума GPS, заданная как скаляр в м2
GPSVelocityNoiseДисперсия шума скорости GPS, заданная как скаляр в (м/с) 2

Данные датчика, указанные как table. В каждой строке данные датчика указываются следующим образом:

  • Accelerometer - Данные акселерометра, заданные как вектор скаляров в м2/с 1 на 3.

  • Gyroscope - данные гироскопа, заданные как вектор скаляров 1 на 3 в рад/с.

  • Magnetometer - Данные магнитометра, заданные как вектор скаляров в мкТ 1 на 3.

  • GPSPosition - данные местоположения GPS, заданные как вектор скаляров в метрах 1 на 3.

  • GPSVelocity - данные о скорости GPS, определяемые как вектор скаляров в м/с.

Если GPS-датчик не производит полные измерения, укажите соответствующую запись для GPSPosition и/или GPSVelocity как NaN. Если установить Cost свойство входа конфигурации тюнера, configКому Custom, то можно использовать другие типы данных для sensorData ввод в зависимости от вашего выбора.

Данные истинности земли, указанные как table. В каждой строке таблица может дополнительно содержать любую из следующих переменных:

  • Orientation - Ориентация от навигационной рамки к рамке кузова, указанной как quaternion или матрицу вращения 3 на 3.

  • Position - Положение в навигационном кадре, определяемое как вектор скаляров в метрах 1 на 3.

  • Velocity - скорость в навигационном кадре, заданная как вектор скаляров в м/с.

  • DeltaAngleBias - Смещение дельта-угла, определяемое как вектор скаляров в радианах 1 на 3.

  • DeltaVelocityBias - Смещение дельта-скорости, определяемое как вектор скаляров в м/с.

  • GeomagneticFieldVector - вектор геомагнитного поля в навигационном кадре, определяемый как вектор скаляров 1 на 3.

  • MagnetometerBias - Смещение магнитометра в корпусе, определяемое как вектор скаляров в мкТ 1 на 3.

Функция обрабатывает каждую строку sensorData и groundTruth таблицы последовательно для вычисления оценки состояния и среднеквадратичной ошибки на основании истинности. Переменные состояния, отсутствующие в groundTruth для сравнения входные данные игнорируются. sensorData и groundTruth таблицы должны иметь одинаковое количество строк.

Если установить Cost свойство входа конфигурации тюнера, configКому Custom, то можно использовать другие типы данных для groundTruth ввод в зависимости от вашего выбора.

Конфигурация тюнера, указанная как tunerconfig объект.

Выходные аргументы

свернуть все

Настроенный шум измерения, возвращаемый в виде структуры. Структура содержит эти поля.

Имя поляОписание
MagnetometerNoiseДисперсия шума магнитометра, заданная как скаляр в (мкТ) 2
GPSPositionNoiseДисперсия позиционного шума GPS, заданная как скаляр в м2
GPSVelocityNoiseДисперсия шума скорости GPS, заданная как скаляр в (м/с) 2

Ссылки

[1] Аббиль, П., Коутс, А., Монтемерло, М., Нг, А. Я. и Трюн, С. Дискриминативная подготовка фильтров Калмана. В робототехнике: наука и системы, том 2, стр. 1, 2005.

См. также

|

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

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

Поддержка