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