Main Content

Automatic Tuning of the insfilterAsync Filter

Open Live Script

The insfilterAsync object is a complex extended Kalman filter that estimates the device pose. However, manually tuning the filter or finding the optimal values for the noise parameters can be a challenging task. This example illustrates how to use the tune function to optimize the filter noise parameters.

Trajectory and Sensor Setup

To illustrate the tuning process of the insfilterAsync filter, use a simple random waypoint trajectory. The imuSensor and gpsSensor objects create inputs for the filter.

% The IMU runs at 100 Hz and the GPS runs at 1 Hz.
imurate = 100;
gpsrate = 1;
decim = imurate/gpsrate;

% Create a random waypoint trajectory.
rng(1)
Npts = 4; % number of waypoints
wpPer = 5; % time between waypoints
tstart = 0;
tend = wpPer*(Npts -1);
wp = waypointTrajectory('Waypoints',5*rand(Npts,3), ...
        'TimeOfArrival',tstart:wpPer:tend, ...
        'Orientation',[quaternion.ones; randrot(Npts-1,1)], ...
        'SampleRate', imurate);

[Position,Orientation,Velocity,Acceleration,AngularVelocity] = lookupPose(...
    wp, tstart:(1/imurate):tend);

% Set up an IMU and process the trajectory.
imu = imuSensor('SampleRate',imurate);
loadparams(imu,fullfile(matlabroot, ...
    "toolbox\shared\positioning\positioningdata\generic.json"), ...
    "GenericLowCost9Axis");
[Accelerometer, Gyroscope, Magnetometer] = imu(Acceleration, ...
    AngularVelocity, Orientation);
imuData = timetable(Accelerometer,Gyroscope,Magnetometer,'SampleRate',imurate);

% Set up a GPS sensor and process the trajectory.
gps = gpsSensor('SampleRate', gpsrate,'DecayFactor',0.5, ...
    'HorizontalPositionAccuracy',1.6,'VerticalPositionAccuracy',1.6, ...
    'VelocityAccuracy',0.1);
[GPSPosition,GPSVelocity] = gps(Position(1:decim:end,:), Velocity(1:decim:end,:));
gpsData = timetable(GPSPosition,GPSVelocity,'SampleRate',gpsrate);

% Create a timetable for the tune function.
sensorData = synchronize(imuData,gpsData);

% Create a timetable capturing the ground truth pose.
groundTruth = timetable(Position,Orientation,'SampleRate',imurate);

Construct the Filter

The insfilterAsync filter fuses data from multiple sensors operating asynchronously

filtUntuned = insfilterAsync;

Determine Filter Initial Conditions

Set the initial values for the State and StateCovariance properties based on the ground truth. Normally to obtain the initial values, you would use the first several samples of sensorData along with calibration routines. However, in this example the groundTruth is used to set the initial state for fast convergence of the filter.

idx = stateinfo(filtUntuned);
filtUntuned.State(idx.Orientation) = compact(Orientation(1));
filtUntuned.State(idx.AngularVelocity) = AngularVelocity(1,:);
filtUntuned.State(idx.Position) = Position(1,:);
filtUntuned.State(idx.Velocity) = Velocity(1,:);
filtUntuned.State(idx.Acceleration) = Acceleration(1,:);
filtUntuned.State(idx.AccelerometerBias) = imu.Accelerometer.ConstantBias;
filtUntuned.State(idx.GyroscopeBias) = imu.Gyroscope.ConstantBias;
filtUntuned.State(idx.GeomagneticFieldVector) = imu.MagneticField;
filtUntuned.State(idx.MagnetometerBias) = imu.Magnetometer.ConstantBias;
filtUntuned.StateCovariance = 1e-5*eye(numel(filtUntuned.State));

% Create a copy of the filtUntuned object for tuning later.
filtTuned = copy(filtUntuned);

Process sensorData with an Untuned Filter

