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.

Примеры

свернуть все

Открыть 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. 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, заданная как скаляр в m2
GPSVelocityNoiseОтклонение шума скорости GPS, заданная как скаляр в (м/с)2

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Строение тюнера, заданный как tunerconfig объект.

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

свернуть все

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

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

Ссылки

[1] Abbeel, P., Coates, A., Montemerlo, M., Ng, A.Y. and Thrun, S. Discriminative Training of Фильтры Калмана. In Robotics: Science and systems, Vol. 2, pp. 1, 2005.

См. также

|

Введенный в R2021a

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

Поддержка