Параметры конфигурации тюнера Fusion filter
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
ahrs10filter | ahrsfilter | imufilter | insfilterAsync | insfilterErrorState | insfilterMARG | insfilterNonholonomic