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., Coates, A., Montemerlo, M., Ng, A.Y. and Thrun, S. Discriminative Training of Фильтры Калмана. In Robotics: Science and systems, Vol. 2, pp. 1, 2005.