tune
Настройте insfilterMARG параметры, чтобы уменьшать ошибку расчета
Синтаксис
Описание
tunedMeasureNoise = tune(filter,measureNoise,sensorData,groundTruth)insfilterMARG объект фильтра, filter, и шумы измерения, чтобы уменьшать ошибку оценки состояния корневого среднеквадратического (RMS) между объединенными данными о датчике и основной истиной. Функция также возвращает настроенный шум измерения, tunedMeasureNoise. Функция использует значения свойств в фильтре и шуме измерения, обеспеченном в measureNoise структура как первоначальная оценка для алгоритма оптимизации.
tunedMeasureNoise = tune(___,config)tunerconfig объект, config.
Примеры
Настройте insfilterMARG оптимизировать оценку положения
Загрузите записанные данные о датчике и достоверные данные.
load('insfilterMARGTuneData.mat');Составьте таблицы для данных о датчике и данных об истине.
sensorData = table(Accelerometer, Gyroscope, ...
Magnetometer, GPSPosition, GPSVelocity);
groundTruth = table(Orientation, Position);Создайте insfilterMARG объект фильтра, который имеет несколько шумовых свойств.
filter = insfilterMARG('State',initialState,... 'StateCovariance',initialStateCovariance,... 'AccelerometerBiasNoise',1e-7,... 'GyroscopeBiasNoise',1e-7,... 'MagnetometerBiasNoise',1e-7,... 'GeomagneticVectorNoise',1e-7);
Создайте объект настройки тюнера для фильтра. Установите максимальные итерации на восемь. Кроме того, установите настраиваемые параметры.
cfg = tunerconfig('insfilterMARG', 'MaxIterations', 8); cfg.TunableParameters = setdiff(cfg.TunableParameters, ... {'GeomagneticFieldVector', 'AccelerometerBiasNoise', ... 'GyroscopeBiasNoise', 'MagnetometerBiasNoise'});
Используйте функцию шума тюнера, чтобы получить набор начальных шумов датчика, используемых в фильтре.
measNoise = tunernoise('insfilterMARG')measNoise = struct with fields:
MagnetometerNoise: 1
GPSPositionNoise: 1
GPSVelocityNoise: 1
Настройте фильтр и получите настроенные параметры.
tunedParams = tune(filter, measNoise, sensorData, ...
groundTruth, cfg); Iteration Parameter Metric
_________ _________ ______
1 AccelerometerNoise 2.5701
1 GPSPositionNoise 2.5446
1 GPSVelocityNoise 2.5279
1 GeomagneticVectorNoise 2.5268
1 GyroscopeNoise 2.5268
1 MagnetometerNoise 2.5204
2 AccelerometerNoise 2.5203
2 GPSPositionNoise 2.4908
2 GPSVelocityNoise 2.4695
2 GeomagneticVectorNoise 2.4684
2 GyroscopeNoise 2.4684
2 MagnetometerNoise 2.4615
3 AccelerometerNoise 2.4615
3 GPSPositionNoise 2.4265
3 GPSVelocityNoise 2.4000
3 GeomagneticVectorNoise 2.3988
3 GyroscopeNoise 2.3988
3 MagnetometerNoise 2.3911
4 AccelerometerNoise 2.3911
4 GPSPositionNoise 2.3500
4 GPSVelocityNoise 2.3164
4 GeomagneticVectorNoise 2.3153
4 GyroscopeNoise 2.3153
4 MagnetometerNoise 2.3068
5 AccelerometerNoise 2.3068
5 GPSPositionNoise 2.2587
5 GPSVelocityNoise 2.2166
5 GeomagneticVectorNoise 2.2154
5 GyroscopeNoise 2.2154
5 MagnetometerNoise 2.2063
6 AccelerometerNoise 2.2063
6 GPSPositionNoise 2.1505
6 GPSVelocityNoise 2.0981
6 GeomagneticVectorNoise 2.0971
6 GyroscopeNoise 2.0971
6 MagnetometerNoise 2.0875
7 AccelerometerNoise 2.0874
7 GPSPositionNoise 2.0240
7 GPSVelocityNoise 1.9601
7 GeomagneticVectorNoise 1.9594
7 GyroscopeNoise 1.9594
7 MagnetometerNoise 1.9499
8 AccelerometerNoise 1.9499
8 GPSPositionNoise 1.8802
8 GPSVelocityNoise 1.8035
8 GeomagneticVectorNoise 1.8032
8 GyroscopeNoise 1.8032
8 MagnetometerNoise 1.7959
Объедините данные о датчике с помощью настроенного фильтра.
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(Magnetometer(ii,1))) fusemag(filter,Magnetometer(ii,:),... tunedParams.MagnetometerNoise); end if all(~isnan(GPSPosition(ii,1))) fusegps(filter,GPSPosition(ii,:),... tunedParams.GPSPositionNoise,GPSVelocity(ii,:),... tunedParams.GPSVelocityNoise); end [posEstTuned(ii,:),qEstTuned(ii,:)] = pose(filter); end
Вычислите ошибки RMS.
orientationErrorTuned = rad2deg(dist(qEstTuned,Orientation)); rmsOrientationErrorTuned = sqrt(mean(orientationErrorTuned.^2))
rmsOrientationErrorTuned = 0.8580
positionErrorTuned = sqrt(sum((posEstTuned - Position).^2,2)); rmsPositionErrorTuned = sqrt(mean(positionErrorTuned.^2))
rmsPositionErrorTuned = 1.7946
Визуализируйте результаты.
figure(); t = (0:N-1)./filter.IMUSampleRate; subplot(2,1,1) plot(t,positionErrorTuned,'b'); title("Tuned insfilterMARG" + 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)');
Входные параметры
Выходные аргументы
Ссылки
[1] Abbeel, P., Коутс, A., Montemerlo, M., Ын, А.И. и Трун, S. Отличительное Обучение Фильтров Калмана. В Робототехнике: Наука и системы, Издание 2, стр 1, 2005.