tune

Tune insfilterNonholonomic parameters to reduce estimation error

Syntax

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

tunedMeasureNoise = tune(___,config)

Description

tunedMeasureNoise = tune(filter,measureNoise,sensorData,groundTruth) adjusts the properties of the insfilterNonholonomic filter object, filter, and measurement noises 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 based on a tunerconfig object, config.

Examples

Tune insfilterNonholonomic to Optimize Pose Estimate

Load the recorded sensor data and ground truth data.

load('insfilterNonholonomicTuneData.mat');

Create tables for the sensor data and the truth data.

sensorData = table(Accelerometer, Gyroscope, ...
        GPSPosition, GPSVelocity);
groundTruth = table(Orientation, Position);

Create an insfilterNonholonimic filter object.

filter = insfilterNonholonomic('State', initialState, ...
        'StateCovariance', initialStateCovariance, ...
        'DecimationFactor', 1);

Create a tuner configuration object for the filter. Set the maximum number of iterations to 30.

config = tunerconfig('insfilterNonholonomic','MaxIterations',30);

Use the tunernoise function to obtain a set of initial sensor noises used in the filter.

measNoise = tunernoise('insfilterNonholonomic')

measNoise = struct with fields:
    GPSPositionNoise: 1
    GPSVelocityNoise: 1

Tune the filter and obtain the tuned measurement noise.

