Exponenta Banner
exponenta event banner

tune

Настройка insfilterNonholonomic параметры для уменьшения ошибки расчета

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

Синтаксис

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

Описание

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

пример

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

Примеры

свернуть все

Открыть Live Script

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

load('insfilterNonholonomicTuneData.mat');

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

sensorData = table(Accelerometer, Gyroscope, ...
        GPSPosition, GPSVelocity);
groundTruth = table(Orientation, Position);

Создайте insfilterNonholonimic объект фильтра.

filter = insfilterNonholonomic('State', initialState, ...
        'StateCovariance', initialStateCovariance, ...
        'DecimationFactor', 1);

Создайте объект строения тюнера для фильтра. Установите максимальное количество итераций равным 30. 

config = tunerconfig('insfilterNonholonomic','MaxIterations',30);

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

measNoise = tunernoise('insfilterNonholonomic')
measNoise = struct with fields:
    GPSPositionNoise: 1
    GPSVelocityNoise: 1

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

tunedNoise = tune(filter,measNoise,sensorData,groundTruth,config);
    Iteration    Parameter                       Metric
    _________    _________                       ______
    1            GyroscopeNoise                  3.4877
    1            AccelerometerNoise              3.3961
    1            GyroscopeBiasNoise              3.3961
    1            GyroscopeBiasDecayFactor        3.3961
    1            AccelerometerBiasNoise          3.3961
    1            AccelerometerBiasDecayFactor    3.3961
    1            ZeroVelocityConstraintNoise     3.3935
    1            GPSPositionNoise                3.2848
    1            GPSVelocityNoise                3.2798
    2            GyroscopeNoise                  3.2641
    2            AccelerometerNoise              3.1715
    2            GyroscopeBiasNoise              3.1715
    2            GyroscopeBiasDecayFactor        2.9661
    2            AccelerometerBiasNoise          2.9661
    2            AccelerometerBiasDecayFactor    2.9661
    2            ZeroVelocityConstraintNoise     2.9617
    2            GPSPositionNoise                2.8438
    2            GPSVelocityNoise                2.8384
    3            GyroscopeNoise                  2.8373
    3            AccelerometerNoise              2.7382
    3            GyroscopeBiasNoise              2.7382
    3            GyroscopeBiasDecayFactor        2.7382
    3            AccelerometerBiasNoise          2.7382
    3            AccelerometerBiasDecayFactor    2.7382
    3            ZeroVelocityConstraintNoise     2.7335
    3            GPSPositionNoise                2.6105
    3            GPSVelocityNoise                2.6045
    4            GyroscopeNoise                  2.6023
    4            AccelerometerNoise              2.5001
    4            GyroscopeBiasNoise              2.5001
    4            GyroscopeBiasDecayFactor        2.5001
    4            AccelerometerBiasNoise          2.5001
    4            AccelerometerBiasDecayFactor    2.5001
    4            ZeroVelocityConstraintNoise     2.4953
    4            GPSPositionNoise                2.3692
    4            GPSVelocityNoise                2.3626
    5            GyroscopeNoise                  2.3595
    5            AccelerometerNoise              2.2561
    5            GyroscopeBiasNoise              2.2561
    5            GyroscopeBiasDecayFactor        2.2508
    5            AccelerometerBiasNoise          2.2508
    5            AccelerometerBiasDecayFactor    2.2508
    5            ZeroVelocityConstraintNoise     2.2469
    5            GPSPositionNoise                2.1265
    5            GPSVelocityNoise                2.1191
    6            GyroscopeNoise                  2.1148
    6            AccelerometerNoise              2.0150
    6            GyroscopeBiasNoise              2.0150
    6            GyroscopeBiasDecayFactor        2.0150
    6            AccelerometerBiasNoise          2.0150
    6            AccelerometerBiasDecayFactor    2.0150
    6            ZeroVelocityConstraintNoise     2.0116
    6            GPSPositionNoise                1.8970
    6            GPSVelocityNoise                1.8888
    7            GyroscopeNoise                  1.8847
    7            AccelerometerNoise              1.7921
    7            GyroscopeBiasNoise              1.7921
    7            GyroscopeBiasDecayFactor        1.7845
    7            AccelerometerBiasNoise          1.7845
    7            AccelerometerBiasDecayFactor    1.7845
    7            ZeroVelocityConstraintNoise     1.7815
    7            GPSPositionNoise                1.6794
    7            GPSVelocityNoise                1.6708
    8            GyroscopeNoise                  1.6679
    8            AccelerometerNoise              1.5886
    8            GyroscopeBiasNoise              1.5886
    8            GyroscopeBiasDecayFactor        1.5866
    8            AccelerometerBiasNoise          1.5866
    8            AccelerometerBiasDecayFactor    1.5866
    8            ZeroVelocityConstraintNoise     1.5850
    8            GPSPositionNoise                1.5057
    8            GPSVelocityNoise                1.4965
    9            GyroscopeNoise                  1.4950
    9            AccelerometerNoise              1.4364
    9            GyroscopeBiasNoise              1.4364
    9            GyroscopeBiasDecayFactor        1.4364
    9            AccelerometerBiasNoise          1.4364
    9            AccelerometerBiasDecayFactor    1.4364
    9            ZeroVelocityConstraintNoise     1.4355
    9            GPSPositionNoise                1.3894
    9            GPSVelocityNoise                1.3790
    10           GyroscopeNoise                  1.3773
    10           AccelerometerNoise              1.3422
    10           GyroscopeBiasNoise              1.3422
    10           GyroscopeBiasDecayFactor        1.3421
    10           AccelerometerBiasNoise          1.3421
    10           AccelerometerBiasDecayFactor    1.3421
    10           ZeroVelocityConstraintNoise     1.3399
    10           GPSPositionNoise                1.3319
    10           GPSVelocityNoise                1.3190
    11           GyroscopeNoise                  1.3159
    11           AccelerometerNoise              1.3102
    11           GyroscopeBiasNoise              1.3102
    11           GyroscopeBiasDecayFactor        1.3100
    11           AccelerometerBiasNoise          1.3100
    11           AccelerometerBiasDecayFactor    1.3100
    11           ZeroVelocityConstraintNoise     1.3069
    11           GPSPositionNoise                1.2964
    11           GPSVelocityNoise                1.2762
    12           GyroscopeNoise                  1.2740
    12           AccelerometerNoise              1.2655
    12           GyroscopeBiasNoise              1.2655
    12           GyroscopeBiasDecayFactor        1.2641
    12           AccelerometerBiasNoise          1.2641
    12           AccelerometerBiasDecayFactor    1.2641
    12           ZeroVelocityConstraintNoise     1.2631
    12           GPSPositionNoise                1.2511
    12           GPSVelocityNoise                1.2198
    13           GyroscopeNoise                  1.2184
    13           AccelerometerNoise              1.2058
    13           GyroscopeBiasNoise              1.2058
    13           GyroscopeBiasDecayFactor        1.2029
    13           AccelerometerBiasNoise          1.2029
    13           AccelerometerBiasDecayFactor    1.2029
    13           ZeroVelocityConstraintNoise     1.2029
    13           GPSPositionNoise                1.1874
    13           GPSVelocityNoise                1.1408
    14           GyroscopeNoise                  1.1403
    14           AccelerometerNoise              1.1236
    14           GyroscopeBiasNoise              1.1236
    14           GyroscopeBiasDecayFactor        1.1186
    14           AccelerometerBiasNoise          1.1186
    14           AccelerometerBiasDecayFactor    1.1186
    14           ZeroVelocityConstraintNoise     1.1183
    14           GPSPositionNoise                1.0975
    14           GPSVelocityNoise                1.0348
    15           GyroscopeNoise                  1.0347
    15           AccelerometerNoise              1.0155
    15           GyroscopeBiasNoise              1.0155
    15           GyroscopeBiasDecayFactor        1.0081
    15           AccelerometerBiasNoise          1.0081
    15           AccelerometerBiasDecayFactor    1.0081
    15           ZeroVelocityConstraintNoise     1.0076
    15           GPSPositionNoise                0.9813
    15           GPSVelocityNoise                0.9078
    16           GyroscopeNoise                  0.9074
    16           AccelerometerNoise              0.8926
    16           GyroscopeBiasNoise              0.8926
    16           GyroscopeBiasDecayFactor        0.8823
    16           AccelerometerBiasNoise          0.8823
    16           AccelerometerBiasDecayFactor    0.8823
    16           ZeroVelocityConstraintNoise     0.8815
    16           GPSPositionNoise                0.8526
    16           GPSVelocityNoise                0.7926
    17           GyroscopeNoise                  0.7920
    17           AccelerometerNoise              0.7870
    17           GyroscopeBiasNoise              0.7870
    17           GyroscopeBiasDecayFactor        0.7742
    17           AccelerometerBiasNoise          0.7742
    17           AccelerometerBiasDecayFactor    0.7742
    17           ZeroVelocityConstraintNoise     0.7730
    17           GPSPositionNoise                0.7665
    17           GPSVelocityNoise                0.7665
    18           GyroscopeNoise                  0.7662
    18           AccelerometerNoise              0.7638
    18           GyroscopeBiasNoise              0.7638
    18           GyroscopeBiasDecayFactor        0.7495
    18           AccelerometerBiasNoise          0.7495
    18           AccelerometerBiasDecayFactor    0.7495
    18           ZeroVelocityConstraintNoise     0.7482
    18           GPSPositionNoise                0.7482
    18           GPSVelocityNoise                0.7475
    19           GyroscopeNoise                  0.7474
    19           AccelerometerNoise              0.7474
    19           GyroscopeBiasNoise              0.7474
    19           GyroscopeBiasDecayFactor        0.7474
    19           AccelerometerBiasNoise          0.7474
    19           AccelerometerBiasDecayFactor    0.7474
    19           ZeroVelocityConstraintNoise     0.7453
    19           GPSPositionNoise                0.7416
    19           GPSVelocityNoise                0.7382
    20           GyroscopeNoise                  0.7378
    20           AccelerometerNoise              0.7370
    20           GyroscopeBiasNoise              0.7370
    20           GyroscopeBiasDecayFactor        0.7370
    20           AccelerometerBiasNoise          0.7370
    20           AccelerometerBiasDecayFactor    0.7370
    20           ZeroVelocityConstraintNoise     0.7345
    20           GPSPositionNoise                0.7345
    20           GPSVelocityNoise                0.7345
    21           GyroscopeNoise                  0.7334
    21           AccelerometerNoise              0.7334
    21           GyroscopeBiasNoise              0.7334
    21           GyroscopeBiasDecayFactor        0.7334
    21           AccelerometerBiasNoise          0.7334
    21           AccelerometerBiasDecayFactor    0.7334
    21           ZeroVelocityConstraintNoise     0.7306
    21           GPSPositionNoise                0.7279
    21           GPSVelocityNoise                0.7268
    22           GyroscopeNoise                  0.7248
    22           AccelerometerNoise              0.7247
    22           GyroscopeBiasNoise              0.7247
    22           GyroscopeBiasDecayFactor        0.7234
    22           AccelerometerBiasNoise          0.7234
    22           AccelerometerBiasDecayFactor    0.7234
    22           ZeroVelocityConstraintNoise     0.7207
    22           GPSPositionNoise                0.7206
    22           GPSVelocityNoise                0.7170
    23           GyroscopeNoise                  0.7138
    23           AccelerometerNoise              0.7134
    23           GyroscopeBiasNoise              0.7134
    23           GyroscopeBiasDecayFactor        0.7134
    23           AccelerometerBiasNoise          0.7134
    23           AccelerometerBiasDecayFactor    0.7134
    23           ZeroVelocityConstraintNoise     0.7122
    23           GPSPositionNoise                0.7122
    23           GPSVelocityNoise                0.7122
    24           GyroscopeNoise                  0.7081
    24           AccelerometerNoise              0.7080
    24           GyroscopeBiasNoise              0.7080
    24           GyroscopeBiasDecayFactor        0.7080
    24           AccelerometerBiasNoise          0.7080
    24           AccelerometerBiasDecayFactor    0.7080
    24           ZeroVelocityConstraintNoise     0.7080
    24           GPSPositionNoise                0.7080
    24           GPSVelocityNoise                0.7072
    25           GyroscopeNoise                  0.7009
    25           AccelerometerNoise              0.7009
    25           GyroscopeBiasNoise              0.7009
    25           GyroscopeBiasDecayFactor        0.7007
    25           AccelerometerBiasNoise          0.7007
    25           AccelerometerBiasDecayFactor    0.7007
    25           ZeroVelocityConstraintNoise     0.7005
    25           GPSPositionNoise                0.6997
    25           GPSVelocityNoise                0.6997
    26           GyroscopeNoise                  0.6912
    26           AccelerometerNoise              0.6906
    26           GyroscopeBiasNoise              0.6906
    26           GyroscopeBiasDecayFactor        0.6906
    26           AccelerometerBiasNoise          0.6906
    26           AccelerometerBiasDecayFactor    0.6906
    26           ZeroVelocityConstraintNoise     0.6896
    26           GPSPositionNoise                0.6896
    26           GPSVelocityNoise                0.6896
    27           GyroscopeNoise                  0.6840
    27           AccelerometerNoise              0.6831
    27           GyroscopeBiasNoise              0.6831
    27           GyroscopeBiasDecayFactor        0.6831
    27           AccelerometerBiasNoise          0.6831
    27           AccelerometerBiasDecayFactor    0.6831
    27           ZeroVelocityConstraintNoise     0.6818
    27           GPSPositionNoise                0.6816
    27           GPSVelocityNoise                0.6816
    28           GyroscopeNoise                  0.6816
    28           AccelerometerNoise              0.6809
    28           GyroscopeBiasNoise              0.6809
    28           GyroscopeBiasDecayFactor        0.6809
    28           AccelerometerBiasNoise          0.6809
    28           AccelerometerBiasDecayFactor    0.6809
    28           ZeroVelocityConstraintNoise     0.6804
    28           GPSPositionNoise                0.6802
    28           GPSVelocityNoise                0.6802
    29           GyroscopeNoise                  0.6793
    29           AccelerometerNoise              0.6785
    29           GyroscopeBiasNoise              0.6785
    29           GyroscopeBiasDecayFactor        0.6785
    29           AccelerometerBiasNoise          0.6785
    29           AccelerometerBiasDecayFactor    0.6785
    29           ZeroVelocityConstraintNoise     0.6778
    29           GPSPositionNoise                0.6773
    29           GPSVelocityNoise                0.6773
    30           GyroscopeNoise                  0.6773
    30           AccelerometerNoise              0.6769
    30           GyroscopeBiasNoise              0.6769
    30           GyroscopeBiasDecayFactor        0.6769
    30           AccelerometerBiasNoise          0.6769
    30           AccelerometerBiasDecayFactor    0.6769
    30           ZeroVelocityConstraintNoise     0.6769
    30           GPSPositionNoise                0.6769
    30           GPSVelocityNoise                0.6769

