Ajuste personalizado de filtros Fusion
Utilice la función tune para optimizar los parámetros de ruido de varios filtros de fusión, incluido el objeto ahrsfilter. Este ejemplo muestra cómo personalizar una función de costo para varios objetivos de optimización.
Cargar datos del sensor y de ground-truth
Los datos del sensor contienen grabaciones de sensores de un VANT ejecutando algunas pequeñas maniobras. Cree un objeto ahrsfilter para fusionar los datos del sensor y estimar esas maniobras.
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");
Ajuste el filtro para mejorar la estimación
El rendimiento del ahrsfilter sin ajustar los parámetros de ruido no es ideal. Utilice la función tune para mejorar el rendimiento del filtro.
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
Filtre los datos del sensor utilizando el filtro ajustado y muestre el error de orientación.
oEstTuned = filt(sensorData.Accelerometer,sensorData.Gyroscope,sensorData.Magnetometer);
plotPerformance(oEstTuned,groundTruth.Orientation,"Tuned Filter Error - Default Configuration");
Utilice CustomCostFcn y MATLAB Coder para acelerar y optimizar el ajuste
El rendimiento del filtro mejora después del ajuste, pero el proceso de ajuste a menudo puede llevar mucho tiempo. El objeto tunerconfig permite una función de costo personalizada para optimizar este proceso. También puedes usar MATLAB ® Coder™ para crear una función mex para acelerar la velocidad de ajuste. La función de costo personalizada debe tener una firma cost = fcn(params,sensorData,groundTruth), donde cost es un número real escalar, params es una estructura de parámetros de ruido a optimizar, sensorData es una tabla de datos de sensores y groundTruth es una tabla de datos de ground-truth .
A partir del último apartado, el ahrsfilter no estimó muy bien la orientación durante algunas de las maniobras. En lugar de utilizar el error cuadrático medio predeterminado, la función de coste personalizada utiliza términos de orden superior para penalizar más severamente los valores atípicos.
Muestra los detalles de la función de coste personalizada. La función se adjunta como un archivo 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) ));
Para crear un archivo mex, necesita argumentos de entrada ejemplares. Cree un ejemplo de parámetros y copie las propiedades de 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
Generar codigo.
codegen customFcn.m -args {paramEx sensorData, groundTruth}
Code generation successful.
Utilice la función mex para ajustar rápidamente.
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
Filtra los datos y muestra el error de orientación.
oEst = filt(sensorData.Accelerometer, sensorData.Gyroscope, sensorData.Magnetometer);
plotPerformance(oEst, groundTruth.Orientation, "Tuned Filter Error - Custom Cost Function");
Funciones de apoyo
plotPerformance Grafique el error de orientación
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
