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.

Примеры

свернуть все

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

свернуть все

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

Имя поляОписание
AccelerometerNoiseОтклонение шума акселерометра, заданная как скаляр в (m2/ с)2
GyroscopeNoiseОтклонение шума гироскопа, заданная как скаляр в (рад/с)2
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.

См. также

|

Введенный в R2020b

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

Поддержка

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