Exponenta Banner

tune

Настройка ahrs10filter параметры для уменьшения ошибки расчета

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

Синтаксис

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

Описание

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

пример

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

Примеры

свернуть все

Открыть Live Script

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

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Отклонение шума альтиметра, заданная как скаляр в m2

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

свернуть все

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

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

Ссылки

[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

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

Поддержка