tune

Tune insfilterErrorState parameters to reduce estimation error

Since R2021a

Syntax

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

tunedMeasureNoise = tune(___,config)

Description

tunedMeasureNoise = tune(filter,measureNoise,sensorData,groundTruth) adjusts the properties of the insfilterErrorState 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 insfilterErrorState to Optimize Pose Estimate

Load the recorded sensor data and ground truth data.

load('insfilterErrorStateTuneData.mat');

Create tables for the sensor data and the truth data.

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

Create an insfilterErrorState filter object.

filter = insfilterErrorState('State',initialState, ...
    'StateCovariance',initialStateCovariance);

Create a tuner configuration object for the filter. Use the tuner noise function to obtain a set of initial sensor noises used in the filter.

cfg = tunerconfig('insfilterErrorState','MaxIterations',40);
measNoise = tunernoise('insfilterErrorState')

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

Tune the filter and obtain the tuned parameters.

tunedmn = tune(filter,measNoise,sensorData, ...
    groundTruth,cfg);

    Iteration    Parameter                 Metric
    _________    _________                 ______
    1            AccelerometerNoise        4.1291
    1            GyroscopeNoise            4.1291
    1            AccelerometerBiasNoise    4.1290
    1            GyroscopeBiasNoise        4.1290
    1            GPSPositionNoise          4.0213
    1            GPSVelocityNoise          4.0051
    1            MVOPositionNoise          3.9949
    1            MVOOrientationNoise       3.9886
    2            AccelerometerNoise        3.9886
    2            GyroscopeNoise            3.9886
    2            AccelerometerBiasNoise    3.9886
    2            GyroscopeBiasNoise        3.9886
    2            GPSPositionNoise          3.8381
    2            GPSVelocityNoise          3.8268
    2            MVOPositionNoise          3.8219
    2            MVOOrientationNoise       3.8035
    3            AccelerometerNoise        3.8035
    3            GyroscopeNoise            3.8035
    3            AccelerometerBiasNoise    3.8035
    3            GyroscopeBiasNoise        3.8035
    3            GPSPositionNoise          3.6299
    3            GPSVelocityNoise          3.6276
    3            MVOPositionNoise          3.6241
    3            MVOOrientationNoise       3.5911
    4            AccelerometerNoise        3.5911
    4            GyroscopeNoise            3.5911
    4            AccelerometerBiasNoise    3.5911
    4            GyroscopeBiasNoise        3.5911
    4            GPSPositionNoise          3.1728
    4            GPSVelocityNoise          3.1401
    4            MVOPositionNoise          2.7686
    4            MVOOrientationNoise       2.6632
    5            AccelerometerNoise        2.6632
    5            GyroscopeNoise            2.6632
    5            AccelerometerBiasNoise    2.6632
    5            GyroscopeBiasNoise        2.6632
    5            GPSPositionNoise          2.3242
    5            GPSVelocityNoise          2.2291
    5            MVOPositionNoise          2.2291
    5            MVOOrientationNoise       2.0904
    6            AccelerometerNoise        2.0903
    6            GyroscopeNoise            2.0903
    6            AccelerometerBiasNoise    2.0903
    6            GyroscopeBiasNoise        2.0903
    6            GPSPositionNoise          2.0903
    6            GPSVelocityNoise          2.0141
    6            MVOPositionNoise          1.9952
    6            MVOOrientationNoise       1.8497
    7            AccelerometerNoise        1.8497
    7            GyroscopeNoise            1.8496
    7            AccelerometerBiasNoise    1.8496
    7            GyroscopeBiasNoise        1.8496
    7            GPSPositionNoise          1.8398
    7            GPSVelocityNoise          1.7528
    7            MVOPositionNoise          1.7362
    7            MVOOrientationNoise       1.5762
    8            AccelerometerNoise        1.5762
    8            GyroscopeNoise            1.5762
    8            AccelerometerBiasNoise    1.5762
    8            GyroscopeBiasNoise        1.5762
    8            GPSPositionNoise          1.5762
    8            GPSVelocityNoise          1.5107
    8            MVOPositionNoise          1.4786
    8            MVOOrientationNoise       1.3308
    9            AccelerometerNoise        1.3308
    9            GyroscopeNoise            1.3308
    9            AccelerometerBiasNoise    1.3308
    9            GyroscopeBiasNoise        1.3308
    9            GPSPositionNoise          1.3308
    9            GPSVelocityNoise          1.2934
    9            MVOPositionNoise          1.2525
    9            MVOOrientationNoise       1.1462
    10           AccelerometerNoise        1.1462
    10           GyroscopeNoise            1.1462
    10           AccelerometerBiasNoise    1.1462
    10           GyroscopeBiasNoise        1.1462
    10           GPSPositionNoise          1.1443
    10           GPSVelocityNoise          1.1332
    10           MVOPositionNoise          1.0964
    10           MVOOrientationNoise       1.0382
    11           AccelerometerNoise        1.0382
    11           GyroscopeNoise            1.0382
    11           AccelerometerBiasNoise    1.0382
    11           GyroscopeBiasNoise        1.0382
    11           GPSPositionNoise          1.0348
    11           GPSVelocityNoise          1.0348
    11           MVOPositionNoise          1.0081
    11           MVOOrientationNoise       0.9734
    12           AccelerometerNoise        0.9734
    12           GyroscopeNoise            0.9734
    12           AccelerometerBiasNoise    0.9734
    12           GyroscopeBiasNoise        0.9734
    12           GPSPositionNoise          0.9693
    12           GPSVelocityNoise          0.9682
    12           MVOPositionNoise          0.9488
    12           MVOOrientationNoise       0.9244
    13           AccelerometerNoise        0.9244
    13           GyroscopeNoise            0.9244
    13           AccelerometerBiasNoise    0.9244
    13           GyroscopeBiasNoise        0.9244
    13           GPSPositionNoise          0.9203
    13           GPSVelocityNoise          0.9199
    13           MVOPositionNoise          0.9045
    13           MVOOrientationNoise       0.8846
    14           AccelerometerNoise        0.8846
    14           GyroscopeNoise            0.8846
    14           AccelerometerBiasNoise    0.8845
    14           GyroscopeBiasNoise        0.8845
    14           GPSPositionNoise          0.8807
    14           GPSVelocityNoise          0.8807
    14           MVOPositionNoise          0.8659
    14           MVOOrientationNoise       0.8501
    15           AccelerometerNoise        0.8501
    15           GyroscopeNoise            0.8501
    15           AccelerometerBiasNoise    0.8500
    15           GyroscopeBiasNoise        0.8500
    15           GPSPositionNoise          0.8457
    15           GPSVelocityNoise          0.8453
    15           MVOPositionNoise          0.8299
    15           MVOOrientationNoise       0.8173
    16           AccelerometerNoise        0.8173
    16           GyroscopeNoise            0.8173
    16           AccelerometerBiasNoise    0.8172
    16           GyroscopeBiasNoise        0.8172
    16           GPSPositionNoise          0.8122
    16           GPSVelocityNoise          0.8116
    16           MVOPositionNoise          0.7961
    16           MVOOrientationNoise       0.7858
    17           AccelerometerNoise        0.7858
    17           GyroscopeNoise            0.7858
    17           AccelerometerBiasNoise    0.7857
    17           GyroscopeBiasNoise        0.7857
    17           GPSPositionNoise          0.7807
    17           GPSVelocityNoise          0.7800
    17           MVOPositionNoise          0.7655
    17           MVOOrientationNoise       0.7572
    18           AccelerometerNoise        0.7572
    18           GyroscopeNoise            0.7572
    18           AccelerometerBiasNoise    0.7570
    18           GyroscopeBiasNoise        0.7570
    18           GPSPositionNoise          0.7525
    18           GPSVelocityNoise          0.7520
    18           MVOPositionNoise          0.7401
    18           MVOOrientationNoise       0.7338
    19           AccelerometerNoise        0.7337
    19           GyroscopeNoise            0.7337
    19           AccelerometerBiasNoise    0.7335
    19           GyroscopeBiasNoise        0.7335
    19           GPSPositionNoise          0.7293
    19           GPSVelocityNoise          0.7290
    19           MVOPositionNoise          0.7185
    19           MVOOrientationNoise       0.7140
    20           AccelerometerNoise        0.7138
    20           GyroscopeNoise            0.7138
    20           AccelerometerBiasNoise    0.7134
    20           GyroscopeBiasNoise        0.7134
    20           GPSPositionNoise          0.7086
    20           GPSVelocityNoise          0.7068
    20           MVOPositionNoise          0.6956
    20           MVOOrientationNoise       0.6926
    21           AccelerometerNoise        0.6922
    21           GyroscopeNoise            0.6922
    21           AccelerometerBiasNoise    0.6916
    21           GyroscopeBiasNoise        0.6916
    21           GPSPositionNoise          0.6862
    21           GPSVelocityNoise          0.6822
    21           MVOPositionNoise          0.6682
    21           MVOOrientationNoise       0.6667
    22           AccelerometerNoise        0.6660
    22           GyroscopeNoise            0.6660
    22           AccelerometerBiasNoise    0.6650
    22           GyroscopeBiasNoise        0.6650
    22           GPSPositionNoise          0.6605
    22           GPSVelocityNoise          0.6541
    22           MVOPositionNoise          0.6372
    22           MVOOrientationNoise       0.6368
    23           AccelerometerNoise        0.6356
    23           GyroscopeNoise            0.6356
    23           AccelerometerBiasNoise    0.6344
    23           GyroscopeBiasNoise        0.6344
    23           GPSPositionNoise          0.6324
    23           GPSVelocityNoise          0.6252
    23           MVOPositionNoise          0.6087
    23           MVOOrientationNoise       0.6087
    24           AccelerometerNoise        0.6075
    24           GyroscopeNoise            0.6075
    24           AccelerometerBiasNoise    0.6068
    24           GyroscopeBiasNoise        0.6068
    24           GPSPositionNoise          0.6061
    24           GPSVelocityNoise          0.6032
    24           MVOPositionNoise          0.6032
    24           MVOOrientationNoise       0.6032
    25           AccelerometerNoise        0.6017
    25           GyroscopeNoise            0.6017
    25           AccelerometerBiasNoise    0.6012
    25           GyroscopeBiasNoise        0.6012
    25           GPSPositionNoise          0.6010
    25           GPSVelocityNoise          0.6005
    25           MVOPositionNoise          0.6005
    25           MVOOrientationNoise       0.6005
    26           AccelerometerNoise        0.5992
    26           GyroscopeNoise            0.5992
    26           AccelerometerBiasNoise    0.5987
    26           GyroscopeBiasNoise        0.5987
    26           GPSPositionNoise          0.5983
    26           GPSVelocityNoise          0.5983
    26           MVOPositionNoise          0.5983
    26           MVOOrientationNoise       0.5983
    27           AccelerometerNoise        0.5975
    27           GyroscopeNoise            0.5975
    27           AccelerometerBiasNoise    0.5974
    27           GyroscopeBiasNoise        0.5974
    27           GPSPositionNoise          0.5973
    27           GPSVelocityNoise          0.5972
    27           MVOPositionNoise          0.5971
    27           MVOOrientationNoise       0.5971
    28           AccelerometerNoise        0.5971
    28           GyroscopeNoise            0.5971
    28           AccelerometerBiasNoise    0.5970
    28           GyroscopeBiasNoise        0.5970
    28           GPSPositionNoise          0.5970
    28           GPSVelocityNoise          0.5970
    28           MVOPositionNoise          0.5970
    28           MVOOrientationNoise       0.5970
    29           AccelerometerNoise        0.5970
    29           GyroscopeNoise            0.5970
    29           AccelerometerBiasNoise    0.5970
    29           GyroscopeBiasNoise        0.5970
    29           GPSPositionNoise          0.5970
    29           GPSVelocityNoise          0.5970
    29           MVOPositionNoise          0.5970
    29           MVOOrientationNoise       0.5970
    30           AccelerometerNoise        0.5969
    30           GyroscopeNoise            0.5969
    30           AccelerometerBiasNoise    0.5969
    30           GyroscopeBiasNoise        0.5969
    30           GPSPositionNoise          0.5969
    30           GPSVelocityNoise          0.5969
    30           MVOPositionNoise          0.5968
    30           MVOOrientationNoise       0.5968
    31           AccelerometerNoise        0.5968
    31           GyroscopeNoise            0.5968
    31           AccelerometerBiasNoise    0.5968
    31           GyroscopeBiasNoise        0.5968
    31           GPSPositionNoise          0.5968
    31           GPSVelocityNoise          0.5968
    31           MVOPositionNoise          0.5967
    31           MVOOrientationNoise       0.5967
    32           AccelerometerNoise        0.5967
    32           GyroscopeNoise            0.5967
    32           AccelerometerBiasNoise    0.5967
    32           GyroscopeBiasNoise        0.5967
    32           GPSPositionNoise          0.5967
    32           GPSVelocityNoise          0.5967
    32           MVOPositionNoise          0.5966
    32           MVOOrientationNoise       0.5966
    33           AccelerometerNoise        0.5966
    33           GyroscopeNoise            0.5966
    33           AccelerometerBiasNoise    0.5966
    33           GyroscopeBiasNoise        0.5966
    33           GPSPositionNoise          0.5966
    33           GPSVelocityNoise          0.5966
    33           MVOPositionNoise          0.5965
    33           MVOOrientationNoise       0.5965
    34           AccelerometerNoise        0.5965
    34           GyroscopeNoise            0.5965
    34           AccelerometerBiasNoise    0.5965
    34           GyroscopeBiasNoise        0.5965
    34           GPSPositionNoise          0.5965
    34           GPSVelocityNoise          0.5964
    34           MVOPositionNoise          0.5964
    34           MVOOrientationNoise       0.5964
    35           AccelerometerNoise        0.5964
    35           GyroscopeNoise            0.5964
    35           AccelerometerBiasNoise    0.5963
    35           GyroscopeBiasNoise        0.5963
    35           GPSPositionNoise          0.5963
    35           GPSVelocityNoise          0.5963
    35           MVOPositionNoise          0.5963
    35           MVOOrientationNoise       0.5963
    36           AccelerometerNoise        0.5963
    36           GyroscopeNoise            0.5963
    36           AccelerometerBiasNoise    0.5963
    36           GyroscopeBiasNoise        0.5963
    36           GPSPositionNoise          0.5963
    36           GPSVelocityNoise          0.5963
    36           MVOPositionNoise          0.5963
    36           MVOOrientationNoise       0.5963
    37           AccelerometerNoise        0.5963
    37           GyroscopeNoise            0.5963
    37           AccelerometerBiasNoise    0.5963
    37           GyroscopeBiasNoise        0.5963
    37           GPSPositionNoise          0.5962
    37           GPSVelocityNoise          0.5962
    37           MVOPositionNoise          0.5962
    37           MVOOrientationNoise       0.5962
    38           AccelerometerNoise        0.5962
    38           GyroscopeNoise            0.5962
    38           AccelerometerBiasNoise    0.5962
    38           GyroscopeBiasNoise        0.5962
    38           GPSPositionNoise          0.5962
    38           GPSVelocityNoise          0.5961
    38           MVOPositionNoise          0.5961
    38           MVOOrientationNoise       0.5961
    39           AccelerometerNoise        0.5961
    39           GyroscopeNoise            0.5961
    39           AccelerometerBiasNoise    0.5961
    39           GyroscopeBiasNoise        0.5961
    39           GPSPositionNoise          0.5961
    39           GPSVelocityNoise          0.5960
    39           MVOPositionNoise          0.5960
    39           MVOOrientationNoise       0.5960
    40           AccelerometerNoise        0.5960
    40           GyroscopeNoise            0.5960
    40           AccelerometerBiasNoise    0.5960
    40           GyroscopeBiasNoise        0.5960
    40           GPSPositionNoise          0.5960
    40           GPSVelocityNoise          0.5959
    40           MVOPositionNoise          0.5959
    40           MVOOrientationNoise       0.5959

