tune

Tune imufilter 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 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.

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("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",ObjectiveLimit=0.03);
reset(fuse)
tune(fuse,ld.sensorData,ld.groundTruth,cfg);
    Iteration    Parameter                        Metric
    _________    _________                        ______
    1            AccelerometerNoise               0.0857
    1            GyroscopeNoise                   0.0855
    1            GyroscopeDriftNoise              0.0855
    1            LinearAccelerationNoise          0.0851
    1            LinearAccelerationDecayFactor    0.0844
    2            AccelerometerNoise               0.0844
    2            GyroscopeNoise                   0.0842
    2            GyroscopeDriftNoise              0.0842
    2            LinearAccelerationNoise          0.0840
    2            LinearAccelerationDecayFactor    0.0836
    3            AccelerometerNoise               0.0836
    3            GyroscopeNoise                   0.0834
    3            GyroscopeDriftNoise              0.0834
    3            LinearAccelerationNoise          0.0834
    3            LinearAccelerationDecayFactor    0.0831
    4            AccelerometerNoise               0.0831
    4            GyroscopeNoise                   0.0829
    4            GyroscopeDriftNoise              0.0829
    4            LinearAccelerationNoise          0.0829
    4            LinearAccelerationDecayFactor    0.0827
    5            AccelerometerNoise               0.0827
    5            GyroscopeNoise                   0.0824
    5            GyroscopeDriftNoise              0.0824
    5            LinearAccelerationNoise          0.0824
    5            LinearAccelerationDecayFactor    0.0822
    6            AccelerometerNoise               0.0822
    6            GyroscopeNoise                   0.0819
    6            GyroscopeDriftNoise              0.0819
    6            LinearAccelerationNoise          0.0819
    6            LinearAccelerationDecayFactor    0.0818
    7            AccelerometerNoise               0.0818
    7            GyroscopeNoise                   0.0814
    7            GyroscopeDriftNoise              0.0814
    7            LinearAccelerationNoise          0.0814
    7            LinearAccelerationDecayFactor    0.0813
    8            AccelerometerNoise               0.0813
    8            GyroscopeNoise                   0.0808
    8            GyroscopeDriftNoise              0.0808
    8            LinearAccelerationNoise          0.0808
    8            LinearAccelerationDecayFactor    0.0807
    9            AccelerometerNoise               0.0807
    9            GyroscopeNoise                   0.0802
    9            GyroscopeDriftNoise              0.0802
    9            LinearAccelerationNoise          0.0802
    9            LinearAccelerationDecayFactor    0.0801
    10           AccelerometerNoise               0.0801
    10           GyroscopeNoise                   0.0794
    10           GyroscopeDriftNoise              0.0794
    10           LinearAccelerationNoise          0.0794
    10           LinearAccelerationDecayFactor    0.0794
    11           AccelerometerNoise               0.0794
    11           GyroscopeNoise                   0.0785
    11           GyroscopeDriftNoise              0.0785
    11           LinearAccelerationNoise          0.0785
    11           LinearAccelerationDecayFactor    0.0785
    12           AccelerometerNoise               0.0785
    12           GyroscopeNoise                   0.0775
    12           GyroscopeDriftNoise              0.0775
    12           LinearAccelerationNoise          0.0775
    12           LinearAccelerationDecayFactor    0.0774
    13           AccelerometerNoise               0.0774
    13           GyroscopeNoise                   0.0762
    13           GyroscopeDriftNoise              0.0762
    13           LinearAccelerationNoise          0.0762
    13           LinearAccelerationDecayFactor    0.0761
    14           AccelerometerNoise               0.0761
    14           GyroscopeNoise                   0.0746
    14           GyroscopeDriftNoise              0.0746
    14           LinearAccelerationNoise          0.0746
    14           LinearAccelerationDecayFactor    0.0745
    15           AccelerometerNoise               0.0745
    15           GyroscopeNoise                   0.0727
    15           GyroscopeDriftNoise              0.0727
    15           LinearAccelerationNoise          0.0727
    15           LinearAccelerationDecayFactor    0.0726
    16           AccelerometerNoise               0.0726
    16           GyroscopeNoise                   0.0706
    16           GyroscopeDriftNoise              0.0706
    16           LinearAccelerationNoise          0.0705
    16           LinearAccelerationDecayFactor    0.0705
    17           AccelerometerNoise               0.0705
    17           GyroscopeNoise                   0.0684
    17           GyroscopeDriftNoise              0.0684
    17           LinearAccelerationNoise          0.0683
    17           LinearAccelerationDecayFactor    0.0683
    18           AccelerometerNoise               0.0683
    18           GyroscopeNoise                   0.0662
    18           GyroscopeDriftNoise              0.0662
    18           LinearAccelerationNoise          0.0662
    18           LinearAccelerationDecayFactor    0.0662
    19           AccelerometerNoise               0.0662
    19           GyroscopeNoise                   0.0644
    19           GyroscopeDriftNoise              0.0644
    19           LinearAccelerationNoise          0.0644
    19           LinearAccelerationDecayFactor    0.0644
    20           AccelerometerNoise               0.0644
    20           GyroscopeNoise                   0.0630
    20           GyroscopeDriftNoise              0.0630
    20           LinearAccelerationNoise          0.0630
    20           LinearAccelerationDecayFactor    0.0630

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 = 
4.9108

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

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

Figure contains an axes object. The axes object with title imufilter - Tuned vs Untuned Error, xlabel Time (s), ylabel Orientation Error (degrees) contains 2 objects of type line. These objects represent Untuned, Tuned.

Input Arguments

collapse all

Filter object, specified as an imufilter 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.

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