Use the tunernoise function to create measurement noises which also need to be tuned. To illustrate the necessity for tuning, first use the filter with its default parameters. 

mn = tunernoise('insfilterAsync');
[posUntunedEst, orientUntunedEst] = fuse(filtUntuned, sensorData, mn);

Tune the Filter and Process sensorData

Use the tune function to minimize the root mean squared (RMS) error between the groundTruth and state estimates.

cfg = tunerconfig(class(filtTuned),'MaxIterations',15,'StepForward',1.1);
tunedmn = tune(filtTuned,mn,sensorData,groundTruth,cfg);
    Iteration    Parameter                 Metric
    _________    _________                 ______
    1            AccelerometerNoise        4.7634
    1            GyroscopeNoise            4.7439
    1            MagnetometerNoise         4.7260
    1            GPSPositionNoise          4.6562
    1            GPSVelocityNoise          4.4895
    1            QuaternionNoise           4.4895
    1            AngularVelocityNoise      4.1764
    1            PositionNoise             4.1764
    1            VelocityNoise             4.1764
    1            AccelerationNoise         4.1657
    1            GyroscopeBiasNoise        4.1657
    1            AccelerometerBiasNoise    4.1615
    1            GeomagneticVectorNoise    4.1615
    1            MagnetometerBiasNoise     4.1466
    2            AccelerometerNoise        4.1466
    2            GyroscopeNoise            4.1466
    2            MagnetometerNoise         4.1218
    2            GPSPositionNoise          4.1218
    2            GPSVelocityNoise          3.9411
    2            QuaternionNoise           3.9404
    2            AngularVelocityNoise      3.2970
    2            PositionNoise             3.2970
    2            VelocityNoise             3.2970
    2            AccelerationNoise         3.2735
    2            GyroscopeBiasNoise        3.2735
    2            AccelerometerBiasNoise    3.2711
    2            GeomagneticVectorNoise    3.2711
    2            MagnetometerBiasNoise     3.2709
    3            AccelerometerNoise        3.2704
    3            GyroscopeNoise            3.2475
    3            MagnetometerNoise         3.2338
    3            GPSPositionNoise          3.2338
    3            GPSVelocityNoise          3.1333
    3            QuaternionNoise           3.1333
    3            AngularVelocityNoise      3.0267
    3            PositionNoise             3.0267
    3            VelocityNoise             3.0267
    3            AccelerationNoise         3.0267
    3            GyroscopeBiasNoise        3.0267
    3            AccelerometerBiasNoise    3.0248
    3            GeomagneticVectorNoise    3.0248
    3            MagnetometerBiasNoise     3.0184
    4            AccelerometerNoise        3.0156
    4            GyroscopeNoise            3.0028
    4            MagnetometerNoise         2.9903
    4            GPSPositionNoise          2.9872
    4            GPSVelocityNoise          2.9043
    4            QuaternionNoise           2.9043
    4            AngularVelocityNoise      2.7873
    4            PositionNoise             2.7873
    4            VelocityNoise             2.7873
    4            AccelerationNoise         2.7833
    4            GyroscopeBiasNoise        2.7833
    4            AccelerometerBiasNoise    2.7821
    4            GeomagneticVectorNoise    2.7821
    4            MagnetometerBiasNoise     2.7727
    5            AccelerometerNoise        2.7693
    5            GyroscopeNoise            2.7591
    5            MagnetometerNoise         2.7591
    5            GPSPositionNoise          2.7580
    5            GPSVelocityNoise          2.6806
    5            QuaternionNoise           2.6804
    5            AngularVelocityNoise      2.5956
    5            PositionNoise             2.5956
    5            VelocityNoise             2.5956
    5            AccelerationNoise         2.5880
    5            GyroscopeBiasNoise        2.5880
    5            AccelerometerBiasNoise    2.5874
    5            GeomagneticVectorNoise    2.5874
    5            MagnetometerBiasNoise     2.5693
    6            AccelerometerNoise        2.5651
    6            GyroscopeNoise            2.5529
    6            MagnetometerNoise         2.5472
    6            GPSPositionNoise          2.5344
    6            GPSVelocityNoise          2.4514
    6            QuaternionNoise           2.4514
    6            AngularVelocityNoise      2.3829
    6            PositionNoise             2.3829
    6            VelocityNoise             2.3829
    6            AccelerationNoise         2.3826
    6            GyroscopeBiasNoise        2.3826
    6            AccelerometerBiasNoise    2.3822
    6            GeomagneticVectorNoise    2.3822
    6            MagnetometerBiasNoise     2.3675
    7            AccelerometerNoise        2.3640
    7            GyroscopeNoise            2.3547
    7            MagnetometerNoise         2.3540
    7            GPSPositionNoise          2.3428
    7            GPSVelocityNoise          2.2634
    7            QuaternionNoise           2.2633
    7            AngularVelocityNoise      2.2179
    7            PositionNoise             2.2179
    7            VelocityNoise             2.2179
    7            AccelerationNoise         2.2159
    7            GyroscopeBiasNoise        2.2159
    7            AccelerometerBiasNoise    2.2150
    7            GeomagneticVectorNoise    2.2150
    7            MagnetometerBiasNoise     2.2020
    8            AccelerometerNoise        2.1967
    8            GyroscopeNoise            2.1951
    8            MagnetometerNoise         2.1948
    8            GPSPositionNoise          2.1804
    8            GPSVelocityNoise          2.1153
    8            QuaternionNoise           2.1153
    8            AngularVelocityNoise      2.0978
    8            PositionNoise             2.0978
    8            VelocityNoise             2.0978
    8            AccelerationNoise         2.0944
    8            GyroscopeBiasNoise        2.0944
    8            AccelerometerBiasNoise    2.0944
    8            GeomagneticVectorNoise    2.0944
    8            MagnetometerBiasNoise     2.0822
    9            AccelerometerNoise        2.0771
    9            GyroscopeNoise            2.0758
    9            MagnetometerNoise         2.0754
    9            GPSPositionNoise          2.0645
    9            GPSVelocityNoise          2.0028
    9            QuaternionNoise           2.0027
    9            AngularVelocityNoise      1.9943
    9            PositionNoise             1.9943
    9            VelocityNoise             1.9943
    9            AccelerationNoise         1.9883
    9            GyroscopeBiasNoise        1.9883
    9            AccelerometerBiasNoise    1.9876
    9            GeomagneticVectorNoise    1.9876
    9            MagnetometerBiasNoise     1.9770
    10           AccelerometerNoise        1.9707
    10           GyroscopeNoise            1.9707
    10           MagnetometerNoise         1.9706
    10           GPSPositionNoise          1.9564
    10           GPSVelocityNoise          1.9005
    10           QuaternionNoise           1.9003
    10           AngularVelocityNoise      1.9003
    10           PositionNoise             1.9003
    10           VelocityNoise             1.9003
    10           AccelerationNoise         1.8999
    10           GyroscopeBiasNoise        1.8999
    10           AccelerometerBiasNoise    1.8992
    10           GeomagneticVectorNoise    1.8992
    10           MagnetometerBiasNoise     1.8885
    11           AccelerometerNoise        1.8815
    11           GyroscopeNoise            1.8807
    11           MagnetometerNoise         1.8806
    11           GPSPositionNoise          1.8674
    11           GPSVelocityNoise          1.8111
    11           QuaternionNoise           1.8110
    11           AngularVelocityNoise      1.8110
    11           PositionNoise             1.8110
    11           VelocityNoise             1.8110
    11           AccelerationNoise         1.8094
    11           GyroscopeBiasNoise        1.8094
    11           AccelerometerBiasNoise    1.8085
    11           GeomagneticVectorNoise    1.8085
    11           MagnetometerBiasNoise     1.7984
    12           AccelerometerNoise        1.7921
    12           GyroscopeNoise            1.7919
    12           MagnetometerNoise         1.7902
    12           GPSPositionNoise          1.7774
    12           GPSVelocityNoise          1.7315
    12           QuaternionNoise           1.7313
    12           AngularVelocityNoise      1.7282
    12           PositionNoise             1.7282
    12           VelocityNoise             1.7282
    12           AccelerationNoise         1.7278
    12           GyroscopeBiasNoise        1.7278
    12           AccelerometerBiasNoise    1.7268
    12           GeomagneticVectorNoise    1.7268
    12           MagnetometerBiasNoise     1.7160
    13           AccelerometerNoise        1.7103
    13           GyroscopeNoise            1.7094
    13           MagnetometerNoise         1.7094
    13           GPSPositionNoise          1.6980
    13           GPSVelocityNoise          1.6589
    13           QuaternionNoise           1.6587
    13           AngularVelocityNoise      1.6587
    13           PositionNoise             1.6587
    13           VelocityNoise             1.6587
    13           AccelerationNoise         1.6575
    13           GyroscopeBiasNoise        1.6575
    13           AccelerometerBiasNoise    1.6566
    13           GeomagneticVectorNoise    1.6566
    13           MagnetometerBiasNoise     1.6480
    14           AccelerometerNoise        1.6432
    14           GyroscopeNoise            1.6422
    14           MagnetometerNoise         1.6415
    14           GPSPositionNoise          1.6330
    14           GPSVelocityNoise          1.6097
    14           QuaternionNoise           1.6095
    14           AngularVelocityNoise      1.6093
    14           PositionNoise             1.6093
    14           VelocityNoise             1.6093
    14           AccelerationNoise         1.6083
    14           GyroscopeBiasNoise        1.6083
    14           AccelerometerBiasNoise    1.6077
    14           GeomagneticVectorNoise    1.6077
    14           MagnetometerBiasNoise     1.6012
    15           AccelerometerNoise        1.5972
    15           GyroscopeNoise            1.5949
    15           MagnetometerNoise         1.5942
    15           GPSPositionNoise          1.5886
    15           GPSVelocityNoise          1.5802
    15           QuaternionNoise           1.5801
    15           AngularVelocityNoise      1.5786
    15           PositionNoise             1.5786
    15           VelocityNoise             1.5786
    15           AccelerationNoise         1.5786
    15           GyroscopeBiasNoise        1.5786
    15           AccelerometerBiasNoise    1.5783
    15           GeomagneticVectorNoise    1.5783
    15           MagnetometerBiasNoise     1.5737

[posTunedEst, orientTunedEst] = fuse(filtTuned,sensorData,tunedmn);

Compare Tuned vs Untuned Filter

Plot the position estimates from the tuned and untuned filters along with the ground truth positions. Then, plot the orientation error (quaternion distance) in degrees for both tuned and untuned filters. The tuned filter estimates the position and orientation better than the untuned filter.

% Position error
figure;
t = sensorData.Time;
subplot(3,1,1);
plot(t, [posTunedEst(:,1) posUntunedEst(:,1) Position(:,1)]);
title('Position');
ylabel('X-axis');
legend('Tuned', 'Untuned', 'Truth');
subplot(3,1,2);
plot(t, [posTunedEst(:,2) posUntunedEst(:,2) Position(:,2)]);
ylabel('Y-axis');
subplot(3,1,3);
plot(t, [posTunedEst(:,3) posUntunedEst(:,3) Position(:,3)]);
ylabel('Z-axis');

% Orientation Error
figure;
plot(t, rad2deg(dist(Orientation, orientTunedEst)), ...
    t, rad2deg(dist(Orientation, orientUntunedEst)));
title('Orientation Error');
ylabel('Degrees');
legend('Tuned', 'Untuned');

Navigation Toolbox Documentation

Support