Настраиваемая настройка фильтров слияния

Используйте 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");

Используйте CustomCostFcn и MATLAB Coder(R), чтобы ускорить и оптимизировать настройку

Эффективность фильтра улучшается после настройки, но процесс настройки часто может занять много времени. 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
Для просмотра документации необходимо авторизоваться на сайте