Exponenta Banner

tune

Настройте insfilterMARG параметры, чтобы уменьшать ошибку расчета

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

Синтаксис

tunedMeasureNoise = tune(filter,measureNoise,sensorData,groundTruth)
tunedMeasureNoise = tune(___,config)

Описание

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

пример

tunedMeasureNoise = tune(___,config) задает настраивающуюся настройку на основе tunerconfig объект, config.

Примеры

свернуть все

Скрипт Open Live Script

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

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 objects. Axes object 1 with title Tuned insfilterMARG Euclidean Distance Position Error contains an object of type line. Axes object 2 with title Orientation Error contains an object of type line.

Входные параметры

свернуть все

Объект фильтра в виде insfilterMARG объект.

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

Имя поляОписание
MagnetometerNoiseОтклонение шума магнитометра в виде скаляра в (μT)2
GPSPositionNoiseОтклонение шума положения GPS в виде скаляра в m2
GPSVelocityNoiseОтклонение скоростного шума GPS в виде скаляра в (m/s)2

Данные о датчике в виде table. В каждой строке данные о датчике заданы как:

  • Accelerometer — Данные об акселерометре в виде 1 3 вектора из скаляров в m2S.

  • Gyroscope — Данные о гироскопе в виде 1 3 вектора из скаляров в rad/s.

  • Magnetometer — Данные о магнитометре в виде 1 3 вектора из скаляров в μT.

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

  • GPSVelocity — Скоростные данные о GPS в виде 1 3 вектора из скаляров в m/s.

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

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

  • Orientation — Ориентация от навигации структурирует к системе координат тела в виде quaternion или 3х3 матрица вращения.

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

  • Velocity — Скорость в навигации структурирует в виде 1 3 вектор из скаляров в m/s.

  • DeltaAngleBias — Угол Delta смещает в виде 1 3 вектор из скаляров в радианах.

  • DeltaVelocityBias — Скорость Delta смещает в виде 1 3 вектор из скаляров в m/s.

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

  • MagnetometerBias — Смещение магнитометра в системе координат тела в виде 1 3 вектора из скаляров в μT.

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

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

Настройка тюнера в виде tunerconfig объект.

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

свернуть все

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

Имя поляОписание
MagnetometerNoiseОтклонение шума магнитометра в виде скаляра в (μT)2
GPSPositionNoiseОтклонение шума положения GPS в виде скаляра в m2
GPSVelocityNoiseОтклонение скоростного шума GPS в виде скаляра в (m/s)2

Ссылки

[1] Abbeel, P., Коутс, A., Montemerlo, M., Ын, А.И. и Трун, S. Отличительное Обучение Фильтров Калмана. В Робототехнике: Наука и системы, Издание 2, стр 1, 2005.

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

|

Введенный в R2021a

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

Поддержка