Exponenta Banner
exponenta event banner

мелодия

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

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

Синтаксис

tunedMeasureNoise = tune (фильтр, measureNoise, sensorData, groundTruth)
tunedMeasureNoise = tune (___, конфигурация)

Описание

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

пример

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

Примеры

свернуть все

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

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

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, заданная как скаляр в м2
GPSVelocityNoiseДисперсия шума скорости GPS, заданная как скаляр в (м/с) 2

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

свернуть все

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

Имя поляОписание
GPSPositionNoiseДисперсия позиционного шума GPS, заданная как скаляр в м2
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

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

Поддержка