Настройка insfilterAsync параметры для уменьшения ошибки расчета
tunedMeasureNoise = tune(filter,measureNoise,sensorData,groundTruth)insfilterAsync объект фильтра, filterи измерительные шумы для уменьшения среднеквадратичного состояния (RMS) ошибки расчета между данными сплавленного датчика и основной истины. Функция также возвращает настроенный шум измерения tunedMeasureNoise. Функция использует значения свойств в фильтре и шум измерения, предоставленный в measureNoise структура как начальная оценка для алгоритма оптимизации.
tunedMeasureNoise = tune(___,config)tunerconfig объект, config.
insfilterAsync Оптимизация оценки положенияЗагрузите записанные данные датчика и достоверные данные.
load('insfilterAsyncTuneData.mat');Создайте расписания для данных датчика и данных истинности.
sensorData = timetable(Accelerometer, Gyroscope, ... Magnetometer, GPSPosition, GPSVelocity, 'SampleRate', 100); groundTruth = timetable(Orientation, Position, ... 'SampleRate', 100);
Создайте insfilterAsync объект фильтра, который имеет несколько шумовых свойств.
filter = insfilterAsync('State', initialState, ... 'StateCovariance', initialStateCovariance, ... 'AccelerometerBiasNoise', 1e-7, ... 'GyroscopeBiasNoise', 1e-7, ... 'MagnetometerBiasNoise', 1e-7, ... 'GeomagneticVectorNoise', 1e-7);
Создайте объект строения тюнера для фильтра. Установите максимальные итерации равными двум. Кроме того, установите настраиваемые параметры как неопределенные свойства.
config = tunerconfig('insfilterAsync','MaxIterations',8); config.TunableParameters = setdiff(config.TunableParameters, ... {'GeomagneticVectorNoise', 'AccelerometerBiasNoise', ... 'GyroscopeBiasNoise', 'MagnetometerBiasNoise'}); config.TunableParameters
ans = 1×10 string
"AccelerationNoise" "AccelerometerNoise" "AngularVelocityNoise" "GPSPositionNoise" "GPSVelocityNoise" "GyroscopeNoise" "MagnetometerNoise" "PositionNoise" "QuaternionNoise" "VelocityNoise"
Используйте функцию шума тюнера, чтобы получить набор начальных шумов датчика, используемых в фильтре.
measNoise = tunernoise('insfilterAsync')measNoise = struct with fields:
AccelerometerNoise: 1
GyroscopeNoise: 1
MagnetometerNoise: 1
GPSPositionNoise: 1
GPSVelocityNoise: 1
Настройте фильтр и получите настроенные параметры.
tunedParams = tune(filter,measNoise,sensorData,groundTruth,config);
Iteration Parameter Metric
_________ _________ ______
1 AccelerationNoise 2.1345
1 AccelerometerNoise 2.1264
1 AngularVelocityNoise 1.9659
1 GPSPositionNoise 1.9341
1 GPSVelocityNoise 1.8420
1 GyroscopeNoise 1.7589
1 MagnetometerNoise 1.7362
1 PositionNoise 1.7362
1 QuaternionNoise 1.7218
1 VelocityNoise 1.7218
2 AccelerationNoise 1.7190
2 AccelerometerNoise 1.7170
2 AngularVelocityNoise 1.6045
2 GPSPositionNoise 1.5948
2 GPSVelocityNoise 1.5323
2 GyroscopeNoise 1.4803
2 MagnetometerNoise 1.4703
2 PositionNoise 1.4703
2 QuaternionNoise 1.4632
2 VelocityNoise 1.4632
3 AccelerationNoise 1.4596
3 AccelerometerNoise 1.4548
3 AngularVelocityNoise 1.3923
3 GPSPositionNoise 1.3810
3 GPSVelocityNoise 1.3322
3 GyroscopeNoise 1.2998
3 MagnetometerNoise 1.2976
3 PositionNoise 1.2976
3 QuaternionNoise 1.2943
3 VelocityNoise 1.2943
4 AccelerationNoise 1.2906
4 AccelerometerNoise 1.2836
4 AngularVelocityNoise 1.2491
4 GPSPositionNoise 1.2258
4 GPSVelocityNoise 1.1880
4 GyroscopeNoise 1.1701
4 MagnetometerNoise 1.1698
4 PositionNoise 1.1698
4 QuaternionNoise 1.1688
4 VelocityNoise 1.1688
5 AccelerationNoise 1.1650
5 AccelerometerNoise 1.1569
5 AngularVelocityNoise 1.1454
5 GPSPositionNoise 1.1100
5 GPSVelocityNoise 1.0778
5 GyroscopeNoise 1.0709
5 MagnetometerNoise 1.0675
5 PositionNoise 1.0675
5 QuaternionNoise 1.0669
5 VelocityNoise 1.0669
6 AccelerationNoise 1.0634
6 AccelerometerNoise 1.0549
6 AngularVelocityNoise 1.0549
6 GPSPositionNoise 1.0180
6 GPSVelocityNoise 0.9866
6 GyroscopeNoise 0.9810
6 MagnetometerNoise 0.9775
6 PositionNoise 0.9775
6 QuaternionNoise 0.9768
6 VelocityNoise 0.9768
7 AccelerationNoise 0.9735
7 AccelerometerNoise 0.9652
7 AngularVelocityNoise 0.9652
7 GPSPositionNoise 0.9283
7 GPSVelocityNoise 0.8997
7 GyroscopeNoise 0.8947
7 MagnetometerNoise 0.8920
7 PositionNoise 0.8920
7 QuaternionNoise 0.8912
7 VelocityNoise 0.8912
8 AccelerationNoise 0.8885
8 AccelerometerNoise 0.8811
8 AngularVelocityNoise 0.8807
8 GPSPositionNoise 0.8479
8 GPSVelocityNoise 0.8238
8 GyroscopeNoise 0.8165
8 MagnetometerNoise 0.8165
8 PositionNoise 0.8165
8 QuaternionNoise 0.8159
8 VelocityNoise 0.8159
Предохраните данные датчика с помощью настроенного фильтра.
dt = seconds(diff(groundTruth.Time)); N = size(sensorData,1); qEst = quaternion.zeros(N,1); posEst = zeros(N,3); % Iterate the filter for prediction and correction using sensor data. for ii=1:N if ii ~= 1 predict(filter, dt(ii-1)); end if all(~isnan(Accelerometer(ii,:))) fuseaccel(filter,Accelerometer(ii,:), ... tunedParams.AccelerometerNoise); end if all(~isnan(Gyroscope(ii,:))) fusegyro(filter, Gyroscope(ii,:), ... tunedParams.GyroscopeNoise); end 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 [posEst(ii,:), qEst(ii,:)] = pose(filter); end
Вычислите ошибки RMS.
orientationError = rad2deg(dist(qEst, Orientation)); rmsorientationError = sqrt(mean(orientationError.^2))
rmsorientationError = 2.7801
positionError = sqrt(sum((posEst - Position).^2, 2)); rmspositionError = sqrt(mean( positionError.^2))
rmspositionError = 0.5966
Визуализация результатов.
figure(); t = (0:N-1)./ groundTruth.Properties.SampleRate; subplot(2,1,1) plot(t, positionError, 'b'); title("Tuned insfilterAsync" + newline + "Euclidean Distance Position Error") xlabel('Time (s)'); ylabel('Position Error (meters)') subplot(2,1,2) plot(t, orientationError, '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.