Main Content

tune

Tune insfilterMARG parameters to reduce estimation error

Since R2021a

collapse all in page

Syntax

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

Description

tunedMeasureNoise = tune(filter,measureNoise,sensorData,groundTruth) adjusts the properties of the insfilterMARG 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.

example

Examples

collapse all

Open Live Script

Load the recorded sensor data and ground truth data.

load('insfilterMARGTuneData.mat');

Create tables for the sensor data and the truth data.

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

Create an insfilterMARG filter object that has a few noise properties.

filter = insfilterMARG('State',initialState,...
        'StateCovariance',initialStateCovariance,...
        'AccelerometerBiasNoise',1e-7,...
        'GyroscopeBiasNoise',1e-7,...
        'MagnetometerBiasNoise',1e-7,...
        'GeomagneticVectorNoise',1e-7);

Create a tuner configuration object for the filter. Set the maximum iterations to eight. Also, set the tunable parameters.

cfg = tunerconfig('insfilterMARG', 'MaxIterations', 8);
cfg.TunableParameters = setdiff(cfg.TunableParameters, ...
    {'GeomagneticFieldVector', 'AccelerometerBiasNoise', ...
    'GyroscopeBiasNoise', 'MagnetometerBiasNoise'});

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

measNoise = tunernoise('insfilterMARG')
measNoise = struct with fields:
    MagnetometerNoise: 1
     GPSPositionNoise: 1
     GPSVelocityNoise: 1

Tune the filter and obtain the tuned parameters.

tunedParams = tune(filter, measNoise, sensorData, ...
        groundTruth, cfg);
    Iteration    Parameter                 Metric
    _________    _________                 ______
    1            AccelerometerNoise        2.5701
    1            GPSPositionNoise          2.5446
    1            GPSVelocityNoise          2.5279
    1            GeomagneticVectorNoise    2.5268
    1            GyroscopeNoise            2.5268
    1            MagnetometerNoise         2.5204
    2            AccelerometerNoise        2.5203
    2            GPSPositionNoise          2.4908
    2            GPSVelocityNoise          2.4695
    2            GeomagneticVectorNoise    2.4684
    2            GyroscopeNoise            2.4684
    2            MagnetometerNoise         2.4615
    3            AccelerometerNoise        2.4615
    3            GPSPositionNoise          2.4265
    3            GPSVelocityNoise          2.4000
    3            GeomagneticVectorNoise    2.3988
    3            GyroscopeNoise            2.3988
    3            MagnetometerNoise         2.3911
    4            AccelerometerNoise        2.3911
    4            GPSPositionNoise          2.3500
    4            GPSVelocityNoise          2.3164
    4            GeomagneticVectorNoise    2.3153
    4            GyroscopeNoise            2.3153
    4            MagnetometerNoise         2.3068
    5            AccelerometerNoise        2.3068
    5            GPSPositionNoise          2.2587
    5            GPSVelocityNoise          2.2166
    5            GeomagneticVectorNoise    2.2154
    5            GyroscopeNoise            2.2154
    5            MagnetometerNoise         2.2063
    6            AccelerometerNoise        2.2063
    6            GPSPositionNoise          2.1505
    6            GPSVelocityNoise          2.0981
    6            GeomagneticVectorNoise    2.0971
    6            GyroscopeNoise            2.0971
    6            MagnetometerNoise         2.0875
    7            AccelerometerNoise        2.0874
    7            GPSPositionNoise          2.0240
    7            GPSVelocityNoise          1.9601
    7            GeomagneticVectorNoise    1.9594
    7            GyroscopeNoise            1.9594
    7            MagnetometerNoise         1.9499
    8            AccelerometerNoise        1.9499
    8            GPSPositionNoise          1.8802
    8            GPSVelocityNoise          1.8035
    8            GeomagneticVectorNoise    1.8032
    8            GyroscopeNoise            1.8032
    8            MagnetometerNoise         1.7959

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(Magnetometer(ii,1)))
        fusemag(filter,Magnetometer(ii,:),...
            tunedParams.MagnetometerNoise);
    end
    if all(~isnan(GPSPosition(ii,1)))
        fusegps(filter,GPSPosition(ii,:),...
            tunedParams.GPSPositionNoise,GPSVelocity(ii,:),...
            tunedParams.GPSVelocityNoise);
    end
    [posEstTuned(ii,:),qEstTuned(ii,:)] = pose(filter);
end

Compute the RMS errors. 

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

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

Visualize the results.

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

Figure contains 2 axes objects. Axes object 1 with title Tuned insfilterMARG Euclidean Distance Position Error, xlabel Time (s), ylabel Position Error (meters) contains an object of type line. Axes object 2 with title Orientation Error, xlabel Time (s), ylabel Orientation Error (degrees) contains an object of type line.

Input Arguments

collapse all

Filter object, specified as an insfilterMARG 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 must contain these fields:

Field nameDescription
MagnetometerNoiseVariance of magnetometer noise, specified as a scalar in (μT)2
GPSPositionNoiseVariance of GPS position noise, specified as a scalar in m2
GPSVelocityNoiseVariance of GPS velocity noise, specified as a scalar in (m/s)2

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.

  • Magnetometer — Magnetometer data, specified as a 1-by-3 vector of scalars in μT.

  • GPSPosition — GPS position data, specified as a 1-by-3 vector of scalars in [degrees, degrees, 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.

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.

  • DeltaAngleBias — Delta angle bias, specified as a 1-by-3 vector of scalars in radians.

  • DeltaVelocityBias — Delta velocity bias, specified as a 1-by-3 vector of scalars in m/s.

  • GeomagneticFieldVector — Geomagnetic field vector in navigation frame, specified as a 1-by-3 vector of scalars.

  • MagnetometerBias — Magnetometer bias in body frame, specified as a 1-by-3 vector of scalars in μT.

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.

Tuner configuration, specified as a tunerconfig object.

Output Arguments

collapse all

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

Field nameDescription
MagnetometerNoiseVariance of magnetometer noise, specified as a scalar in (μT)2
GPSPositionNoiseVariance of GPS position noise, specified as a scalar in m2
GPSVelocityNoiseVariance 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

See Also

|