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 costes 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 (R) 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 coste personalizada para optimizar este proceso. También puede utilizar MATLAB Coder para crear una función mex para acelerar la velocidad de ajuste. La función de coste 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 para optimizarse, sensorData
es una tabla de datos de sensores y groundTruth
es una tabla de datos de ground-truth.
Del último tramo, 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.m
function 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
Trazar 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