Параметры конфигурации тюнера фильтра Fusion
tunerconfig объект создает настройку тюнера для фильтра сплава, используемого, чтобы настроить фильтр для уменьшаемой ошибки расчета.
config = tunerconfig(filterName)tunerconfig объект, управляющий алгоритмом оптимизации функции мелодии фильтра сплава.
config = tunerconfig(filterName,Name,Value)tunerconfig свойства объектов с помощью одного или нескольких аргументов пары "имя-значение". Name имя свойства и Value соответствующее значение. Name должен появиться в кавычках. Можно задать несколько аргументов пары "имя-значение" в любом порядке как Name1,Value1,...,NameN,ValueN. Любые незаданные свойства берут значения по умолчанию.
Например, tunerconfig('imufilter','MaxIterations',3) создайте tunerconfig объект для imufilter объект максимум с трех позволенных итераций.
Создайте объект tunerconfig для insfilterAsync объект.
config = tunerconfig('insfilterAsync')config =
tunerconfig with properties:
TunableParameters: [1×14 string]
StepForward: 1.1000
StepBackward: 0.5000
MaxIterations: 20
OptimalityTolerance: 0.1000
Display: iter
Cost: RMS
Отобразите настраиваемые параметры по умолчанию.
config.TunableParameters
ans = 1×14 string
"AccelerometerNoise" "GyroscopeNoise" "MagnetometerNoise" "GPSPositionNoise" "GPSVelocityNoise" "QuaternionNoise" "AngularVelocityNoise" "PositionNoise" "VelocityNoise" "AccelerationNoise" "GyroscopeBiasNoise" "AccelerometerBiasNoise" "GeomagneticVectorNoise" "MagnetometerBiasNoise"
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)');
imufilter оптимизировать оценку ориентацииЗагрузите зарегистрированные данные о датчике и достоверные данные.
ld = load('imufilterTuneData.mat'); qTrue = ld.groundTruth.Orientation; % true orientation
Создайте imufilter возразите и плавьте фильтр с данными о датчике.
fuse = imufilter;
qEstUntuned = fuse(ld.sensorData.Accelerometer, ...
ld.sensorData.Gyroscope);Создайте tunerconfig возразите и настройте imufilter, чтобы улучшить оценку ориентации.
cfg = tunerconfig('imufilter');
tune(fuse, ld.sensorData, ld.groundTruth, cfg); Iteration Parameter Metric
_________ _________ ______
1 AccelerometerNoise 0.1149
1 GyroscopeNoise 0.1146
1 GyroscopeDriftNoise 0.1146
1 LinearAccelerationNoise 0.1122
1 LinearAccelerationDecayFactor 0.1103
2 AccelerometerNoise 0.1102
2 GyroscopeNoise 0.1098
2 GyroscopeDriftNoise 0.1098
2 LinearAccelerationNoise 0.1070
2 LinearAccelerationDecayFactor 0.1053
3 AccelerometerNoise 0.1053
3 GyroscopeNoise 0.1048
3 GyroscopeDriftNoise 0.1048
3 LinearAccelerationNoise 0.1016
3 LinearAccelerationDecayFactor 0.1002
4 AccelerometerNoise 0.1001
4 GyroscopeNoise 0.0996
4 GyroscopeDriftNoise 0.0996
4 LinearAccelerationNoise 0.0962
4 LinearAccelerationDecayFactor 0.0950
5 AccelerometerNoise 0.0950
5 GyroscopeNoise 0.0943
5 GyroscopeDriftNoise 0.0943
5 LinearAccelerationNoise 0.0910
5 LinearAccelerationDecayFactor 0.0901
6 AccelerometerNoise 0.0900
6 GyroscopeNoise 0.0893
6 GyroscopeDriftNoise 0.0893
6 LinearAccelerationNoise 0.0862
6 LinearAccelerationDecayFactor 0.0855
7 AccelerometerNoise 0.0855
7 GyroscopeNoise 0.0848
7 GyroscopeDriftNoise 0.0848
7 LinearAccelerationNoise 0.0822
7 LinearAccelerationDecayFactor 0.0818
8 AccelerometerNoise 0.0817
8 GyroscopeNoise 0.0811
8 GyroscopeDriftNoise 0.0811
8 LinearAccelerationNoise 0.0791
8 LinearAccelerationDecayFactor 0.0789
9 AccelerometerNoise 0.0788
9 GyroscopeNoise 0.0782
9 GyroscopeDriftNoise 0.0782
9 LinearAccelerationNoise 0.0769
9 LinearAccelerationDecayFactor 0.0768
10 AccelerometerNoise 0.0768
10 GyroscopeNoise 0.0762
10 GyroscopeDriftNoise 0.0762
10 LinearAccelerationNoise 0.0754
10 LinearAccelerationDecayFactor 0.0753
11 AccelerometerNoise 0.0753
11 GyroscopeNoise 0.0747
11 GyroscopeDriftNoise 0.0747
11 LinearAccelerationNoise 0.0741
11 LinearAccelerationDecayFactor 0.0740
12 AccelerometerNoise 0.0740
12 GyroscopeNoise 0.0734
12 GyroscopeDriftNoise 0.0734
12 LinearAccelerationNoise 0.0728
12 LinearAccelerationDecayFactor 0.0728
13 AccelerometerNoise 0.0728
13 GyroscopeNoise 0.0721
13 GyroscopeDriftNoise 0.0721
13 LinearAccelerationNoise 0.0715
13 LinearAccelerationDecayFactor 0.0715
14 AccelerometerNoise 0.0715
14 GyroscopeNoise 0.0706
14 GyroscopeDriftNoise 0.0706
14 LinearAccelerationNoise 0.0700
14 LinearAccelerationDecayFactor 0.0700
15 AccelerometerNoise 0.0700
15 GyroscopeNoise 0.0690
15 GyroscopeDriftNoise 0.0690
15 LinearAccelerationNoise 0.0684
15 LinearAccelerationDecayFactor 0.0684
16 AccelerometerNoise 0.0684
16 GyroscopeNoise 0.0672
16 GyroscopeDriftNoise 0.0672
16 LinearAccelerationNoise 0.0668
16 LinearAccelerationDecayFactor 0.0667
17 AccelerometerNoise 0.0667
17 GyroscopeNoise 0.0655
17 GyroscopeDriftNoise 0.0655
17 LinearAccelerationNoise 0.0654
17 LinearAccelerationDecayFactor 0.0654
18 AccelerometerNoise 0.0654
18 GyroscopeNoise 0.0641
18 GyroscopeDriftNoise 0.0641
18 LinearAccelerationNoise 0.0640
18 LinearAccelerationDecayFactor 0.0639
19 AccelerometerNoise 0.0639
19 GyroscopeNoise 0.0627
19 GyroscopeDriftNoise 0.0627
19 LinearAccelerationNoise 0.0627
19 LinearAccelerationDecayFactor 0.0624
20 AccelerometerNoise 0.0624
20 GyroscopeNoise 0.0614
20 GyroscopeDriftNoise 0.0614
20 LinearAccelerationNoise 0.0613
20 LinearAccelerationDecayFactor 0.0613
Объедините данные о датчике снова с помощью настроенного фильтра.
qEstTuned = fuse(ld.sensorData.Accelerometer, ...
ld.sensorData.Gyroscope);Сравните настроенную и ненастроенную ошибочную эффективность RMS фильтра.
dUntuned = rad2deg(dist(qEstUntuned, qTrue)); dTuned = rad2deg(dist(qEstTuned, qTrue)); rmsUntuned = sqrt(mean(dUntuned.^2))
rmsUntuned = 6.5864
rmsTuned = sqrt(mean(dTuned.^2))
rmsTuned = 3.5098
Визуализируйте результаты.
N = numel(dUntuned); t = (0:N-1)./ fuse.SampleRate; plot(t, dUntuned, 'r', t, dTuned, 'b'); legend('Untuned', 'Tuned'); title('imufilter - Tuned vs Untuned Error') xlabel('Time (s)'); ylabel('Orientation Error (degrees)');
Загрузите записанные данные о датчике и достоверные данные.
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);
Создайте объект настройки тюнера для фильтра. Задайте OutputFcn свойство как индивидуально настраиваемая функция, myOutputFcn, который сохраняет последние настроенные параметры в файле MAT.
config = tunerconfig('insfilterAsync', ... 'MaxIterations',5, ... 'Display','none', ... 'OutputFcn', @myOutputFcn); config.TunableParameters = setdiff(config.TunableParameters, ... {'GeomagneticVectorNoise', 'AccelerometerBiasNoise', ... 'GyroscopeBiasNoise', 'MagnetometerBiasNoise'}); config.TunableParameters
ans = 1x10 string
Columns 1 through 3
"AccelerationNoise" "AccelerometerNoise" "AngularVelocityN..."
Columns 4 through 6
"GPSPositionNoise" "GPSVelocityNoise" "GyroscopeNoise"
Columns 7 through 9
"MagnetometerNoise" "PositionNoise" "QuaternionNoise"
Column 10
"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);
Отобразите параметры сохранения с помощью сохраненного файла.
fileObject = matfile('myfile.mat');
fileObject.paramsans = struct with fields:
AccelerationNoise: [88.8995 88.8995 88.8995]
AccelerometerBiasNoise: [1.0000e-07 1.0000e-07 1.0000e-07]
AccelerometerNoise: 0.7942
AngularVelocityNoise: [0.0089 0.0089 0.0089]
GPSPositionNoise: 1.1664
GPSVelocityNoise: 0.5210
GeomagneticVectorNoise: [1.0000e-07 1.0000e-07 1.0000e-07]
GyroscopeBiasNoise: [1.0000e-07 1.0000e-07 1.0000e-07]
GyroscopeNoise: 0.5210
MagnetometerBiasNoise: [1.0000e-07 1.0000e-07 1.0000e-07]
MagnetometerNoise: 1.0128
PositionNoise: [5.2100e-07 5.2100e-07 5.2100e-07]
QuaternionNoise: [1.3239e-06 1.3239e-06 1.3239e-06 1.3239e-06]
ReferenceLocation: [0 0 0]
State: [28x1 double]
StateCovariance: [28x28 double]
VelocityNoise: [6.3678e-07 6.3678e-07 6.3678e-07]
Выходная функция
function stop = myOutputFcn(params, ~) save('myfile.mat','params'); % overwrite the file with latest stop = false; end
insfilterAsync | insfilterNonholonomic | insfilterMARG | insfilterErrorState | ahrsfilter | ahrs10filter | imufilter