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.

Примеры

свернуть все

Открыть 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. Axes 1 with title Tuned insfilterErrorState Euclidean Distance Position Error contains an object of type line. Axes 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, заданная как скаляр в (м/с)2

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

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

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

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

  • MVOPosition - Положение камеры в локальной навигационной системе координат, заданное как действительный 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 скаляров в м/с.

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

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

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

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

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

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

свернуть все

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

Имя поляОписание
MVOOrientationNoiseКовариация измерения ориентации монокулярной визуальной одометрии, заданная в виде скаляра, 3-элементного вектора или матрицы 3 на 3 в рад2
MVOPositionNoiseКовариация измерения положения MVO, заданная в виде скаляра, 3-элементного вектора или матрицы 3 на 3 в m2
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.

См. также

|

Введенный в R2021a

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

Поддержка

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