Exponenta Banner

tune

Настройте insfilterErrorState параметры, чтобы уменьшать ошибку расчета

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

Синтаксис

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

Описание

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

пример

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

Примеры

свернуть все

Скрипт Open Live Script

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

load('insfilterErrorStateTuneData.mat');

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

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

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

filter = insfilterErrorState('State',initialState, ...
    'StateCovariance',initialStateCovariance);

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

cfg = tunerconfig('insfilterErrorState','MaxIterations',40);
measNoise = tunernoise('insfilterErrorState')
measNoise = struct with fields:
    MVOOrientationNoise: 1
       MVOPositionNoise: 1
       GPSPositionNoise: 1
       GPSVelocityNoise: 1

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

tunedmn = tune(filter,measNoise,sensorData, ...
    groundTruth,cfg);
    Iteration    Parameter                 Metric
    _________    _________                 ______
    1            AccelerometerNoise        4.1291
    1            GyroscopeNoise            4.1291
    1            AccelerometerBiasNoise    4.1290
    1            GyroscopeBiasNoise        4.1290
    1            GPSPositionNoise          4.0213
    1            GPSVelocityNoise          4.0051
    1            MVOPositionNoise          3.9949
    1            MVOOrientationNoise       3.9886
    2            AccelerometerNoise        3.9886
    2            GyroscopeNoise            3.9886
    2            AccelerometerBiasNoise    3.9886
    2            GyroscopeBiasNoise        3.9886
    2            GPSPositionNoise          3.8381
    2            GPSVelocityNoise          3.8268
    2            MVOPositionNoise          3.8219
    2            MVOOrientationNoise       3.8035
    3            AccelerometerNoise        3.8035
    3            GyroscopeNoise            3.8035
    3            AccelerometerBiasNoise    3.8035
    3            GyroscopeBiasNoise        3.8035
    3            GPSPositionNoise          3.6299
    3            GPSVelocityNoise          3.6276
    3            MVOPositionNoise          3.6241
    3            MVOOrientationNoise       3.5911
    4            AccelerometerNoise        3.5911
    4            GyroscopeNoise            3.5911
    4            AccelerometerBiasNoise    3.5911
    4            GyroscopeBiasNoise        3.5911
    4            GPSPositionNoise          3.1728
    4            GPSVelocityNoise          3.1401
    4            MVOPositionNoise          2.7686
    4            MVOOrientationNoise       2.6632
    5            AccelerometerNoise        2.6632
    5            GyroscopeNoise            2.6632
    5            AccelerometerBiasNoise    2.6632
    5            GyroscopeBiasNoise        2.6632
    5            GPSPositionNoise          2.3242
    5            GPSVelocityNoise          2.2291
    5            MVOPositionNoise          2.2291
    5            MVOOrientationNoise       2.0904
    6            AccelerometerNoise        2.0903
    6            GyroscopeNoise            2.0903
    6            AccelerometerBiasNoise    2.0903
    6            GyroscopeBiasNoise        2.0903
    6            GPSPositionNoise          2.0903
    6            GPSVelocityNoise          2.0141
    6            MVOPositionNoise          1.9952
    6            MVOOrientationNoise       1.8497
    7            AccelerometerNoise        1.8497
    7            GyroscopeNoise            1.8496
    7            AccelerometerBiasNoise    1.8496
    7            GyroscopeBiasNoise        1.8496
    7            GPSPositionNoise          1.8398
    7            GPSVelocityNoise          1.7528
    7            MVOPositionNoise          1.7362
    7            MVOOrientationNoise       1.5762
    8            AccelerometerNoise        1.5762
    8            GyroscopeNoise            1.5762
    8            AccelerometerBiasNoise    1.5762
    8            GyroscopeBiasNoise        1.5762
    8            GPSPositionNoise          1.5762
    8            GPSVelocityNoise          1.5107
    8            MVOPositionNoise          1.4786
    8            MVOOrientationNoise       1.3308
    9            AccelerometerNoise        1.3308
    9            GyroscopeNoise            1.3308
    9            AccelerometerBiasNoise    1.3308
    9            GyroscopeBiasNoise        1.3308
    9            GPSPositionNoise          1.3308
    9            GPSVelocityNoise          1.2934
    9            MVOPositionNoise          1.2525
    9            MVOOrientationNoise       1.1462
    10           AccelerometerNoise        1.1462
    10           GyroscopeNoise            1.1462
    10           AccelerometerBiasNoise    1.1462
    10           GyroscopeBiasNoise        1.1462
    10           GPSPositionNoise          1.1443
    10           GPSVelocityNoise          1.1332
    10           MVOPositionNoise          1.0964
    10           MVOOrientationNoise       1.0382
    11           AccelerometerNoise        1.0382
    11           GyroscopeNoise            1.0382
    11           AccelerometerBiasNoise    1.0382
    11           GyroscopeBiasNoise        1.0382
    11           GPSPositionNoise          1.0348
    11           GPSVelocityNoise          1.0348
    11           MVOPositionNoise          1.0081
    11           MVOOrientationNoise       0.9734
    12           AccelerometerNoise        0.9734
    12           GyroscopeNoise            0.9734
    12           AccelerometerBiasNoise    0.9734
    12           GyroscopeBiasNoise        0.9734
    12           GPSPositionNoise          0.9693
    12           GPSVelocityNoise          0.9682
    12           MVOPositionNoise          0.9488
    12           MVOOrientationNoise       0.9244
    13           AccelerometerNoise        0.9244
    13           GyroscopeNoise            0.9244
    13           AccelerometerBiasNoise    0.9244
    13           GyroscopeBiasNoise        0.9244
    13           GPSPositionNoise          0.9203
    13           GPSVelocityNoise          0.9199
    13           MVOPositionNoise          0.9045
    13           MVOOrientationNoise       0.8846
    14           AccelerometerNoise        0.8846
    14           GyroscopeNoise            0.8846
    14           AccelerometerBiasNoise    0.8845
    14           GyroscopeBiasNoise        0.8845
    14           GPSPositionNoise          0.8807
    14           GPSVelocityNoise          0.8807
    14           MVOPositionNoise          0.8659
    14           MVOOrientationNoise       0.8501
    15           AccelerometerNoise        0.8501
    15           GyroscopeNoise            0.8501
    15           AccelerometerBiasNoise    0.8500
    15           GyroscopeBiasNoise        0.8500
    15           GPSPositionNoise          0.8457
    15           GPSVelocityNoise          0.8453
    15           MVOPositionNoise          0.8299
    15           MVOOrientationNoise       0.8173
    16           AccelerometerNoise        0.8173
    16           GyroscopeNoise            0.8173
    16           AccelerometerBiasNoise    0.8172
    16           GyroscopeBiasNoise        0.8172
    16           GPSPositionNoise          0.8122
    16           GPSVelocityNoise          0.8116
    16           MVOPositionNoise          0.7961
    16           MVOOrientationNoise       0.7858
    17           AccelerometerNoise        0.7858
    17           GyroscopeNoise            0.7858
    17           AccelerometerBiasNoise    0.7857
    17           GyroscopeBiasNoise        0.7857
    17           GPSPositionNoise          0.7807
    17           GPSVelocityNoise          0.7800
    17           MVOPositionNoise          0.7655
    17           MVOOrientationNoise       0.7572
    18           AccelerometerNoise        0.7572
    18           GyroscopeNoise            0.7572
    18           AccelerometerBiasNoise    0.7570
    18           GyroscopeBiasNoise        0.7570
    18           GPSPositionNoise          0.7525
    18           GPSVelocityNoise          0.7520
    18           MVOPositionNoise          0.7401
    18           MVOOrientationNoise       0.7338
    19           AccelerometerNoise        0.7337
    19           GyroscopeNoise            0.7337
    19           AccelerometerBiasNoise    0.7335
    19           GyroscopeBiasNoise        0.7335
    19           GPSPositionNoise          0.7293
    19           GPSVelocityNoise          0.7290
    19           MVOPositionNoise          0.7185
    19           MVOOrientationNoise       0.7140
    20           AccelerometerNoise        0.7138
    20           GyroscopeNoise            0.7138
    20           AccelerometerBiasNoise    0.7134
    20           GyroscopeBiasNoise        0.7134
    20           GPSPositionNoise          0.7086
    20           GPSVelocityNoise          0.7068
    20           MVOPositionNoise          0.6956
    20           MVOOrientationNoise       0.6926
    21           AccelerometerNoise        0.6922
    21           GyroscopeNoise            0.6922
    21           AccelerometerBiasNoise    0.6916
    21           GyroscopeBiasNoise        0.6916
    21           GPSPositionNoise          0.6862
    21           GPSVelocityNoise          0.6822
    21           MVOPositionNoise          0.6682
    21           MVOOrientationNoise       0.6667
    22           AccelerometerNoise        0.6660
    22           GyroscopeNoise            0.6660
    22           AccelerometerBiasNoise    0.6650
    22           GyroscopeBiasNoise        0.6650
    22           GPSPositionNoise          0.6605
    22           GPSVelocityNoise          0.6541
    22           MVOPositionNoise          0.6372
    22           MVOOrientationNoise       0.6368
    23           AccelerometerNoise        0.6356
    23           GyroscopeNoise            0.6356
    23           AccelerometerBiasNoise    0.6344
    23           GyroscopeBiasNoise        0.6344
    23           GPSPositionNoise          0.6324
    23           GPSVelocityNoise          0.6252
    23           MVOPositionNoise          0.6087
    23           MVOOrientationNoise       0.6087
    24           AccelerometerNoise        0.6075
    24           GyroscopeNoise            0.6075
    24           AccelerometerBiasNoise    0.6068
    24           GyroscopeBiasNoise        0.6068
    24           GPSPositionNoise          0.6061
    24           GPSVelocityNoise          0.6032
    24           MVOPositionNoise          0.6032
    24           MVOOrientationNoise       0.6032
    25           AccelerometerNoise        0.6017
    25           GyroscopeNoise            0.6017
    25           AccelerometerBiasNoise    0.6012
    25           GyroscopeBiasNoise        0.6012
    25           GPSPositionNoise          0.6010
    25           GPSVelocityNoise          0.6005
    25           MVOPositionNoise          0.6005
    25           MVOOrientationNoise       0.6005
    26           AccelerometerNoise        0.5992
    26           GyroscopeNoise            0.5992
    26           AccelerometerBiasNoise    0.5987
    26           GyroscopeBiasNoise        0.5987
    26           GPSPositionNoise          0.5983
    26           GPSVelocityNoise          0.5983
    26           MVOPositionNoise          0.5983
    26           MVOOrientationNoise       0.5980
    27           AccelerometerNoise        0.5973
    27           GyroscopeNoise            0.5973
    27           AccelerometerBiasNoise    0.5972
    27           GyroscopeBiasNoise        0.5972
    27           GPSPositionNoise          0.5970
    27           GPSVelocityNoise          0.5970
    27           MVOPositionNoise          0.5970
    27           MVOOrientationNoise       0.5970
    28           AccelerometerNoise        0.5970
    28           GyroscopeNoise            0.5970
    28           AccelerometerBiasNoise    0.5970
    28           GyroscopeBiasNoise        0.5970
    28           GPSPositionNoise          0.5970
    28           GPSVelocityNoise          0.5970
    28           MVOPositionNoise          0.5969
    28           MVOOrientationNoise       0.5966
    29           AccelerometerNoise        0.5966
    29           GyroscopeNoise            0.5966
    29           AccelerometerBiasNoise    0.5966
    29           GyroscopeBiasNoise        0.5966
    29           GPSPositionNoise          0.5966
    29           GPSVelocityNoise          0.5966
    29           MVOPositionNoise          0.5965
    29           MVOOrientationNoise       0.5965
    30           AccelerometerNoise        0.5965
    30           GyroscopeNoise            0.5965
    30           AccelerometerBiasNoise    0.5965
    30           GyroscopeBiasNoise        0.5965
    30           GPSPositionNoise          0.5964
    30           GPSVelocityNoise          0.5964
    30           MVOPositionNoise          0.5964
    30           MVOOrientationNoise       0.5961
    31           AccelerometerNoise        0.5961
    31           GyroscopeNoise            0.5961
    31           AccelerometerBiasNoise    0.5960
    31           GyroscopeBiasNoise        0.5960
    31           GPSPositionNoise          0.5960
    31           GPSVelocityNoise          0.5960
    31           MVOPositionNoise          0.5960
    31           MVOOrientationNoise       0.5958
    32           AccelerometerNoise        0.5956
    32           GyroscopeNoise            0.5956
    32           AccelerometerBiasNoise    0.5956
    32           GyroscopeBiasNoise        0.5956
    32           GPSPositionNoise          0.5955
    32           GPSVelocityNoise          0.5955
    32           MVOPositionNoise          0.5954
    32           MVOOrientationNoise       0.5954
    33           AccelerometerNoise        0.5954
    33           GyroscopeNoise            0.5954
    33           AccelerometerBiasNoise    0.5953
    33           GyroscopeBiasNoise        0.5953
    33           GPSPositionNoise          0.5953
    33           GPSVelocityNoise          0.5953
    33           MVOPositionNoise          0.5952
    33           MVOOrientationNoise       0.5950
    34           AccelerometerNoise        0.5949
    34           GyroscopeNoise            0.5949
    34           AccelerometerBiasNoise    0.5949
    34           GyroscopeBiasNoise        0.5949
    34           GPSPositionNoise          0.5948
    34           GPSVelocityNoise          0.5947
    34           MVOPositionNoise          0.5946
    34           MVOOrientationNoise       0.5946
    35           AccelerometerNoise        0.5946
    35           GyroscopeNoise            0.5946
    35           AccelerometerBiasNoise    0.5946
    35           GyroscopeBiasNoise        0.5946
    35           GPSPositionNoise          0.5946
    35           GPSVelocityNoise          0.5945
    35           MVOPositionNoise          0.5944
    35           MVOOrientationNoise       0.5943
    36           AccelerometerNoise        0.5942
    36           GyroscopeNoise            0.5942
    36           AccelerometerBiasNoise    0.5942
    36           GyroscopeBiasNoise        0.5942
    36           GPSPositionNoise          0.5941
    36           GPSVelocityNoise          0.5941
    36           MVOPositionNoise          0.5939
    36           MVOOrientationNoise       0.5939
    37           AccelerometerNoise        0.5939
    37           GyroscopeNoise            0.5939
    37           AccelerometerBiasNoise    0.5939
    37           GyroscopeBiasNoise        0.5939
    37           GPSPositionNoise          0.5939
    37           GPSVelocityNoise          0.5939
    37           MVOPositionNoise          0.5937
    37           MVOOrientationNoise       0.5937
    38           AccelerometerNoise        0.5937
    38           GyroscopeNoise            0.5937
    38           AccelerometerBiasNoise    0.5937
    38           GyroscopeBiasNoise        0.5937
    38           GPSPositionNoise          0.5936
    38           GPSVelocityNoise          0.5935
    38           MVOPositionNoise          0.5935
    38           MVOOrientationNoise       0.5935
    39           AccelerometerNoise        0.5935
    39           GyroscopeNoise            0.5935
    39           AccelerometerBiasNoise    0.5935
    39           GyroscopeBiasNoise        0.5935
    39           GPSPositionNoise          0.5934
    39           GPSVelocityNoise          0.5934
    39           MVOPositionNoise          0.5934
    39           MVOOrientationNoise       0.5934
    40           AccelerometerNoise        0.5934
    40           GyroscopeNoise            0.5934
    40           AccelerometerBiasNoise    0.5934
    40           GyroscopeBiasNoise        0.5934
    40           GPSPositionNoise          0.5933
    40           GPSVelocityNoise          0.5933
    40           MVOPositionNoise          0.5933
    40           MVOOrientationNoise       0.5933