Предохраните данные датчика с помощью настроенного фильтра. Получите расчетное положение и ориентацию.

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(GPSPosition(ii,1)))
        fusegps(filter, GPSPosition(ii,:), ...
            tunedNoise.GPSPositionNoise,GPSVelocity(ii,:), ...
            tunedNoise.GPSVelocityNoise);
    end
    [posEstTuned(ii,:),qEstTuned(ii,:)] = pose(filter);
end

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

orientationErrorTuned = rad2deg(dist(qEstTuned,Orientation));
rmsOrientationErrorTuned = sqrt(mean(orientationErrorTuned.^2))
rmsOrientationErrorTuned = 1.6857

positionErrorTuned = sqrt(sum((posEstTuned-Position).^2,2));
rmsPositionErrorTuned = sqrt(mean(positionErrorTuned.^2))
rmsPositionErrorTuned = 1.6667

Визуализация результатов.

figure;
t = (0:N-1)./filter.IMUSampleRate;
subplot(2,1,1)
plot(t,positionErrorTuned,'b');
title("Tuned insfilterNonholonomic" + 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 insfilterNonholonomic Euclidean Distance Position Error contains an object of type line. Axes 2 with title Orientation Error contains an object of type line.

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

свернуть все

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

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

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

Типы данных: struct

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

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

  • Gyroscope - Данные гироскопа, заданные как вектор 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 скаляров в м/с.

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

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

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

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

Типы данных: table

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

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

свернуть все

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

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

Типы данных: struct

Ссылки

[1] Abbeel, Pieter, et al. Дискриминационное обучение фильтров Калмана. Robotics: Science and Systems I, Robotics: Science and Systems Foundation, 2005. DOI.org (Crossref), doi:10.15607/RSS.2005.I.038.

См. также

| |

Введенный в R2020b

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

Поддержка