Main Content

Custom Tuning of Fusion Filters

Use the tune function to optimize the noise parameters of several fusion filters, including the ahrsfilter object. This example shows how to customize a cost function for various optimization goals.

Load Sensor Data and Ground Truth

The sensor data contains sensor recordings of a UAV executing some small maneuvers. Create an ahrsfilter object to fuse the sensor data and estimate those maneuvers.

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");

Tune the Filter to Improve Estimation

The performance of the ahrsfilter without tuning noise parameters is not ideal. Use the tune function to improve the filter performance.

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

Filter the sensor data using the tuned filter and show the orientation error.

oEstTuned = filt(sensorData.Accelerometer,sensorData.Gyroscope,sensorData.Magnetometer);
plotPerformance(oEstTuned,groundTruth.Orientation,"Tuned Filter Error - Default Configuration");

Use the CustomCostFcn and MATLAB Coder (R) to Accelerate and Optimize Tuning

The performance of the filter is improved after tuning but the tuning process can often take a long time. The tunerconfig object allows for a custom cost function to optimize this process. You can also use MATLAB Coder to create a mex function to accelerate the tuning speed. The custom cost function must have a signature cost = fcn(params,sensorData,groundTruth), where cost is a scalar real number, params is a struct of noise parameters to be optimized, sensorData is a table of sensor data, and groundTruth is a table ground truth data.

From the last section, the ahrsfilter did not estimate the orientation very well during some of the maneuvers. Instead of using the default root-mean-squared error, the custom cost function uses higher order terms to more severely penalize outliers.

Display the details of the custom cost function. The function is attached as an m-file.

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) ));

To create a mex file, you need exemplar input arguments. Create a parameters exemplar and copy the properties from 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

Generate code.

codegen customFcn.m -args {paramEx sensorData, groundTruth}
Code generation successful.

Use the mex function to tune quickly.

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

Filter the data and show the orientation error.

oEst = filt(sensorData.Accelerometer, sensorData.Gyroscope, sensorData.Magnetometer);
plotPerformance(oEst, groundTruth.Orientation, "Tuned Filter Error - Custom Cost Function");

Supporting Functions

plotPerformance Plot the orientation error

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