Fuse the sensor data using the tuned filter.

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,:), ...
            tunedmn.GPSPositionNoise,GPSVelocity(ii,:), ...
            tunedmn.GPSVelocityNoise);
    end
    if all(~isnan(MVOPosition(ii,1)))
        fusemvo(filter,MVOPosition(ii,:),tunedmn.MVOPositionNoise, ...
            MVOOrientation{ii},tunedmn.MVOOrientationNoise);
    end
    [posEstTuned(ii,:),qEstTuned(ii,:)] = pose(filter);
end

Compute the RMS errors.

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

rmsOrientationErrorTuned = 
4.4999

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

rmsPositionErrorTuned = 
0.1172

Visualize the results.

figure;
t = (0:N-1)./filter.IMUSampleRate;
subplot(2,1,1)
plot(t, positionErrorTuned,'b');
title("Tuned insfilterErrorState" + 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

insfilterErrorState object

Filter object, specified as an insfilterErrorState 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
MVOOrientationNoise	Orientation measurement covariance of monocular visual odometry, specified as a scalar, 3-element vector, or 3-by-3 matrix in rad2
MVOPositionNoise	Position measurement covariance of MVO, specified as a scalar, 3-element vector, or 3-by-3 matrix in m2
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

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.

• MVOOrienation — Orientation of the camera with respect to the local navigation frame, specified as a scalar quaternion or 3-by-3 rotation matrix. The quaternion or rotation matrix is a frame rotation from the local navigation frame to the current camera coordinate system.

• MVOPosition — Position of camera in the local navigation frame, specified as a real 3-element row vector in meters.

• 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.

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.

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

• VisualOdometryScale — Visual odometry scale factor, specified as a scalar.

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.

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
MVOOrientationNoise	Orientation measurement covariance of monocular visual odometry, specified as a scalar, 3-element vector, or 3-by-3 matrix in rad2
MVOPositionNoise	Position measurement covariance of MVO, specified as a scalar, 3-element vector, or 3-by-3 matrix in m2
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

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 R2021a