Используйте tune функция, чтобы оптимизировать шумовые параметры нескольких фильтров сплава, включая ahrsfilter объект. В этом примере показано, как к пользовательскому функция стоимости для различных целей оптимизации.
Данные о датчике содержат записи датчика UAV, выполняющего некоторые маленькие маневры. Создайте ahrsfilter объект объединить данные о датчике и оценить те маневры.
load AHRSCustomTune.mat % Create a filter to process the data, decimating by 10. filt = ahrsfilter('SampleRate',Fs,'DecimationFactor',10); % Filter the sensor data and show the estimation error. oEstInit = filt(sensorData.Accelerometer, sensorData.Gyroscope,sensorData.Magnetometer); plotPerformance(oEstInit,groundTruth.Orientation, "Initial, Untuned Error");
Эффективность ahrsfilter не настраивая шумовые параметры не идеально. Используйте tune функция, чтобы улучшать производительность фильтра.
reset(filt); cfg1 = tunerconfig("ahrsfilter","MaxIterations",20,"ObjectiveLimit",0.0001); tune(filt,sensorData,groundTruth(1:10:end,:),cfg1);
Iteration Parameter Metric
_________ _________ ______
1 AccelerometerNoise 0.4382
1 GyroscopeNoise 0.4371
1 MagnetometerNoise 0.4370
1 GyroscopeDriftNoise 0.4370
1 LinearAccelerationNoise 0.4202
1 MagneticDisturbanceNoise 0.4188
1 LinearAccelerationDecayFactor 0.4087
1 MagneticDisturbanceDecayFactor 0.4087
2 AccelerometerNoise 0.4086
2 GyroscopeNoise 0.4066
2 MagnetometerNoise 0.4066
2 GyroscopeDriftNoise 0.4066
2 LinearAccelerationNoise 0.3937
2 MagneticDisturbanceNoise 0.3932
2 LinearAccelerationDecayFactor 0.3856
2 MagneticDisturbanceDecayFactor 0.3854
3 AccelerometerNoise 0.3853
3 GyroscopeNoise 0.3826
3 MagnetometerNoise 0.3825
3 GyroscopeDriftNoise 0.3825
3 LinearAccelerationNoise 0.3690
3 MagneticDisturbanceNoise 0.3676
3 LinearAccelerationDecayFactor 0.3613
3 MagneticDisturbanceDecayFactor 0.3611
4 AccelerometerNoise 0.3610
4 GyroscopeNoise 0.3577
4 MagnetometerNoise 0.3576
4 GyroscopeDriftNoise 0.3576
4 LinearAccelerationNoise 0.3431
4 MagneticDisturbanceNoise 0.3414
4 LinearAccelerationDecayFactor 0.3364
4 MagneticDisturbanceDecayFactor 0.3363
5 AccelerometerNoise 0.3362
5 GyroscopeNoise 0.3328
5 MagnetometerNoise 0.3326
5 GyroscopeDriftNoise 0.3326
5 LinearAccelerationNoise 0.3190
5 MagneticDisturbanceNoise 0.3183
5 LinearAccelerationDecayFactor 0.3152
5 MagneticDisturbanceDecayFactor 0.3150
6 AccelerometerNoise 0.3149
6 GyroscopeNoise 0.3121
6 MagnetometerNoise 0.3119
6 GyroscopeDriftNoise 0.3119
6 LinearAccelerationNoise 0.3040
6 MagneticDisturbanceNoise 0.3035
6 LinearAccelerationDecayFactor 0.3024
6 MagneticDisturbanceDecayFactor 0.3022
7 AccelerometerNoise 0.3022
7 GyroscopeNoise 0.2990
7 MagnetometerNoise 0.2989
7 GyroscopeDriftNoise 0.2989
7 LinearAccelerationNoise 0.2970
7 MagneticDisturbanceNoise 0.2955
7 LinearAccelerationDecayFactor 0.2952
7 MagneticDisturbanceDecayFactor 0.2948
8 AccelerometerNoise 0.2948
8 GyroscopeNoise 0.2903
8 MagnetometerNoise 0.2902
8 GyroscopeDriftNoise 0.2902
8 LinearAccelerationNoise 0.2883
8 MagneticDisturbanceNoise 0.2860
8 LinearAccelerationDecayFactor 0.2856
8 MagneticDisturbanceDecayFactor 0.2851
9 AccelerometerNoise 0.2851
9 GyroscopeNoise 0.2778
9 MagnetometerNoise 0.2777
9 GyroscopeDriftNoise 0.2777
9 LinearAccelerationNoise 0.2709
9 MagneticDisturbanceNoise 0.2698
9 LinearAccelerationDecayFactor 0.2690
9 MagneticDisturbanceDecayFactor 0.2689
10 AccelerometerNoise 0.2689
10 GyroscopeNoise 0.2593
10 MagnetometerNoise 0.2593
10 GyroscopeDriftNoise 0.2593
10 LinearAccelerationNoise 0.2492
10 MagneticDisturbanceNoise 0.2490
10 LinearAccelerationDecayFactor 0.2482
10 MagneticDisturbanceDecayFactor 0.2482
11 AccelerometerNoise 0.2481
11 GyroscopeNoise 0.2370
11 MagnetometerNoise 0.2369
11 GyroscopeDriftNoise 0.2369
11 LinearAccelerationNoise 0.2240
11 MagneticDisturbanceNoise 0.2237
11 LinearAccelerationDecayFactor 0.2230
11 MagneticDisturbanceDecayFactor 0.2228
12 AccelerometerNoise 0.2227
12 GyroscopeNoise 0.2117
12 MagnetometerNoise 0.2117
12 GyroscopeDriftNoise 0.2117
12 LinearAccelerationNoise 0.1984
12 MagneticDisturbanceNoise 0.1979
12 LinearAccelerationDecayFactor 0.1974
12 MagneticDisturbanceDecayFactor 0.1974
13 AccelerometerNoise 0.1973
13 GyroscopeNoise 0.1878
13 MagnetometerNoise 0.1878
13 GyroscopeDriftNoise 0.1878
13 LinearAccelerationNoise 0.1766
13 MagneticDisturbanceNoise 0.1763
13 LinearAccelerationDecayFactor 0.1761
13 MagneticDisturbanceDecayFactor 0.1761
14 AccelerometerNoise 0.1760
14 GyroscopeNoise 0.1686
14 MagnetometerNoise 0.1685
14 GyroscopeDriftNoise 0.1685
14 LinearAccelerationNoise 0.1601
14 MagneticDisturbanceNoise 0.1599
14 LinearAccelerationDecayFactor 0.1597
14 MagneticDisturbanceDecayFactor 0.1597
15 AccelerometerNoise 0.1596
15 GyroscopeNoise 0.1536
15 MagnetometerNoise 0.1536
15 GyroscopeDriftNoise 0.1536
15 LinearAccelerationNoise 0.1472
15 MagneticDisturbanceNoise 0.1469
15 LinearAccelerationDecayFactor 0.1469
15 MagneticDisturbanceDecayFactor 0.1469
16 AccelerometerNoise 0.1468
16 GyroscopeNoise 0.1422
16 MagnetometerNoise 0.1422
16 GyroscopeDriftNoise 0.1422
16 LinearAccelerationNoise 0.1380
16 MagneticDisturbanceNoise 0.1378
16 LinearAccelerationDecayFactor 0.1377
16 MagneticDisturbanceDecayFactor 0.1377
17 AccelerometerNoise 0.1377
17 GyroscopeNoise 0.1352
17 MagnetometerNoise 0.1351
17 GyroscopeDriftNoise 0.1351
17 LinearAccelerationNoise 0.1351
17 MagneticDisturbanceNoise 0.1351
17 LinearAccelerationDecayFactor 0.1351
17 MagneticDisturbanceDecayFactor 0.1351
18 AccelerometerNoise 0.1351
18 GyroscopeNoise 0.1351
18 MagnetometerNoise 0.1351
18 GyroscopeDriftNoise 0.1351
18 LinearAccelerationNoise 0.1351
18 MagneticDisturbanceNoise 0.1351
18 LinearAccelerationDecayFactor 0.1350
18 MagneticDisturbanceDecayFactor 0.1350
19 AccelerometerNoise 0.1350
19 GyroscopeNoise 0.1348
19 MagnetometerNoise 0.1344
19 GyroscopeDriftNoise 0.1344
19 LinearAccelerationNoise 0.1344
19 MagneticDisturbanceNoise 0.1344
19 LinearAccelerationDecayFactor 0.1344
19 MagneticDisturbanceDecayFactor 0.1344
20 AccelerometerNoise 0.1344
20 GyroscopeNoise 0.1344
20 MagnetometerNoise 0.1344
20 GyroscopeDriftNoise 0.1344
20 LinearAccelerationNoise 0.1344
20 MagneticDisturbanceNoise 0.1344
20 LinearAccelerationDecayFactor 0.1344
20 MagneticDisturbanceDecayFactor 0.1344
Отфильтруйте данные о датчике с помощью настроенного фильтра и покажите ошибку ориентации.
oEstTuned = filt(sensorData.Accelerometer,sensorData.Gyroscope,sensorData.Magnetometer);
plotPerformance(oEstTuned,groundTruth.Orientation,"Tuned Filter Error - Default Configuration");
Производительность фильтра улучшается после настройки, но настраивающий процесс может часто занимать много времени. tunerconfig объект позволяет, чтобы пользовательская функция стоимости оптимизировала этот процесс. Можно также использовать MATLAB Coder, чтобы создать MEX-функцию, чтобы ускорить настраивающуюся скорость. Пользовательская функция стоимости должна иметь подпись cost = fcn(params,sensorData,groundTruth), где cost скалярное вещественное число, params struct шумовых параметров, которые будут оптимизированы, sensorData таблица данных о датчике и groundTruth таблица достоверные данные.
От последнего раздела, ahrsfilter не оценил ориентацию очень хорошо во время некоторых маневров. Вместо того, чтобы использовать среднеквадратическую ошибку по умолчанию, пользовательская функция стоимости использует термины высшего порядка, чтобы более сильно оштрафовать выбросы.
Отобразите детали пользовательской функции стоимости. Функция присоединяется как m-файл.
type customFcn.mfunction c = customFcn(params, sensorData, groundTruth)
% Custom Cost function for optimizing the ahrsfilter
% Set any nontunable parameters in the constructor
decim = 10;
h = ahrsfilter('SampleRate', 200, 'DecimationFactor', decim);
% Parameterize the filter instance with the current-best parameters from
% the params struct.
h.AccelerometerNoise = params.AccelerometerNoise;
h.GyroscopeNoise = params.GyroscopeNoise;
h.MagnetometerNoise = params.MagnetometerNoise;
h.GyroscopeDriftNoise = params.GyroscopeDriftNoise;
h.LinearAccelerationNoise = params.LinearAccelerationNoise;
h.MagneticDisturbanceNoise = params.MagneticDisturbanceNoise;
h.LinearAccelerationDecayFactor = params.LinearAccelerationDecayFactor;
h.MagneticDisturbanceDecayFactor = params.MagneticDisturbanceDecayFactor;
h.ExpectedMagneticFieldStrength = params.ExpectedMagneticFieldStrength;
% Fuse sensor data
qest = h(sensorData.Accelerometer, sensorData.Gyroscope, ...
sensorData.Magnetometer);
% Compute the orientation error
d = dist(qest, groundTruth.Orientation(1:decim:end));
% Penalize outliers heavily by using the 6th power.
c = sqrt(sqrt( mean(d(10:end,:).^6) ));
Чтобы создать файл MEX, вам нужны входные параметры образца. Создайте образец параметров и скопируйте свойства с filt.
p = {'AccelerometerNoise', 'DecimationFactor', 'ExpectedMagneticFieldStrength', ...
'GyroscopeDriftNoise', 'GyroscopeNoise', 'InitialProcessNoise', 'LinearAccelerationDecayFactor', ...
'LinearAccelerationNoise', 'MagneticDisturbanceDecayFactor', 'MagneticDisturbanceNoise', ...
'MagnetometerNoise', 'OrientationFormat', 'SampleRate'}p = 1×13 cell
{'AccelerometerNoise'} {'DecimationFactor'} {'ExpectedMagneticFieldStrength'} {'GyroscopeDriftNoise'} {'GyroscopeNoise'} {'InitialProcessNoise'} {'LinearAccelerationDecayFactor'} {'LinearAccelerationNoise'} {'MagneticDisturbanceDecayFactor'} {'MagneticDisturbanceNoise'} {'MagnetometerNoise'} {'OrientationFormat'} {'SampleRate'}
for idx=1:numel(p) paramEx.(p{idx}) = filt.(p{idx}); end disp(paramEx);
AccelerometerNoise: 5.4972e-07
DecimationFactor: 10
ExpectedMagneticFieldStrength: 50
GyroscopeDriftNoise: 4.0927e-11
GyroscopeNoise: 0.0041
InitialProcessNoise: [12×12 double]
LinearAccelerationDecayFactor: 0.0050
LinearAccelerationNoise: 1.4370e-04
MagneticDisturbanceDecayFactor: 0.9872
MagneticDisturbanceNoise: 0.0360
MagnetometerNoise: 0.1278
OrientationFormat: 'quaternion'
SampleRate: 200
Сгенерируйте код.
codegen customFcn.m -args {paramEx sensorData, groundTruth}
Code generation successful.
Используйте MEX-функцию, чтобы настроиться быстро.
cfg = tunerconfig("ahrsfilter", "Cost", "Custom", ... "CustomCostFcn", @customFcn_mex, "MaxIterations", 20, "ObjectiveLimit", 0.0001); reset(filt); tune(filt, sensorData, groundTruth, cfg);
Iteration Parameter Metric
_________ _________ ______
1 AccelerometerNoise 0.1581
1 GyroscopeNoise 0.1544
1 MagnetometerNoise 0.1544
1 GyroscopeDriftNoise 0.1544
1 LinearAccelerationNoise 0.1504
1 MagneticDisturbanceNoise 0.1498
1 LinearAccelerationDecayFactor 0.1497
1 MagneticDisturbanceDecayFactor 0.1474
2 AccelerometerNoise 0.1474
2 GyroscopeNoise 0.1437
2 MagnetometerNoise 0.1436
2 GyroscopeDriftNoise 0.1436
2 LinearAccelerationNoise 0.1395
2 MagneticDisturbanceNoise 0.1387
2 LinearAccelerationDecayFactor 0.1387
2 MagneticDisturbanceDecayFactor 0.1367
3 AccelerometerNoise 0.1367
3 GyroscopeNoise 0.1332
3 MagnetometerNoise 0.1332
3 GyroscopeDriftNoise 0.1332
3 LinearAccelerationNoise 0.1293
3 MagneticDisturbanceNoise 0.1284
3 LinearAccelerationDecayFactor 0.1284
3 MagneticDisturbanceDecayFactor 0.1271
4 AccelerometerNoise 0.1270
4 GyroscopeNoise 0.1243
4 MagnetometerNoise 0.1242
4 GyroscopeDriftNoise 0.1242
4 LinearAccelerationNoise 0.1209
4 MagneticDisturbanceNoise 0.1201
4 LinearAccelerationDecayFactor 0.1201
4 MagneticDisturbanceDecayFactor 0.1193
5 AccelerometerNoise 0.1193
5 GyroscopeNoise 0.1180
5 MagnetometerNoise 0.1178
5 GyroscopeDriftNoise 0.1178
5 LinearAccelerationNoise 0.1158
5 MagneticDisturbanceNoise 0.1152
5 LinearAccelerationDecayFactor 0.1152
5 MagneticDisturbanceDecayFactor 0.1147
6 AccelerometerNoise 0.1147
6 GyroscopeNoise 0.1147
6 MagnetometerNoise 0.1143
6 GyroscopeDriftNoise 0.1143
6 LinearAccelerationNoise 0.1132
6 MagneticDisturbanceNoise 0.1123
6 LinearAccelerationDecayFactor 0.1123
6 MagneticDisturbanceDecayFactor 0.1118
7 AccelerometerNoise 0.1118
7 GyroscopeNoise 0.1113
7 MagnetometerNoise 0.1108
7 GyroscopeDriftNoise 0.1108
7 LinearAccelerationNoise 0.1100
7 MagneticDisturbanceNoise 0.1093
7 LinearAccelerationDecayFactor 0.1093
7 MagneticDisturbanceDecayFactor 0.1093
8 AccelerometerNoise 0.1093
8 GyroscopeNoise 0.1088
8 MagnetometerNoise 0.1084
8 GyroscopeDriftNoise 0.1084
8 LinearAccelerationNoise 0.1084
8 MagneticDisturbanceNoise 0.1084
8 LinearAccelerationDecayFactor 0.1084
8 MagneticDisturbanceDecayFactor 0.1084
9 AccelerometerNoise 0.1084
9 GyroscopeNoise 0.1076
9 MagnetometerNoise 0.1072
9 GyroscopeDriftNoise 0.1072
9 LinearAccelerationNoise 0.1072
9 MagneticDisturbanceNoise 0.1072
9 LinearAccelerationDecayFactor 0.1072
9 MagneticDisturbanceDecayFactor 0.1071
10 AccelerometerNoise 0.1071
10 GyroscopeNoise 0.1068
10 MagnetometerNoise 0.1065
10 GyroscopeDriftNoise 0.1065
10 LinearAccelerationNoise 0.1062
10 MagneticDisturbanceNoise 0.1060
10 LinearAccelerationDecayFactor 0.1060
10 MagneticDisturbanceDecayFactor 0.1059
11 AccelerometerNoise 0.1059
11 GyroscopeNoise 0.1057
11 MagnetometerNoise 0.1055
11 GyroscopeDriftNoise 0.1055
11 LinearAccelerationNoise 0.1049
11 MagneticDisturbanceNoise 0.1048
11 LinearAccelerationDecayFactor 0.1048
11 MagneticDisturbanceDecayFactor 0.1048
12 AccelerometerNoise 0.1048
12 GyroscopeNoise 0.1047
12 MagnetometerNoise 0.1045
12 GyroscopeDriftNoise 0.1045
12 LinearAccelerationNoise 0.1038
12 MagneticDisturbanceNoise 0.1036
12 LinearAccelerationDecayFactor 0.1036
12 MagneticDisturbanceDecayFactor 0.1035
13 AccelerometerNoise 0.1035
13 GyroscopeNoise 0.1035
13 MagnetometerNoise 0.1033
13 GyroscopeDriftNoise 0.1033
13 LinearAccelerationNoise 0.1029
13 MagneticDisturbanceNoise 0.1027
13 LinearAccelerationDecayFactor 0.1027
13 MagneticDisturbanceDecayFactor 0.1027
14 AccelerometerNoise 0.1027
14 GyroscopeNoise 0.1024
14 MagnetometerNoise 0.1021
14 GyroscopeDriftNoise 0.1021
14 LinearAccelerationNoise 0.1019
14 MagneticDisturbanceNoise 0.1018
14 LinearAccelerationDecayFactor 0.1018
14 MagneticDisturbanceDecayFactor 0.1018
15 AccelerometerNoise 0.1018
15 GyroscopeNoise 0.1014
15 MagnetometerNoise 0.1012
15 GyroscopeDriftNoise 0.1012
15 LinearAccelerationNoise 0.1012
15 MagneticDisturbanceNoise 0.1012
15 LinearAccelerationDecayFactor 0.1011
15 MagneticDisturbanceDecayFactor 0.1011
16 AccelerometerNoise 0.1011
16 GyroscopeNoise 0.1008
16 MagnetometerNoise 0.1008
16 GyroscopeDriftNoise 0.1008
16 LinearAccelerationNoise 0.1006
16 MagneticDisturbanceNoise 0.1005
16 LinearAccelerationDecayFactor 0.1004
16 MagneticDisturbanceDecayFactor 0.1004
17 AccelerometerNoise 0.1004
17 GyroscopeNoise 0.1001
17 MagnetometerNoise 0.1001
17 GyroscopeDriftNoise 0.1001
17 LinearAccelerationNoise 0.0998
17 MagneticDisturbanceNoise 0.0998
17 LinearAccelerationDecayFactor 0.0996
17 MagneticDisturbanceDecayFactor 0.0995
18 AccelerometerNoise 0.0995
18 GyroscopeNoise 0.0992
18 MagnetometerNoise 0.0992
18 GyroscopeDriftNoise 0.0992
18 LinearAccelerationNoise 0.0990
18 MagneticDisturbanceNoise 0.0989
18 LinearAccelerationDecayFactor 0.0987
18 MagneticDisturbanceDecayFactor 0.0986
19 AccelerometerNoise 0.0986
19 GyroscopeNoise 0.0980
19 MagnetometerNoise 0.0980
19 GyroscopeDriftNoise 0.0980
19 LinearAccelerationNoise 0.0980
19 MagneticDisturbanceNoise 0.0980
19 LinearAccelerationDecayFactor 0.0978
19 MagneticDisturbanceDecayFactor 0.0975
20 AccelerometerNoise 0.0975
20 GyroscopeNoise 0.0965
20 MagnetometerNoise 0.0965
20 GyroscopeDriftNoise 0.0965
20 LinearAccelerationNoise 0.0964
20 MagneticDisturbanceNoise 0.0964
20 LinearAccelerationDecayFactor 0.0964
20 MagneticDisturbanceDecayFactor 0.0964
Отфильтруйте данные и покажите ошибку ориентации.
oEst = filt(sensorData.Accelerometer, sensorData.Gyroscope, sensorData.Magnetometer);
plotPerformance(oEst, groundTruth.Orientation, "Tuned Filter Error - Custom Cost Function");
plotPerformance Постройте ошибку ориентации
function plotPerformance(est, act, txt) % Plot the orientation error in degrees in a new figure window. figure; plot(rad2deg(dist(est, act(1:10:end)))); ylabel("Degrees") title(txt); end