Exponenta Banner
exponenta event banner

мелодия

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

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

Синтаксис

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

Описание

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

пример

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

Примеры

свернуть все

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

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

load('ahrs10filterTuneData.mat');

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

sensorData = table(Accelerometer,Gyroscope,...
    Magnetometer,Altimeter);
groundTruth = table(Orientation, Altitude);

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

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

Создайте объект конфигурации тюнера для фильтра. Установите максимум итераций равным десяти и установите целевой предел равным 0,001.

cfg = tunerconfig('ahrs10filter','MaxIterations',10,...
          'ObjectiveLimit',1e-3);

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

measNoise = tunernoise('ahrs10filter')
measNoise = struct with fields:
    MagnetometerNoise: 1
       AltimeterNoise: 1

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

tunedNoise = tune(filter, measNoise, sensorData, ...
    groundTruth, cfg);
    Iteration    Parameter                 Metric
    _________    _________                 ______
    1            AccelerometerNoise        0.0526
    1            GyroscopeNoise            0.0526
    1            MagnetometerNoise         0.0523
    1            AltimeterNoise            0.0515
    1            AccelerometerBiasNoise    0.0510
    1            GyroscopeBiasNoise        0.0510
    1            GeomagneticVectorNoise    0.0510
    1            MagnetometerBiasNoise     0.0508
    2            AccelerometerNoise        0.0508
    2            GyroscopeNoise            0.0508
    2            MagnetometerNoise         0.0504
    2            AltimeterNoise            0.0494
    2            AccelerometerBiasNoise    0.0490
    2            GyroscopeBiasNoise        0.0490
    2            GeomagneticVectorNoise    0.0490
    2            MagnetometerBiasNoise     0.0487
    3            AccelerometerNoise        0.0487
    3            GyroscopeNoise            0.0487
    3            MagnetometerNoise         0.0482
    3            AltimeterNoise            0.0472
    3            AccelerometerBiasNoise    0.0467
    3            GyroscopeBiasNoise        0.0467
    3            GeomagneticVectorNoise    0.0467
    3            MagnetometerBiasNoise     0.0463
    4            AccelerometerNoise        0.0463
    4            GyroscopeNoise            0.0463
    4            MagnetometerNoise         0.0456
    4            AltimeterNoise            0.0446
    4            AccelerometerBiasNoise    0.0442
    4            GyroscopeBiasNoise        0.0442
    4            GeomagneticVectorNoise    0.0442
    4            MagnetometerBiasNoise     0.0437
    5            AccelerometerNoise        0.0437
    5            GyroscopeNoise            0.0437
    5            MagnetometerNoise         0.0428
    5            AltimeterNoise            0.0417
    5            AccelerometerBiasNoise    0.0413
    5            GyroscopeBiasNoise        0.0413
    5            GeomagneticVectorNoise    0.0413
    5            MagnetometerBiasNoise     0.0408
    6            AccelerometerNoise        0.0408
    6            GyroscopeNoise            0.0408
    6            MagnetometerNoise         0.0397
    6            AltimeterNoise            0.0385
    6            AccelerometerBiasNoise    0.0381
    6            GyroscopeBiasNoise        0.0381
    6            GeomagneticVectorNoise    0.0381
    6            MagnetometerBiasNoise     0.0375
    7            AccelerometerNoise        0.0375
    7            GyroscopeNoise            0.0375
    7            MagnetometerNoise         0.0363
    7            AltimeterNoise            0.0351
    7            AccelerometerBiasNoise    0.0347
    7            GyroscopeBiasNoise        0.0347
    7            GeomagneticVectorNoise    0.0347
    7            MagnetometerBiasNoise     0.0342
    8            AccelerometerNoise        0.0342
    8            GyroscopeNoise            0.0342
    8            MagnetometerNoise         0.0331
    8            AltimeterNoise            0.0319
    8            AccelerometerBiasNoise    0.0316
    8            GyroscopeBiasNoise        0.0316
    8            GeomagneticVectorNoise    0.0316
    8            MagnetometerBiasNoise     0.0313
    9            AccelerometerNoise        0.0313
    9            GyroscopeNoise            0.0313
    9            MagnetometerNoise         0.0313
    9            AltimeterNoise            0.0301
    9            AccelerometerBiasNoise    0.0298
    9            GyroscopeBiasNoise        0.0298
    9            GeomagneticVectorNoise    0.0298
    9            MagnetometerBiasNoise     0.0296
    10           AccelerometerNoise        0.0296
    10           GyroscopeNoise            0.0296
    10           MagnetometerNoise         0.0296
    10           AltimeterNoise            0.0285
    10           AccelerometerBiasNoise    0.0283
    10           GyroscopeBiasNoise        0.0283
    10           GeomagneticVectorNoise    0.0283
    10           MagnetometerBiasNoise     0.0282

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

N = size(sensorData,1);
qEstTuned = quaternion.zeros(N,1);
altEstTuned = zeros(N,1);
for ii=1:N
    predict(filter,Accelerometer(ii,:),Gyroscope(ii,:));
    if all(~isnan(Magnetometer(ii,1)))
        fusemag(filter, Magnetometer(ii,:),tunedNoise.MagnetometerNoise);
    end
    if ~isnan(Altimeter(ii))
        fusealtimeter(filter, Altimeter(ii),tunedNoise.AltimeterNoise);
    end
    [altEstTuned(ii), qEstTuned(ii)] = pose(filter);
end

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

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

positionErrorTuned = altEstTuned - Altitude;
rmsPositionErrorTuned = sqrt(mean( positionErrorTuned.^2))
rmsPositionErrorTuned = 0.0199

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

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

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

свернуть все

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

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

Имя поляОписание
MagnetometerNoiseДисперсия шума магнитометра, заданная как скаляр в (мкТ) 2
AltimeterNoiseДисперсия шума высотомера, заданная как скаляр в м2

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

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

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

  • Magnetometer - Данные магнитометра, заданные как вектор скаляров в мкТ 1 на 3.

  • Altimeter - данные высотомера, заданные как скаляр в метрах.

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

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

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

  • Altitude - Высота над уровнем моря, заданная как скаляр в метрах.

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

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

  • DeltaVelocityBias - Смещение дельта-скорости, определяемое как вектор скаляров в м/с.

  • GeomagneticFieldVector - вектор геомагнитного поля в навигационном кадре, определяемый как вектор скаляров 1 на 3.

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

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

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

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

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

свернуть все

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

Имя поляОписание
MagnetometerNoiseДисперсия шума магнитометра, заданная как скаляр в (мкТ) 2
AltimeterNoiseДисперсия шума высотомера, заданная как скаляр в м2

Ссылки

[1] Аббиль, П., Коутс, А., Монтемерло, М., Нг, А. Я. и Трюн, С. Дискриминативная подготовка фильтров Калмана. В робототехнике: наука и системы, том 2, стр. 1, 2005.

См. также

|

Представлен в R2021a

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

Поддержка