tune
Настройте ahrs10filter параметры, чтобы уменьшать ошибку расчета
Синтаксис
Описание
tunedMeasureNoise = tune(filter,measureNoise,sensorData,groundTruth)ahrs10filter объект фильтра, filter, и шумы измерения, чтобы уменьшать ошибку оценки состояния корневого среднеквадратического (RMS) между объединенными данными о датчике и основной истиной. Функция также возвращает настроенный шум измерения, tunedMeasureNoise. Функция использует значения свойств в фильтре и шуме измерения, обеспеченном в measureNoise структура как первоначальная оценка для алгоритма оптимизации.
tunedMeasureNoise = tune(___,config)tunerconfig объект, config.
Примеры
Настройте ahrs10filter, чтобы Оптимизировать Оценку Положения
Загрузите записанные данные о датчике и достоверные данные.
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)');
Входные параметры
Выходные аргументы
Ссылки
[1] Abbeel, P., Коутс, A., Montemerlo, M., Ын, А.И. и Трун, S. Отличительное Обучение Фильтров Калмана. В Робототехнике: Наука и системы, Издание 2, стр 1, 2005.