tune

Tune ahrsfilter parameters to reduce estimation error

collapse all in page

Syntax

tune(filter,sensorData,groundTruth)
tune(___,config)

Description

tune(filter,sensorData,groundTruth) adjusts the properties of the ahrsfilter filter object, filter, to reduce the root-mean-squared (RMS) quaternion distance error between the fused sensor data and the ground truth. The function uses the property values in the filter as the initial estimate for the optimization algorithm.

example

tune(___,config) specifies the tuning configuration based on a tunerconfig object, config.

Examples

collapse all

Open Live Script

Load recorded sensor data and ground truth data.

ld = load('ahrsfilterTuneData.mat');
qTrue = ld.groundTruth.Orientation; % true orientation

Create an arhsfitler object.

fuse = ahrsfilter;

Fuse the sensor data using the default, untuned filter.

qEstUntuned = fuse(ld.sensorData.Accelerometer, ...
    ld.sensorData.Gyroscope, ld.sensorData.Magnetometer);

Create a tunerconfig object. Tune the ahrsfilter object to improve the orientation estimation based on the configuration.

config = tunerconfig('ahrsfilter');
tune(fuse,ld.sensorData,ld.groundTruth,config);
    Iteration    Parameter                         Metric
    _________    _________                         ______
    1            AccelerometerNoise                0.1345
    1            GyroscopeNoise                    0.1342
    1            MagnetometerNoise                 0.1341
    1            GyroscopeDriftNoise               0.1341
    1            LinearAccelerationNoise           0.1332
    1            MagneticDisturbanceNoise          0.1324
    1            LinearAccelerationDecayFactor     0.1317
    1            MagneticDisturbanceDecayFactor    0.1316
    2            AccelerometerNoise                0.1316
    2            GyroscopeNoise                    0.1312
    2            MagnetometerNoise                 0.1311
    2            GyroscopeDriftNoise               0.1311
    2            LinearAccelerationNoise           0.1300
    2            MagneticDisturbanceNoise          0.1292
    2            LinearAccelerationDecayFactor     0.1285
    2            MagneticDisturbanceDecayFactor    0.1285
    3            AccelerometerNoise                0.1285
    3            GyroscopeNoise                    0.1280
    3            MagnetometerNoise                 0.1279
    3            GyroscopeDriftNoise               0.1279
    3            LinearAccelerationNoise           0.1267
    3            MagneticDisturbanceNoise          0.1258
    3            LinearAccelerationDecayFactor     0.1253
    3            MagneticDisturbanceDecayFactor    0.1253
    4            AccelerometerNoise                0.1252
    4            GyroscopeNoise                    0.1247
    4            MagnetometerNoise                 0.1246
    4            GyroscopeDriftNoise               0.1246
    4            LinearAccelerationNoise           0.1233
    4            MagneticDisturbanceNoise          0.1224
    4            LinearAccelerationDecayFactor     0.1220
    4            MagneticDisturbanceDecayFactor    0.1220
    5            AccelerometerNoise                0.1220
    5            GyroscopeNoise                    0.1213
    5            MagnetometerNoise                 0.1212
    5            GyroscopeDriftNoise               0.1212
    5            LinearAccelerationNoise           0.1200
    5            MagneticDisturbanceNoise          0.1190
    5            LinearAccelerationDecayFactor     0.1187
    5            MagneticDisturbanceDecayFactor    0.1187
    6            AccelerometerNoise                0.1187
    6            GyroscopeNoise                    0.1180
    6            MagnetometerNoise                 0.1178
    6            GyroscopeDriftNoise               0.1178
    6            LinearAccelerationNoise           0.1167
    6            MagneticDisturbanceNoise          0.1156
    6            LinearAccelerationDecayFactor     0.1155
    6            MagneticDisturbanceDecayFactor    0.1155
    7            AccelerometerNoise                0.1155
    7            GyroscopeNoise                    0.1147
    7            MagnetometerNoise                 0.1145
    7            GyroscopeDriftNoise               0.1145
    7            LinearAccelerationNoise           0.1137
    7            MagneticDisturbanceNoise          0.1126
    7            LinearAccelerationDecayFactor     0.1125
    7            MagneticDisturbanceDecayFactor    0.1125
    8            AccelerometerNoise                0.1125
    8            GyroscopeNoise                    0.1117
    8            MagnetometerNoise                 0.1116
    8            GyroscopeDriftNoise               0.1116
    8            LinearAccelerationNoise           0.1112
    8            MagneticDisturbanceNoise          0.1100
    8            LinearAccelerationDecayFactor     0.1099
    8            MagneticDisturbanceDecayFactor    0.1099
    9            AccelerometerNoise                0.1099
    9            GyroscopeNoise                    0.1091
    9            MagnetometerNoise                 0.1090
    9            GyroscopeDriftNoise               0.1090
    9            LinearAccelerationNoise           0.1090
    9            MagneticDisturbanceNoise          0.1076
    9            LinearAccelerationDecayFactor     0.1075
    9            MagneticDisturbanceDecayFactor    0.1075
    10           AccelerometerNoise                0.1075
    10           GyroscopeNoise                    0.1066
    10           MagnetometerNoise                 0.1064
    10           GyroscopeDriftNoise               0.1064
    10           LinearAccelerationNoise           0.1064
    10           MagneticDisturbanceNoise          0.1049
    10           LinearAccelerationDecayFactor     0.1047
    10           MagneticDisturbanceDecayFactor    0.1047
    11           AccelerometerNoise                0.1047
    11           GyroscopeNoise                    0.1038
    11           MagnetometerNoise                 0.1036
    11           GyroscopeDriftNoise               0.1036
    11           LinearAccelerationNoise           0.1036
    11           MagneticDisturbanceNoise          0.1016
    11           LinearAccelerationDecayFactor     0.1014
    11           MagneticDisturbanceDecayFactor    0.1014
    12           AccelerometerNoise                0.1014
    12           GyroscopeNoise                    0.1005
    12           MagnetometerNoise                 0.1002
    12           GyroscopeDriftNoise               0.1002
    12           LinearAccelerationNoise           0.1002
    12           MagneticDisturbanceNoise          0.0978

