tune

Tune insEKF parameters to reduce estimation error

Since R2022a

collapse all in page

Syntax

tunedMeasureNoise = tune(filter,measureNoise,sensorData,groundTruth)
tunedMeasureNoise = tune(___,config)

Description

tunedMeasureNoise = tune(filter,measureNoise,sensorData,groundTruth) tunes the AdditiveProcessNoise property of the insEKF filter object filter, and the measurement noise, to reduce the root-mean-squared (RMS) state estimation error between the fused sensor data and the ground truth. The function also returns the tuned measurement noise tunedMeasureNoise. The function uses the property values in the filter and the measurement noise provided in the measureNoise structure as the initial estimate for the optimization algorithm.

tunedMeasureNoise = tune(___,config) specifies the tuning configuration using a tunerconfig object config, in addition to all input arguments from the previous syntax.

example

Examples

collapse all

Open Live Script

Load the recorded sensor data and ground truth data.

load("accelGyroINSEKFData.mat");

Create an insEKF filter object. Specify the orientation part of the state in the filter using the initial orientation from the measurement data. Specify the diagonal elements of the state estimate error covariance matrix corresponding to the orientation state as 0.01.

filt = insEKF;
stateparts(filt,"Orientation",compact(initOrient));
statecovparts(filt,"Orientation",1e-2);

Obtain a representative measurement noise structure and use it to estimate states before tuning.

mnoise = tunernoise(filt);
untunedEst = estimateStates(filt,sensorData,mnoise);

Reinitialize the filter, set up a tunerconfig object, and tune the filter.

