tune

Tune imufilter parameters to reduce estimation error

Syntax

tune(filter,sensorData,groundTruth)

tune(___,config)

Description

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

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

Examples

Tune imufilter to Optimize Orientation Estimate

Load recorded sensor data and ground truth data.

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

Create an imufilter object and fuse the filter with the sensor data.

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

Create a tunerconfig object and tune the imufilter to improve the orientation estimate.

cfg = tunerconfig('imufilter');
tune(fuse, ld.sensorData, ld.groundTruth, cfg);

    Iteration    Parameter                        Metric
    _________    _________                        ______
    1            AccelerometerNoise               0.1149
    1            GyroscopeNoise                   0.1146
    1            GyroscopeDriftNoise              0.1146
    1            LinearAccelerationNoise          0.1122
    1            LinearAccelerationDecayFactor    0.1103
    2            AccelerometerNoise               0.1102
    2            GyroscopeNoise                   0.1098
    2            GyroscopeDriftNoise              0.1098
    2            LinearAccelerationNoise          0.1070
    2            LinearAccelerationDecayFactor    0.1053
    3            AccelerometerNoise               0.1053
    3            GyroscopeNoise                   0.1048
    3            GyroscopeDriftNoise              0.1048
    3            LinearAccelerationNoise          0.1016
    3            LinearAccelerationDecayFactor    0.1002
    4            AccelerometerNoise               0.1001
    4            GyroscopeNoise                   0.0996
    4            GyroscopeDriftNoise              0.0996
    4            LinearAccelerationNoise          0.0962
    4            LinearAccelerationDecayFactor    0.0950
    5            AccelerometerNoise               0.0950
    5            GyroscopeNoise                   0.0943
    5            GyroscopeDriftNoise              0.0943
    5            LinearAccelerationNoise          0.0910
    5            LinearAccelerationDecayFactor    0.0901
    6            AccelerometerNoise               0.0900
    6            GyroscopeNoise                   0.0893
    6            GyroscopeDriftNoise              0.0893
    6            LinearAccelerationNoise          0.0862
    6            LinearAccelerationDecayFactor    0.0855
    7            AccelerometerNoise               0.0855
    7            GyroscopeNoise                   0.0848
    7            GyroscopeDriftNoise              0.0848
    7            LinearAccelerationNoise          0.0822
    7            LinearAccelerationDecayFactor    0.0818
    8            AccelerometerNoise               0.0817
    8            GyroscopeNoise                   0.0811
    8            GyroscopeDriftNoise              0.0811
    8            LinearAccelerationNoise          0.0791
    8            LinearAccelerationDecayFactor    0.0789
    9            AccelerometerNoise               0.0788
    9            GyroscopeNoise                   0.0782
    9            GyroscopeDriftNoise              0.0782
    9            LinearAccelerationNoise          0.0769
    9            LinearAccelerationDecayFactor    0.0768
    10           AccelerometerNoise               0.0768
    10           GyroscopeNoise                   0.0762
    10           GyroscopeDriftNoise              0.0762
    10           LinearAccelerationNoise          0.0754
    10           LinearAccelerationDecayFactor    0.0753
    11           AccelerometerNoise               0.0753
    11           GyroscopeNoise                   0.0747
    11           GyroscopeDriftNoise              0.0747
    11           LinearAccelerationNoise          0.0741
    11           LinearAccelerationDecayFactor    0.0740
    12           AccelerometerNoise               0.0740
    12           GyroscopeNoise                   0.0734
    12           GyroscopeDriftNoise              0.0734
    12           LinearAccelerationNoise          0.0728
    12           LinearAccelerationDecayFactor    0.0728
    13           AccelerometerNoise               0.0728
    13           GyroscopeNoise                   0.0721
    13           GyroscopeDriftNoise              0.0721
    13           LinearAccelerationNoise          0.0715
    13           LinearAccelerationDecayFactor    0.0715
    14           AccelerometerNoise               0.0715
    14           GyroscopeNoise                   0.0706
    14           GyroscopeDriftNoise              0.0706
    14           LinearAccelerationNoise          0.0700
    14           LinearAccelerationDecayFactor    0.0700
    15           AccelerometerNoise               0.0700
    15           GyroscopeNoise                   0.0690
    15           GyroscopeDriftNoise              0.0690
    15           LinearAccelerationNoise          0.0684
    15           LinearAccelerationDecayFactor    0.0684
    16           AccelerometerNoise               0.0684
    16           GyroscopeNoise                   0.0672
    16           GyroscopeDriftNoise              0.0672
    16           LinearAccelerationNoise          0.0668
    16           LinearAccelerationDecayFactor    0.0667
    17           AccelerometerNoise               0.0667
    17           GyroscopeNoise                   0.0655
    17           GyroscopeDriftNoise              0.0655
    17           LinearAccelerationNoise          0.0654
    17           LinearAccelerationDecayFactor    0.0654
    18           AccelerometerNoise               0.0654
    18           GyroscopeNoise                   0.0641
    18           GyroscopeDriftNoise              0.0641
    18           LinearAccelerationNoise          0.0640
    18           LinearAccelerationDecayFactor    0.0639
    19           AccelerometerNoise               0.0639
    19           GyroscopeNoise                   0.0627
    19           GyroscopeDriftNoise              0.0627
    19           LinearAccelerationNoise          0.0627
    19           LinearAccelerationDecayFactor    0.0624
    20           AccelerometerNoise               0.0624
    20           GyroscopeNoise                   0.0614
    20           GyroscopeDriftNoise              0.0614
    20           LinearAccelerationNoise          0.0613
    20           LinearAccelerationDecayFactor    0.0613

Fuse the sensor data again using the tuned filter.

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

Compare the tuned and untuned filter RMS error performances.

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

rmsUntuned = 6.5864

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

rmsTuned = 3.5098

Visualize the results.

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

Input Arguments

filter — Filter object

imufilter object

Filter object, specified as an imufilter object.

sensorData — Sensor data

table

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 m²/s.

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

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.

groundTruth — Ground truth data

timetable

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.

config — Tuner configuration

tunerconfig object

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.

Introduced in R2020b

×

Open Example

A modified version of this example exists on your system. Do you want to open this version instead?

No, overwrite the modified version Yes