tune

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

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

Синтаксис

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

Описание

пример

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

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

Примеры

свернуть все

Скрипт Open Live Script

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

load('insfilterAsyncTuneData.mat');

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

sensorData = timetable(Accelerometer, Gyroscope, ...
    Magnetometer, GPSPosition, GPSVelocity, 'SampleRate', 100);
groundTruth = timetable(Orientation, Position, ...
    'SampleRate', 100);

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

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

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

config = tunerconfig('insfilterAsync','MaxIterations',8);
config.TunableParameters = setdiff(config.TunableParameters, ...
    {'GeomagneticVectorNoise', 'AccelerometerBiasNoise', ...
    'GyroscopeBiasNoise', 'MagnetometerBiasNoise'});
config.TunableParameters
ans = 1×10 string
    "AccelerationNoise"    "AccelerometerNoise"    "AngularVelocityNoise"    "GPSPositionNoise"    "GPSVelocityNoise"    "GyroscopeNoise"    "MagnetometerNoise"    "PositionNoise"    "QuaternionNoise"    "VelocityNoise"

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

measNoise = tunernoise('insfilterAsync')
measNoise = struct with fields:
    AccelerometerNoise: 1
        GyroscopeNoise: 1
     MagnetometerNoise: 1
      GPSPositionNoise: 1
      GPSVelocityNoise: 1

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

tunedParams = tune(filter,measNoise,sensorData,groundTruth,config);
    Iteration    Parameter               Metric
    _________    _________               ______
    1            AccelerationNoise       2.1345
    1            AccelerometerNoise      2.1264
    1            AngularVelocityNoise    1.9659
    1            GPSPositionNoise        1.9341
    1            GPSVelocityNoise        1.8420
    1            GyroscopeNoise          1.7589
    1            MagnetometerNoise       1.7362
    1            PositionNoise           1.7362
    1            QuaternionNoise         1.7218
    1            VelocityNoise           1.7218
    2            AccelerationNoise       1.7190
    2            AccelerometerNoise      1.7170
    2            AngularVelocityNoise    1.6045
    2            GPSPositionNoise        1.5948
    2            GPSVelocityNoise        1.5323
    2            GyroscopeNoise          1.4803
    2            MagnetometerNoise       1.4703
    2            PositionNoise           1.4703
    2            QuaternionNoise         1.4632
    2            VelocityNoise           1.4632
    3            AccelerationNoise       1.4596
    3            AccelerometerNoise      1.4548
    3            AngularVelocityNoise    1.3923
    3            GPSPositionNoise        1.3810
    3            GPSVelocityNoise        1.3322
    3            GyroscopeNoise          1.2998
    3            MagnetometerNoise       1.2976
    3            PositionNoise           1.2976
    3            QuaternionNoise         1.2943
    3            VelocityNoise           1.2943
    4            AccelerationNoise       1.2906
    4            AccelerometerNoise      1.2836
    4            AngularVelocityNoise    1.2491
    4            GPSPositionNoise        1.2258
    4            GPSVelocityNoise        1.1880
    4            GyroscopeNoise          1.1701
    4            MagnetometerNoise       1.1698
    4            PositionNoise           1.1698
    4            QuaternionNoise         1.1688
    4            VelocityNoise           1.1688
    5            AccelerationNoise       1.1650
    5            AccelerometerNoise      1.1569
    5            AngularVelocityNoise    1.1454
    5            GPSPositionNoise        1.1100
    5            GPSVelocityNoise        1.0778
    5            GyroscopeNoise          1.0709
    5            MagnetometerNoise       1.0675
    5            PositionNoise           1.0675
    5            QuaternionNoise         1.0669
    5            VelocityNoise           1.0669
    6            AccelerationNoise       1.0634
    6            AccelerometerNoise      1.0549
    6            AngularVelocityNoise    1.0549
    6            GPSPositionNoise        1.0180
    6            GPSVelocityNoise        0.9866
    6            GyroscopeNoise          0.9810
    6            MagnetometerNoise       0.9775
    6            PositionNoise           0.9775
    6            QuaternionNoise         0.9768
    6            VelocityNoise           0.9768
    7            AccelerationNoise       0.9735
    7            AccelerometerNoise      0.9652
    7            AngularVelocityNoise    0.9652
    7            GPSPositionNoise        0.9283
    7            GPSVelocityNoise        0.8997
    7            GyroscopeNoise          0.8947
    7            MagnetometerNoise       0.8920
    7            PositionNoise           0.8920
    7            QuaternionNoise         0.8912
    7            VelocityNoise           0.8912
    8            AccelerationNoise       0.8885
    8            AccelerometerNoise      0.8811
    8            AngularVelocityNoise    0.8807
    8            GPSPositionNoise        0.8479
    8            GPSVelocityNoise        0.8238
    8            GyroscopeNoise          0.8165
    8            MagnetometerNoise       0.8165
    8            PositionNoise           0.8165
    8            QuaternionNoise         0.8159
    8            VelocityNoise           0.8159

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

dt = seconds(diff(groundTruth.Time));
N = size(sensorData,1);
qEst = quaternion.zeros(N,1);
posEst = zeros(N,3);
% Iterate the filter for prediction and correction using sensor data.
for ii=1:N
    if ii ~= 1
        predict(filter, dt(ii-1));
    end
    if all(~isnan(Accelerometer(ii,:)))
        fuseaccel(filter,Accelerometer(ii,:), ...
            tunedParams.AccelerometerNoise);
    end
    if all(~isnan(Gyroscope(ii,:)))
        fusegyro(filter, Gyroscope(ii,:), ...
            tunedParams.GyroscopeNoise);
    end
    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
    [posEst(ii,:), qEst(ii,:)] = pose(filter);
end

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

orientationError = rad2deg(dist(qEst, Orientation));
rmsorientationError = sqrt(mean(orientationError.^2))
rmsorientationError = 2.7801

positionError = sqrt(sum((posEst - Position).^2, 2));
rmspositionError = sqrt(mean( positionError.^2))
rmspositionError = 0.5966

Визуализируйте результаты.

figure();
t = (0:N-1)./ groundTruth.Properties.SampleRate;
subplot(2,1,1)
plot(t, positionError, 'b');
title("Tuned insfilterAsync" + newline + "Euclidean Distance Position Error")
xlabel('Time (s)');
ylabel('Position Error (meters)')
subplot(2,1,2)
plot(t, orientationError, 'b');
title("Orientation Error")
xlabel('Time (s)');
ylabel('Orientation Error (degrees)');

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

свернуть все

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

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

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

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

  • Time — Время, в которое данные получены в виде скаляра в секундах.

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

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

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

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

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

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

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

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

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

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

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

  • Acceleration — Ускорение в навигации структурирует в виде 1 3 вектор из скаляров в m2/s.

  • AccelerometerBias — Угол дельты акселерометра смещает в системе координат тела в виде 1 3 вектор из скаляров в m2/s.

  • GyroscopeBias — Угол дельты гироскопа смещает в системе координат тела в виде 1 3 вектор из скаляров в rad/s.

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

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

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

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

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

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

свернуть все

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

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

Ссылки

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

Введенный в R2020b

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

Поддержка

Для просмотра документации необходимо авторизоваться на сайте