stateparts(filt,"Orientation",compact(initOrient));
statecovparts(filt,"Orientation",1e-2);
cfg = tunerconfig(filt,MaxIterations=10,ObjectiveLimit=1e-4);
tunedmn = tune(filt,mnoise,sensorData,groundTruth,cfg);
    Iteration    Parameter                    Metric
    _________    _________                    ______
    1            AdditiveProcessNoise(1)      0.3787
    1            AdditiveProcessNoise(15)     0.3761
    1            AdditiveProcessNoise(29)     0.3695
    1            AdditiveProcessNoise(43)     0.3655
    1            AdditiveProcessNoise(57)     0.3533
    1            AdditiveProcessNoise(71)     0.3446
    1            AdditiveProcessNoise(85)     0.3431
    1            AdditiveProcessNoise(99)     0.3428
    1            AdditiveProcessNoise(113)    0.3427
    1            AdditiveProcessNoise(127)    0.3426
    1            AdditiveProcessNoise(141)    0.3298
    1            AdditiveProcessNoise(155)    0.3206
    1            AdditiveProcessNoise(169)    0.3200
    1            AccelerometerNoise           0.3199
    1            GyroscopeNoise               0.3198
    2            AdditiveProcessNoise(1)      0.3126
    2            AdditiveProcessNoise(15)     0.3098
    2            AdditiveProcessNoise(29)     0.3018
    2            AdditiveProcessNoise(43)     0.2988
    2            AdditiveProcessNoise(57)     0.2851
    2            AdditiveProcessNoise(71)     0.2784
    2            AdditiveProcessNoise(85)     0.2760
    2            AdditiveProcessNoise(99)     0.2744
    2            AdditiveProcessNoise(113)    0.2744
    2            AdditiveProcessNoise(127)    0.2743
    2            AdditiveProcessNoise(141)    0.2602
    2            AdditiveProcessNoise(155)    0.2537
    2            AdditiveProcessNoise(169)    0.2527
    2            AccelerometerNoise           0.2524
    2            GyroscopeNoise               0.2524
    3            AdditiveProcessNoise(1)      0.2476
    3            AdditiveProcessNoise(15)     0.2432
    3            AdditiveProcessNoise(29)     0.2397
    3            AdditiveProcessNoise(43)     0.2381
    3            AdditiveProcessNoise(57)     0.2255
    3            AdditiveProcessNoise(71)     0.2226
    3            AdditiveProcessNoise(85)     0.2221
    3            AdditiveProcessNoise(99)     0.2202
    3            AdditiveProcessNoise(113)    0.2201
    3            AdditiveProcessNoise(127)    0.2201
    3            AdditiveProcessNoise(141)    0.2090
    3            AdditiveProcessNoise(155)    0.2070
    3            AdditiveProcessNoise(169)    0.2058
    3            AccelerometerNoise           0.2052
    3            GyroscopeNoise               0.2052
    4            AdditiveProcessNoise(1)      0.2051
    4            AdditiveProcessNoise(15)     0.2027
    4            AdditiveProcessNoise(29)     0.2019
    4            AdditiveProcessNoise(43)     0.2000
    4            AdditiveProcessNoise(57)     0.1909
    4            AdditiveProcessNoise(71)     0.1897
    4            AdditiveProcessNoise(85)     0.1882
    4            AdditiveProcessNoise(99)     0.1871
    4            AdditiveProcessNoise(113)    0.1870
    4            AdditiveProcessNoise(127)    0.1870
    4            AdditiveProcessNoise(141)    0.1791
    4            AdditiveProcessNoise(155)    0.1783
    4            AdditiveProcessNoise(169)    0.1751
    4            AccelerometerNoise           0.1748
    4            GyroscopeNoise               0.1747
    5            AdditiveProcessNoise(1)      0.1742
    5            AdditiveProcessNoise(15)     0.1732
    5            AdditiveProcessNoise(29)     0.1712
    5            AdditiveProcessNoise(43)     0.1712
    5            AdditiveProcessNoise(57)     0.1626
    5            AdditiveProcessNoise(71)     0.1615
    5            AdditiveProcessNoise(85)     0.1598
    5            AdditiveProcessNoise(99)     0.1590
    5            AdditiveProcessNoise(113)    0.1589
    5            AdditiveProcessNoise(127)    0.1589
    5            AdditiveProcessNoise(141)    0.1517
    5            AdditiveProcessNoise(155)    0.1508
    5            AdditiveProcessNoise(169)    0.1476
    5            AccelerometerNoise           0.1473
    5            GyroscopeNoise               0.1470
    6            AdditiveProcessNoise(1)      0.1470
    6            AdditiveProcessNoise(15)     0.1470
    6            AdditiveProcessNoise(29)     0.1463
    6            AdditiveProcessNoise(43)     0.1462
    6            AdditiveProcessNoise(57)     0.1367
    6            AdditiveProcessNoise(71)     0.1360
    6            AdditiveProcessNoise(85)     0.1360
    6            AdditiveProcessNoise(99)     0.1350
    6            AdditiveProcessNoise(113)    0.1350
    6            AdditiveProcessNoise(127)    0.1350
    6            AdditiveProcessNoise(141)    0.1289
    6            AdditiveProcessNoise(155)    0.1288
    6            AdditiveProcessNoise(169)    0.1262
    6            AccelerometerNoise           0.1253
    6            GyroscopeNoise               0.1246
    7            AdditiveProcessNoise(1)      0.1246
    7            AdditiveProcessNoise(15)     0.1244
    7            AdditiveProcessNoise(29)     0.1205
    7            AdditiveProcessNoise(43)     0.1203
    7            AdditiveProcessNoise(57)     0.1125
    7            AdditiveProcessNoise(71)     0.1122
    7            AdditiveProcessNoise(85)     0.1117
    7            AdditiveProcessNoise(99)     0.1106
    7            AdditiveProcessNoise(113)    0.1104
    7            AdditiveProcessNoise(127)    0.1104
    7            AdditiveProcessNoise(141)    0.1058
    7            AdditiveProcessNoise(155)    0.1052
    7            AdditiveProcessNoise(169)    0.1035
    7            AccelerometerNoise           0.1024
    7            GyroscopeNoise               0.1014
    8            AdditiveProcessNoise(1)      0.1014
    8            AdditiveProcessNoise(15)     0.1012
    8            AdditiveProcessNoise(29)     0.1012
    8            AdditiveProcessNoise(43)     0.1005
    8            AdditiveProcessNoise(57)     0.0948
    8            AdditiveProcessNoise(71)     0.0948
    8            AdditiveProcessNoise(85)     0.0938
    8            AdditiveProcessNoise(99)     0.0934
    8            AdditiveProcessNoise(113)    0.0931
    8            AdditiveProcessNoise(127)    0.0931
    8            AdditiveProcessNoise(141)    0.0896
    8            AdditiveProcessNoise(155)    0.0889
    8            AdditiveProcessNoise(169)    0.0867
    8            AccelerometerNoise           0.0859
    8            GyroscopeNoise               0.0851
    9            AdditiveProcessNoise(1)      0.0851
    9            AdditiveProcessNoise(15)     0.0850
    9            AdditiveProcessNoise(29)     0.0824
    9            AdditiveProcessNoise(43)     0.0819
    9            AdditiveProcessNoise(57)     0.0771
    9            AdditiveProcessNoise(71)     0.0771
    9            AdditiveProcessNoise(85)     0.0762
    9            AdditiveProcessNoise(99)     0.0759
    9            AdditiveProcessNoise(113)    0.0754
    9            AdditiveProcessNoise(127)    0.0754
    9            AdditiveProcessNoise(141)    0.0734
    9            AdditiveProcessNoise(155)    0.0724
    9            AdditiveProcessNoise(169)    0.0702
    9            AccelerometerNoise           0.0697
    9            GyroscopeNoise               0.0689
    10           AdditiveProcessNoise(1)      0.0689
    10           AdditiveProcessNoise(15)     0.0686
    10           AdditiveProcessNoise(29)     0.0658
    10           AdditiveProcessNoise(43)     0.0655
    10           AdditiveProcessNoise(57)     0.0622
    10           AdditiveProcessNoise(71)     0.0620
    10           AdditiveProcessNoise(85)     0.0616
    10           AdditiveProcessNoise(99)     0.0615
    10           AdditiveProcessNoise(113)    0.0607
    10           AdditiveProcessNoise(127)    0.0606
    10           AdditiveProcessNoise(141)    0.0590
    10           AdditiveProcessNoise(155)    0.0578
    10           AdditiveProcessNoise(169)    0.0565
    10           AccelerometerNoise           0.0562
    10           GyroscopeNoise               0.0557