tunedNoise = tune(filter,measNoise,sensorData,groundTruth,config);

    Iteration    Parameter                       Metric
    _________    _________                       ______
    1            GyroscopeNoise                  3.4877
    1            AccelerometerNoise              3.3961
    1            GyroscopeBiasNoise              3.3961
    1            GyroscopeBiasDecayFactor        3.3961
    1            AccelerometerBiasNoise          3.3961
    1            AccelerometerBiasDecayFactor    3.3961
    1            ZeroVelocityConstraintNoise     3.3935
    1            GPSPositionNoise                3.2848
    1            GPSVelocityNoise                3.2798
    2            GyroscopeNoise                  3.2641
    2            AccelerometerNoise              3.1715
    2            GyroscopeBiasNoise              3.1715
    2            GyroscopeBiasDecayFactor        2.9661
    2            AccelerometerBiasNoise          2.9661
    2            AccelerometerBiasDecayFactor    2.9661
    2            ZeroVelocityConstraintNoise     2.9617
    2            GPSPositionNoise                2.8438
    2            GPSVelocityNoise                2.8384
    3            GyroscopeNoise                  2.8373
    3            AccelerometerNoise              2.7382
    3            GyroscopeBiasNoise              2.7382
    3            GyroscopeBiasDecayFactor        2.7382
    3            AccelerometerBiasNoise          2.7382
    3            AccelerometerBiasDecayFactor    2.7382
    3            ZeroVelocityConstraintNoise     2.7335
    3            GPSPositionNoise                2.6105
    3            GPSVelocityNoise                2.6045
    4            GyroscopeNoise                  2.6023
    4            AccelerometerNoise              2.5001
    4            GyroscopeBiasNoise              2.5001
    4            GyroscopeBiasDecayFactor        2.5001
    4            AccelerometerBiasNoise          2.5001
    4            AccelerometerBiasDecayFactor    2.5001
    4            ZeroVelocityConstraintNoise     2.4953
    4            GPSPositionNoise                2.3692
    4            GPSVelocityNoise                2.3626
    5            GyroscopeNoise                  2.3595
    5            AccelerometerNoise              2.2561
    5            GyroscopeBiasNoise              2.2561
    5            GyroscopeBiasDecayFactor        2.2508
    5            AccelerometerBiasNoise          2.2508
    5            AccelerometerBiasDecayFactor    2.2508
    5            ZeroVelocityConstraintNoise     2.2469
    5            GPSPositionNoise                2.1265
    5            GPSVelocityNoise                2.1191
    6            GyroscopeNoise                  2.1148
    6            AccelerometerNoise              2.0150
    6            GyroscopeBiasNoise              2.0150
    6            GyroscopeBiasDecayFactor        2.0150
    6            AccelerometerBiasNoise          2.0150
    6            AccelerometerBiasDecayFactor    2.0150
    6            ZeroVelocityConstraintNoise     2.0116
    6            GPSPositionNoise                1.8970
    6            GPSVelocityNoise                1.8888
    7            GyroscopeNoise                  1.8847
    7            AccelerometerNoise              1.7921
    7            GyroscopeBiasNoise              1.7921
    7            GyroscopeBiasDecayFactor        1.7845
    7            AccelerometerBiasNoise          1.7845
    7            AccelerometerBiasDecayFactor    1.7845
    7            ZeroVelocityConstraintNoise     1.7815
    7            GPSPositionNoise                1.6794
    7            GPSVelocityNoise                1.6708
    8            GyroscopeNoise                  1.6679
    8            AccelerometerNoise              1.5886
    8            GyroscopeBiasNoise              1.5886
    8            GyroscopeBiasDecayFactor        1.5866
    8            AccelerometerBiasNoise          1.5866
    8            AccelerometerBiasDecayFactor    1.5866
    8            ZeroVelocityConstraintNoise     1.5850
    8            GPSPositionNoise                1.5057
    8            GPSVelocityNoise                1.4965
    9            GyroscopeNoise                  1.4950
    9            AccelerometerNoise              1.4364
    9            GyroscopeBiasNoise              1.4364
    9            GyroscopeBiasDecayFactor        1.4364
    9            AccelerometerBiasNoise          1.4364
    9            AccelerometerBiasDecayFactor    1.4364
    9            ZeroVelocityConstraintNoise     1.4355
    9            GPSPositionNoise                1.3894
    9            GPSVelocityNoise                1.3790
    10           GyroscopeNoise                  1.3773
    10           AccelerometerNoise              1.3422
    10           GyroscopeBiasNoise              1.3422
    10           GyroscopeBiasDecayFactor        1.3421
    10           AccelerometerBiasNoise          1.3421
    10           AccelerometerBiasDecayFactor    1.3421
    10           ZeroVelocityConstraintNoise     1.3399
    10           GPSPositionNoise                1.3319
    10           GPSVelocityNoise                1.3190
    11           GyroscopeNoise                  1.3159
    11           AccelerometerNoise              1.3102
    11           GyroscopeBiasNoise              1.3102
    11           GyroscopeBiasDecayFactor        1.3100
    11           AccelerometerBiasNoise          1.3100
    11           AccelerometerBiasDecayFactor    1.3100
    11           ZeroVelocityConstraintNoise     1.3069
    11           GPSPositionNoise                1.2964
    11           GPSVelocityNoise                1.2762
    12           GyroscopeNoise                  1.2740
    12           AccelerometerNoise              1.2655
    12           GyroscopeBiasNoise              1.2655
    12           GyroscopeBiasDecayFactor        1.2641
    12           AccelerometerBiasNoise          1.2641
    12           AccelerometerBiasDecayFactor    1.2641
    12           ZeroVelocityConstraintNoise     1.2631
    12           GPSPositionNoise                1.2511
    12           GPSVelocityNoise                1.2198
    13           GyroscopeNoise                  1.2184
    13           AccelerometerNoise              1.2058
    13           GyroscopeBiasNoise              1.2058
    13           GyroscopeBiasDecayFactor        1.2029
    13           AccelerometerBiasNoise          1.2029
    13           AccelerometerBiasDecayFactor    1.2029
    13           ZeroVelocityConstraintNoise     1.2029
    13           GPSPositionNoise                1.1874
    13           GPSVelocityNoise                1.1408
    14           GyroscopeNoise                  1.1403
    14           AccelerometerNoise              1.1236
    14           GyroscopeBiasNoise              1.1236
    14           GyroscopeBiasDecayFactor        1.1186
    14           AccelerometerBiasNoise          1.1186
    14           AccelerometerBiasDecayFactor    1.1186
    14           ZeroVelocityConstraintNoise     1.1183
    14           GPSPositionNoise                1.0975
    14           GPSVelocityNoise                1.0348
    15           GyroscopeNoise                  1.0347
    15           AccelerometerNoise              1.0155
    15           GyroscopeBiasNoise              1.0155
    15           GyroscopeBiasDecayFactor        1.0081
    15           AccelerometerBiasNoise          1.0081
    15           AccelerometerBiasDecayFactor    1.0081
    15           ZeroVelocityConstraintNoise     1.0076
    15           GPSPositionNoise                0.9813
    15           GPSVelocityNoise                0.9078
    16           GyroscopeNoise                  0.9074
    16           AccelerometerNoise              0.8926
    16           GyroscopeBiasNoise              0.8926
    16           GyroscopeBiasDecayFactor        0.8823
    16           AccelerometerBiasNoise          0.8823
    16           AccelerometerBiasDecayFactor    0.8823
    16           ZeroVelocityConstraintNoise     0.8815
    16           GPSPositionNoise                0.8526
    16           GPSVelocityNoise                0.7926
    17           GyroscopeNoise                  0.7920
    17           AccelerometerNoise              0.7870
    17           GyroscopeBiasNoise              0.7870
    17           GyroscopeBiasDecayFactor        0.7742
    17           AccelerometerBiasNoise          0.7742
    17           AccelerometerBiasDecayFactor    0.7742
    17           ZeroVelocityConstraintNoise     0.7730
    17           GPSPositionNoise                0.7665
    17           GPSVelocityNoise                0.7665
    18           GyroscopeNoise                  0.7662
    18           AccelerometerNoise              0.7638
    18           GyroscopeBiasNoise              0.7638
    18           GyroscopeBiasDecayFactor        0.7495
    18           AccelerometerBiasNoise          0.7495
    18           AccelerometerBiasDecayFactor    0.7495
    18           ZeroVelocityConstraintNoise     0.7482
    18           GPSPositionNoise                0.7482
    18           GPSVelocityNoise                0.7475
    19           GyroscopeNoise                  0.7474
    19           AccelerometerNoise              0.7474
    19           GyroscopeBiasNoise              0.7474
    19           GyroscopeBiasDecayFactor        0.7474
    19           AccelerometerBiasNoise          0.7474
    19           AccelerometerBiasDecayFactor    0.7474
    19           ZeroVelocityConstraintNoise     0.7453
    19           GPSPositionNoise                0.7416
    19           GPSVelocityNoise                0.7382
    20           GyroscopeNoise                  0.7378
    20           AccelerometerNoise              0.7370
    20           GyroscopeBiasNoise              0.7370
    20           GyroscopeBiasDecayFactor        0.7370
    20           AccelerometerBiasNoise          0.7370
    20           AccelerometerBiasDecayFactor    0.7370
    20           ZeroVelocityConstraintNoise     0.7345
    20           GPSPositionNoise                0.7345
    20           GPSVelocityNoise                0.7345
    21           GyroscopeNoise                  0.7334
    21           AccelerometerNoise              0.7334
    21           GyroscopeBiasNoise              0.7334
    21           GyroscopeBiasDecayFactor        0.7334
    21           AccelerometerBiasNoise          0.7334
    21           AccelerometerBiasDecayFactor    0.7334
    21           ZeroVelocityConstraintNoise     0.7306
    21           GPSPositionNoise                0.7279
    21           GPSVelocityNoise                0.7268
    22           GyroscopeNoise                  0.7248
    22           AccelerometerNoise              0.7247
    22           GyroscopeBiasNoise              0.7247
    22           GyroscopeBiasDecayFactor        0.7234
    22           AccelerometerBiasNoise          0.7234
    22           AccelerometerBiasDecayFactor    0.7234
    22           ZeroVelocityConstraintNoise     0.7207
    22           GPSPositionNoise                0.7206
    22           GPSVelocityNoise                0.7170
    23           GyroscopeNoise                  0.7138
    23           AccelerometerNoise              0.7134
    23           GyroscopeBiasNoise              0.7134
    23           GyroscopeBiasDecayFactor        0.7134
    23           AccelerometerBiasNoise          0.7134
    23           AccelerometerBiasDecayFactor    0.7134
    23           ZeroVelocityConstraintNoise     0.7122
    23           GPSPositionNoise                0.7122
    23           GPSVelocityNoise                0.7122
    24           GyroscopeNoise                  0.7081
    24           AccelerometerNoise              0.7080
    24           GyroscopeBiasNoise              0.7080
    24           GyroscopeBiasDecayFactor        0.7080
    24           AccelerometerBiasNoise          0.7080
    24           AccelerometerBiasDecayFactor    0.7080
    24           ZeroVelocityConstraintNoise     0.7080
    24           GPSPositionNoise                0.7080
    24           GPSVelocityNoise                0.7072
    25           GyroscopeNoise                  0.7009
    25           AccelerometerNoise              0.7009
    25           GyroscopeBiasNoise              0.7009
    25           GyroscopeBiasDecayFactor        0.7007
    25           AccelerometerBiasNoise          0.7007
    25           AccelerometerBiasDecayFactor    0.7007
    25           ZeroVelocityConstraintNoise     0.7005
    25           GPSPositionNoise                0.6997
    25           GPSVelocityNoise                0.6997
    26           GyroscopeNoise                  0.6912
    26           AccelerometerNoise              0.6906
    26           GyroscopeBiasNoise              0.6906
    26           GyroscopeBiasDecayFactor        0.6906
    26           AccelerometerBiasNoise          0.6906
    26           AccelerometerBiasDecayFactor    0.6906
    26           ZeroVelocityConstraintNoise     0.6896
    26           GPSPositionNoise                0.6896
    26           GPSVelocityNoise                0.6896
    27           GyroscopeNoise                  0.6840
    27           AccelerometerNoise              0.6831
    27           GyroscopeBiasNoise              0.6831
    27           GyroscopeBiasDecayFactor        0.6831
    27           AccelerometerBiasNoise          0.6831
    27           AccelerometerBiasDecayFactor    0.6831
    27           ZeroVelocityConstraintNoise     0.6818
    27           GPSPositionNoise                0.6816
    27           GPSVelocityNoise                0.6816
    28           GyroscopeNoise                  0.6816
    28           AccelerometerNoise              0.6809
    28           GyroscopeBiasNoise              0.6809
    28           GyroscopeBiasDecayFactor        0.6809
    28           AccelerometerBiasNoise          0.6809
    28           AccelerometerBiasDecayFactor    0.6809
    28           ZeroVelocityConstraintNoise     0.6804
    28           GPSPositionNoise                0.6802
    28           GPSVelocityNoise                0.6802
    29           GyroscopeNoise                  0.6793
    29           AccelerometerNoise              0.6785
    29           GyroscopeBiasNoise              0.6785
    29           GyroscopeBiasDecayFactor        0.6785
    29           AccelerometerBiasNoise          0.6785
    29           AccelerometerBiasDecayFactor    0.6785
    29           ZeroVelocityConstraintNoise     0.6778
    29           GPSPositionNoise                0.6773
    29           GPSVelocityNoise                0.6773
    30           GyroscopeNoise                  0.6773
    30           AccelerometerNoise              0.6769
    30           GyroscopeBiasNoise              0.6769
    30           GyroscopeBiasDecayFactor        0.6769
    30           AccelerometerBiasNoise          0.6769
    30           AccelerometerBiasDecayFactor    0.6769
    30           ZeroVelocityConstraintNoise     0.6769
    30           GPSPositionNoise                0.6769
    30           GPSVelocityNoise                0.6769

