Используйте tune
функция для оптимизации параметров шума нескольких фильтров плавления, включая ahrsfilter
объект. В этом примере показано, как настроить функцию затрат для различных целей оптимизации.
Данные о датчике содержат записи датчика БПЛА, выполняющего некоторые небольшие маневры. Создайте 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");
Эффективность фильтра улучшается после настройки, но процесс настройки часто может занять много времени. The tunerconfig
объект позволяет использовать пользовательскую функцию затрат для оптимизации этого процесса. Можно также использовать MATLAB Coder, чтобы создать MEX-функцию для ускорения скорости настройки. Функция пользовательских затрат должна иметь подпись cost = fcn(params,sensorData,groundTruth)
, где cost
является скалярным вещественным числом, params
- struct параметров шума, которая должна быть оптимизирована, sensorData
представляет собой таблицу данных о датчике и groundTruth
- таблица, основанная на данных истинности.
Из последнего раздела, ahrsfilter
не очень хорошо оценивал ориентацию во время некоторых маневров. Вместо использования ошибки по умолчанию root-mean-quared, пользовательская функция затрат использует условия более высокого порядка для более строгого наказания выбросов.
Отображение подробных данных пользовательской функции затрат. Функция присоединена как 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) ));
Чтобы создать файл 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