Estimate states again, this time using the tuned filter.

tunedEst = estimateStates(filt,sensorData,tunedmn);

Compare the tuned and untuned estimates against the ground truth data.

times = groundTruth.Properties.RowTimes;
duntuned = rad2deg(dist(untunedEst.Orientation,groundTruth.Orientation));
dtuned = rad2deg(dist(tunedEst.Orientation,groundTruth.Orientation));
plot(times,duntuned,times,dtuned);
xlabel("Time (sec)")
ylabel("Error (deg)")
legend("Untuned","Tuned")
title("Filter Orientation Error")

Figure contains an axes object. The axes object with title Filter Orientation Error, xlabel Time (sec), ylabel Error (deg) contains 2 objects of type line. These objects represent Untuned, Tuned.

Print the root-mean-squared (RMS) error of both the untuned and the tuned filters.

untunedRMSError = sqrt(mean(duntuned.^2));
tunedRMSError = sqrt(mean(dtuned.^2));
fprintf("Untuned RMS error: %.2f degrees\n", ...
    untunedRMSError);
Untuned RMS error: 39.47 degrees

fprintf("Tuned RMS error: %.2f degrees\n", ...
    tunedRMSError);
Tuned RMS error: 6.39 degrees

Input Arguments

collapse all

INS filter, specified as an insEKF object.

Measurement noise, specified as a structure. The function uses the measurement noise input as the initial guess for tuning the measurement noise. The structure should contain the measurement noise for sensor models specified in the Sensors property of the INS filter. For example, if the insEKF filter object only contains an insAccelerometer object and an insGyroscope object, you should specify the structure like this:

Field nameDescription
AccelerometerNoiseVariance of accelerometer noise, specified as a scalar in (m/s2)2.
GyroscopeNoiseVariance of gyroscope noise, specified as a scalar in (rad/s)2.

Tip

Use the tunernoise function to obtain a representative structure for the measureNoise structure. For example:

filter = insEKF;
mNoise = tunerNoise(filter)

Sensor data, specified as a timetable. Each variable name (as a column) in the time table must match one of the sensor names specified in the SensorNames property of the filter. Each entry in the table is the measurement from the sensor at the corresponding row time.

If a sensor does not produce measurements at the row time, specify the corresponding entry as NaN.

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 timetable. In each row, the table contains the truth data for the row time. Each variable name (as a column) in the table must be one of the filter state names that you can obtain using the stateinfo object function.

The function processes each row of the sensorData and groundTruth tables sequentially to calculate the state estimate and RMS error from the ground truth. State variables not present in groundTruth input are ignored for the comparison. The sensorData and the groundTruth tables must have the same row times.

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.

Output Arguments

collapse all

Tuned measurement noise, returned as a structure. The structure contains the same fields as the structure specified in the measureNoise input.

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 R2022a

See Also

| | | | | | | | | | |