Fuse the sensor data using the tuned filter. Obtain estimated pose and orientation.

N = size(sensorData,1);
qEstTuned = quaternion.zeros(N,1);
posEstTuned = zeros(N,3);
for ii=1:N
    predict(filter,Accelerometer(ii,:),Gyroscope(ii,:));
    if all(~isnan(GPSPosition(ii,1)))
        fusegps(filter, GPSPosition(ii,:), ...
            tunedNoise.GPSPositionNoise,GPSVelocity(ii,:), ...
            tunedNoise.GPSVelocityNoise);
    end
    [posEstTuned(ii,:),qEstTuned(ii,:)] = pose(filter);
end

Compute the RMS errors.

orientationErrorTuned = rad2deg(dist(qEstTuned,Orientation));
rmsOrientationErrorTuned = sqrt(mean(orientationErrorTuned.^2))

rmsOrientationErrorTuned = 
1.6857

positionErrorTuned = sqrt(sum((posEstTuned-Position).^2,2));
rmsPositionErrorTuned = sqrt(mean(positionErrorTuned.^2))

rmsPositionErrorTuned = 
1.6667

Visualize the results.

figure;
t = (0:N-1)./filter.IMUSampleRate;
subplot(2,1,1)
plot(t,positionErrorTuned,'b');
title("Tuned insfilterNonholonomic" + newline + ...
    "Euclidean Distance Position Error")
