Exponenta 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.

Примеры

свернуть все

Скрипт Open 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 objects. Axes object 1 with title Tuned insfilterNonholonomic Euclidean Distance Position Error contains an object of type line. Axes object 2 with title Orientation Error contains an object of type line.

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

свернуть все

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

свернуть все

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

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

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

Ссылки

[1] Abbeel, Питер, и др. “Отличительное Обучение Фильтров Калмана”. Робототехника: Наука и Системы I, Робототехника: Наука и Системная Основа, 2005. DOI.org (Crossref), doi:10.15607/RSS.2005. I 038.

Смотрите также

| |

Введенный в R2020b

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

Поддержка