Fuse the sensor data using the tuned filter.

qEstTuned = fuse(ld.sensorData.Accelerometer, ...
    ld.sensorData.Gyroscope, ld.sensorData.Magnetometer);

Compare the tuned and untuned RMS error performances.

dUntuned = rad2deg(dist(qEstUntuned, qTrue));
dTuned = rad2deg(dist(qEstTuned, qTrue));
rmsUntuned = sqrt(mean(dUntuned.^2))
rmsUntuned = 7.7088

rmsTuned = sqrt(mean(dTuned.^2))
rmsTuned = 5.6033

Visualize the errors with respect to time.

N = numel(dUntuned);
t = (0:N-1)./ fuse.SampleRate;
plot(t, dUntuned, 'r', t, dTuned, 'b');
legend('Untuned', 'Tuned');
title('ahrsfilter - Tuned vs Untuned Error')
xlabel('Time (s)');
ylabel('Orientation Error (degrees)');

Input Arguments

collapse all

Filter object, specified as an ahrsfilter object.

Sensor data, specified as a table. In each row, the sensor data is specified as:

  • Accelerometer — Accelerometer data, specified as a 1-by-3 vector of scalars in m2/s.

  • Gyroscope — Gyroscope data, specified as a 1-by-3 vector of scalars in rad/s.

  • Magnetometer — Magnetometer data, specified as a 1-by-3 vector of scalars in μT.

If you set the Cost property of the tuner configuration input, config, to Custom, then you can use other data types for the sensorData input based on your choice.

Ground truth data, specified as a table. The table has only one column of Orientation data. In each row, the orientation is specified as a quaternion object or a 3-by-3 rotation matrix.

The function processes each row of the sensorData and groundTruth tables sequentially to calculate the state estimate and RMS error from the ground truth. Each row of the sensorData and the groundTruth tables must correspond to each other.

If you set the Cost property of the tuner configuration input, config, to Custom, then you can use other data types for the groundTruth input based on your choice.

Tuner configuration, specified as a tunerconfig object.

References

[1] Abbeel, P., Coates, A., Montemerlo, M., Ng, A.Y. and Thrun, S. Discriminative Training of Kalman Filters. In Robotics: Science and systems, Vol. 2, pp. 1, 2005.

Version History

Introduced in R2020b