xlabel('Time (s)');
ylabel('Position Error (meters)')
subplot(2,1,2)
plot(t,orientationErrorTuned,'b');
title("Orientation Error")
xlabel('Time (s)');
ylabel('Orientation Error (degrees)');

Input Arguments

filter — Filter object

infilterAsync object

Filter object, specified as an insfilterNonholonomic object.

measureNoise — Measurement noise

structure

Measurement noise, specified as a structure. The function uses the measurement noise input as the initial guess for tuning the measurement noise. The structure must contain these fields:

Field name	Description
GPSPositionNoise	Variance of GPS position noise, specified as a scalar in m2
GPSVelocityNoise	Variance of GPS velocity noise, specified as a scalar in (m/s)2

Data Types: struct

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.

• GPSPosition — GPS position data, specified as a 1-by-3 vector of latitude in degrees, longitude in degrees, and altitude in meters.

• GPSVelocity — GPS velocity data, specified as a 1-by-3 vector of scalars in m/s.

If the GPS sensor does not produce complete measurements, specify the corresponding entry for GPSPosition and/or GPSVelocity 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.

Data Types: table

groundTruth — Ground truth data

table

Ground truth data, specified as a table. In each row, the table can optionally contain any of these variables:

• Orientation — Orientation from the navigation frame to the body frame, specified as a quaternion or a 3-by-3 rotation matrix.

• Position — Position in navigation frame, specified as a 1-by-3 vector of scalars in meters.

• Velocity — Velocity in navigation frame, specified as a 1-by-3 vector of scalars in m/s.

• GyroscopeBias — Gyroscope delta angle bias in body frame, specified as a 1-by-3 vector of scalars in rad/s.

• AccelerometerBias — Accelerometer delta angle bias in body frame, specified as a 1-by-3 vector of scalars in m²/s.

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 number of rows.

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.

Data Types: table

config — Tuner configuration

tunerconfig object

Tuner configuration, specified as a tunerconfig object.

Output Arguments

tunedMeasureNoise — Tuned measurement noise

structure

Tuned measurement noise, returned as a structure. The structure contains these fields.

Field name	Description
GPSPositionNoise	Variance of GPS position noise, specified as a scalar in m2
GPSVelocityNoise	Variance of GPS velocity noise, specified as a scalar in (m/s)2

Data Types: struct

References

[1] Abbeel, Pieter, et al. “Discriminative Training of Kalman Filters.” Robotics: Science and Systems I, Robotics: Science and Systems Foundation, 2005. DOI.org (Crossref), doi:10.15607/RSS.2005.I.038.

Version History

Introduced in R2020b