Объедините данные о датчике с помощью настроенного фильтра.

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,:), ...
            tunedmn.GPSPositionNoise,GPSVelocity(ii,:), ...
            tunedmn.GPSVelocityNoise);
    end
    if all(~isnan(MVOPosition(ii,1)))
        fusemvo(filter,MVOPosition(ii,:),tunedmn.MVOPositionNoise, ...
            MVOOrientation{ii},tunedmn.MVOOrientationNoise);
    end
    [posEstTuned(ii,:),qEstTuned(ii,:)] = pose(filter);
end

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

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

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

Визуализируйте результаты.

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

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

свернуть все

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

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

Имя поляОписание
MVOOrientationNoiseКовариация измерения ориентации монокулярной визуальной одометрии в виде скаляра, вектора с 3 элементами или 3х3 матрицы в рад2
MVOPositionNoiseКовариация измерения положения MVO в виде скаляра, вектора с 3 элементами или 3х3 матрицы в m2
GPSPositionNoiseОтклонение шума положения GPS в виде скаляра в m2
GPSVelocityNoiseОтклонение скоростного шума GPS в виде скаляра в (m/s)2

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

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

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

  • MVOOrienation — Ориентация камеры относительно локальной навигации структурирует в виде скалярного кватерниона или 3х3 матрицы вращения. Матрица кватерниона или вращения является вращением системы координат от локальной системы координат навигации до текущей системы координат камеры.

  • MVOPosition — Положение камеры в локальной навигации структурирует в виде действительного вектора-строки с 3 элементами в метрах.

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

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

  • VisualOdometryScale — Визуальный масштабный коэффициент одометрии в виде скаляра.

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

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

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

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

свернуть все

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

Имя поляОписание
MVOOrientationNoiseКовариация измерения ориентации монокулярной визуальной одометрии в виде скаляра, вектора с 3 элементами или 3х3 матрицы в рад2
MVOPositionNoiseКовариация измерения положения MVO в виде скаляра, вектора с 3 элементами или 3х3 матрицы в m2
GPSPositionNoiseОтклонение шума положения GPS в виде скаляра в m2
GPSVelocityNoiseОтклонение скоростного шума GPS в виде скаляра в (m/s)2

Ссылки

[1] Abbeel, P., Коутс, A., Montemerlo, M., Ын, А.И. и Трун, S. Отличительное Обучение Фильтров Калмана. В Робототехнике: Наука и системы, Издание 2, стр 1, 2005.

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

|

Введенный в R2021a

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

Поддержка