мелодия

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

Синтаксис

tunedMeasureNoise = tune (фильтр, measureNoise, sensorData, groundTruth)

tunedMeasureNoise = tune (___, конфигурация)

Описание

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

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

Примеры

свернуть все

Мелодия `insfilterAsync` для оптимизации оценки позы

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

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

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)');

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

свернуть все

`filter` - Объект фильтра
`infilterAsync` объект

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

`measureNoise` - Измерительный шум
структура

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

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

`sensorData` - Данные датчика
`duration`

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

Time - время получения данных, указанное как скаляр в секундах.
Accelerometer - Данные акселерометра, заданные как вектор скаляров в ^м2/с 1 на 3.
Gyroscope - данные гироскопа, заданные как вектор скаляров 1 на 3 в рад/с.
Magnetometer - Данные магнитометра, заданные как вектор скаляров в мкТ 1 на 3.
GPSPosition - данные местоположения GPS, заданные как вектор скаляров в метрах 1 на 3.
GPSVelocity - данные о скорости GPS, определяемые как вектор скаляров в м/с.

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

`groundTruth` - Данные о истинности основания
`duration`

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

Orientation - Ориентация от навигационной рамки к рамке кузова, указанной как quaternion или матрицу вращения 3 на 3.
AngularVelocity - Угловая скорость в кадре тела, заданная как вектор скаляров 1 на 3 в рад/с.
Position - Положение в навигационном кадре, определяемое как вектор скаляров в метрах 1 на 3.
Velocity - скорость в навигационном кадре, заданная как вектор скаляров в м/с.
Acceleration - Ускорение в навигационном кадре, определяемое как вектор скаляров 1 на 3 в ^м2/с.
AccelerometerBias - Смещение дельта-угла акселерометра в корпусе, определяемое как вектор скаляров 1 на 3 в ^м2/с.
GyroscopeBias - Смещение дельта-угла гироскопа в кадре тела, определяемое как вектор скаляров 1 на 3 в рад/с.
GeomagneticFieldVector - вектор геомагнитного поля в навигационном кадре, определяемый как вектор скаляров 1 на 3.
MagnetometerBias - Смещение магнитометра в корпусе, определяемое как вектор скаляров в мкТ 1 на 3.

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

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

`config` - Настройка тюнера
`tunerconfig` объект

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

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

свернуть все

`tunedMeasureNoise` - Настроенный измерительный шум
структура

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

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

Ссылки

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

См. также

tunerconfig | tunernoise

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

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

мелодия

Синтаксис

Описание

Примеры

Мелодия `insfilterAsync` для оптимизации оценки позы

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

`filter` - Объект фильтра
`infilterAsync` объект

`measureNoise` - Измерительный шум
структура

`sensorData` - Данные датчика
`duration`

`groundTruth` - Данные о истинности основания
`duration`

`config` - Настройка тюнера
`tunerconfig` объект

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

`tunedMeasureNoise` - Настроенный измерительный шум
структура

Ссылки

См. также

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

Поддержка

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

мелодия

Синтаксис

Описание

Примеры

Мелодия insfilterAsync для оптимизации оценки позы

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

filter - Объект фильтра infilterAsync объект

measureNoise - Измерительный шум структура

sensorData - Данные датчика duration

groundTruth - Данные о истинности основания duration

config - Настройка тюнера tunerconfig объект

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

tunedMeasureNoise - Настроенный измерительный шум структура

Ссылки

См. также

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

Поддержка

Мелодия `insfilterAsync` для оптимизации оценки позы

`filter` - Объект фильтра
`infilterAsync` объект

`measureNoise` - Измерительный шум
структура

`sensorData` - Данные датчика
`duration`

`groundTruth` - Данные о истинности основания
`duration`

`config` - Настройка тюнера
`tunerconfig` объект

`tunedMeasureNoise` - Настроенный измерительный шум
структура