Main Content

Automatic Tuning of the insfilterAsync Filter

The insfilterAsync (Navigation Toolbox) 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 (Navigation Toolbox) 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.5898
    1            GyroscopeNoise            4.5694
    1            MagnetometerNoise         4.5481
    1            GPSPositionNoise          4.4737
    1            GPSVelocityNoise          4.2984
    1            QuaternionNoise           4.2984
    1            AngularVelocityNoise      3.9668
    1            PositionNoise             3.9668
    1            VelocityNoise             3.9668
    1            AccelerationNoise         3.9556
    1            GyroscopeBiasNoise        3.9556
    1            AccelerometerBiasNoise    3.9511
    1            GeomagneticVectorNoise    3.9511
    1            MagnetometerBiasNoise     3.9360
    2            AccelerometerNoise        3.9360
    2            GyroscopeNoise            3.9360
    2            MagnetometerNoise         3.9064
    2            GPSPositionNoise          3.9064
    2            GPSVelocityNoise          3.7129
    2            QuaternionNoise           3.7122
    2            AngularVelocityNoise      3.0116
    2            PositionNoise             3.0116
    2            VelocityNoise             3.0116
    2            AccelerationNoise         2.9850
    2            GyroscopeBiasNoise        2.9850
    2            AccelerometerBiasNoise    2.9824
    2            GeomagneticVectorNoise    2.9824
    2            MagnetometerBiasNoise     2.9821
    3            AccelerometerNoise        2.9821
    3            GyroscopeNoise            2.9613
    3            MagnetometerNoise         2.9432
    3            GPSPositionNoise          2.9432
    3            GPSVelocityNoise          2.8373
    3            QuaternionNoise           2.8369
    3            AngularVelocityNoise      2.6993
    3            PositionNoise             2.6993
    3            VelocityNoise             2.6993
    3            AccelerationNoise         2.6993
    3            GyroscopeBiasNoise        2.6993
    3            AccelerometerBiasNoise    2.6971
    3            GeomagneticVectorNoise    2.6971
    3            MagnetometerBiasNoise     2.6955
    4            AccelerometerNoise        2.6941
    4            GyroscopeNoise            2.6797
    4            MagnetometerNoise         2.6676
    4            GPSPositionNoise          2.6626
    4            GPSVelocityNoise          2.5530
    4            QuaternionNoise           2.5530
    4            AngularVelocityNoise      2.4285
    4            PositionNoise             2.4285
    4            VelocityNoise             2.4285
    4            AccelerationNoise         2.4232
    4            GyroscopeBiasNoise        2.4232
    4            AccelerometerBiasNoise    2.4217
    4            GeomagneticVectorNoise    2.4217
    4            MagnetometerBiasNoise     2.4023
    5            AccelerometerNoise        2.4019
    5            GyroscopeNoise            2.3900
    5            MagnetometerNoise         2.3900
    5            GPSPositionNoise          2.3887
    5            GPSVelocityNoise          2.2986
    5            QuaternionNoise           2.2986
    5            AngularVelocityNoise      2.1945
    5            PositionNoise             2.1945
    5            VelocityNoise             2.1945
    5            AccelerationNoise         2.1863
    5            GyroscopeBiasNoise        2.1863
    5            AccelerometerBiasNoise    2.1856
    5            GeomagneticVectorNoise    2.1856
    5            MagnetometerBiasNoise     2.1615
    6            AccelerometerNoise        2.1613
    6            GyroscopeNoise            2.1393
    6            MagnetometerNoise         2.1199
    6            GPSPositionNoise          2.1112
    6            GPSVelocityNoise          2.0140
    6            QuaternionNoise           2.0140
    6            AngularVelocityNoise      1.9288
    6            PositionNoise             1.9288
    6            VelocityNoise             1.9288
    6            AccelerationNoise         1.9242
    6            GyroscopeBiasNoise        1.9242
    6            AccelerometerBiasNoise    1.9216
    6            GeomagneticVectorNoise    1.9216
    6            MagnetometerBiasNoise     1.9054
    7            AccelerometerNoise        1.9047
    7            GyroscopeNoise            1.8887
    7            MagnetometerNoise         1.8820
    7            GPSPositionNoise          1.8803
    7            GPSVelocityNoise          1.7713
    7            QuaternionNoise           1.7712
    7            AngularVelocityNoise      1.7145
    7            PositionNoise             1.7145
    7            VelocityNoise             1.7145
    7            AccelerationNoise         1.7122
    7            GyroscopeBiasNoise        1.7122
    7            AccelerometerBiasNoise    1.7112
    7            GeomagneticVectorNoise    1.7112
    7            MagnetometerBiasNoise     1.6873
    8            AccelerometerNoise        1.6853
    8            GyroscopeNoise            1.6790
    8            MagnetometerNoise         1.6694
    8            GPSPositionNoise          1.6565
    8            GPSVelocityNoise          1.5700
    8            QuaternionNoise           1.5676
    8            AngularVelocityNoise      1.5366
    8            PositionNoise             1.5366
    8            VelocityNoise             1.5366
    8            AccelerationNoise         1.5366
    8            GyroscopeBiasNoise        1.5366
    8            AccelerometerBiasNoise    1.5353
    8            GeomagneticVectorNoise    1.5337
    8            MagnetometerBiasNoise     1.5166
    9            AccelerometerNoise        1.5129
    9            GyroscopeNoise            1.5117
    9            MagnetometerNoise         1.5114
    9            GPSPositionNoise          1.4953
    9            GPSVelocityNoise          1.4106
    9            QuaternionNoise           1.4106
    9            AngularVelocityNoise      1.3742
    9            PositionNoise             1.3742
    9            VelocityNoise             1.3742
    9            AccelerationNoise         1.3731
    9            GyroscopeBiasNoise        1.3731
    9            AccelerometerBiasNoise    1.3715
    9            GeomagneticVectorNoise    1.3715
    9            MagnetometerBiasNoise     1.3576
    10           AccelerometerNoise        1.3528
    10           GyroscopeNoise            1.3528
    10           MagnetometerNoise         1.3510
    10           GPSPositionNoise          1.3322
    10           GPSVelocityNoise          1.2512
    10           QuaternionNoise           1.2512
    10           AngularVelocityNoise      1.2507
    10           PositionNoise             1.2507
    10           VelocityNoise             1.2507
    10           AccelerationNoise         1.2497
    10           GyroscopeBiasNoise        1.2497
    10           AccelerometerBiasNoise    1.2480
    10           GeomagneticVectorNoise    1.2480
    10           MagnetometerBiasNoise     1.2301
    11           AccelerometerNoise        1.2233
    11           GyroscopeNoise            1.2222
    11           MagnetometerNoise         1.2220
    11           GPSPositionNoise          1.2008
    11           GPSVelocityNoise          1.1043
    11           QuaternionNoise           1.1043
    11           AngularVelocityNoise      1.1038
    11           PositionNoise             1.1038
    11           VelocityNoise             1.1038
    11           AccelerationNoise         1.1028
    11           GyroscopeBiasNoise        1.1028
    11           AccelerometerBiasNoise    1.1013
    11           GeomagneticVectorNoise    1.1013
    11           MagnetometerBiasNoise     1.0867
    12           AccelerometerNoise        1.0782
    12           GyroscopeNoise            1.0767
    12           MagnetometerNoise         1.0733
    12           GPSPositionNoise          1.0505
    12           GPSVelocityNoise          0.9564
    12           QuaternionNoise           0.9563
    12           AngularVelocityNoise      0.9563
    12           PositionNoise             0.9563
    12           VelocityNoise             0.9563
    12           AccelerationNoise         0.9550
    12           GyroscopeBiasNoise        0.9550
    12           AccelerometerBiasNoise    0.9534
    12           GeomagneticVectorNoise    0.9534
    12           MagnetometerBiasNoise     0.9402
    13           AccelerometerNoise        0.9303
    13           GyroscopeNoise            0.9291
    13           MagnetometerNoise         0.9269
    13           GPSPositionNoise          0.9036
    13           GPSVelocityNoise          0.8072
    13           QuaternionNoise           0.8071
    13           AngularVelocityNoise      0.8071
    13           PositionNoise             0.8071
    13           VelocityNoise             0.8071
    13           AccelerationNoise         0.8065
    13           GyroscopeBiasNoise        0.8065
    13           AccelerometerBiasNoise    0.8052
    13           GeomagneticVectorNoise    0.8052
    13           MagnetometerBiasNoise     0.7901
    14           AccelerometerNoise        0.7806
    14           GyroscopeNoise            0.7806
    14           MagnetometerNoise         0.7768
    14           GPSPositionNoise          0.7547
    14           GPSVelocityNoise          0.6932
    14           QuaternionNoise           0.6930
    14           AngularVelocityNoise      0.6923
    14           PositionNoise             0.6923
    14           VelocityNoise             0.6923
    14           AccelerationNoise         0.6906
    14           GyroscopeBiasNoise        0.6906
    14           AccelerometerBiasNoise    0.6896
    14           GeomagneticVectorNoise    0.6896
    14           MagnetometerBiasNoise     0.6801
    15           AccelerometerNoise        0.6704
    15           GyroscopeNoise            0.6676
    15           MagnetometerNoise         0.6653
    15           GPSPositionNoise          0.6469
    15           GPSVelocityNoise          0.6047
    15           QuaternionNoise           0.6047
    15           AngularVelocityNoise      0.6037
    15           PositionNoise             0.6037
    15           VelocityNoise             0.6037
    15           AccelerationNoise         0.6019
    15           GyroscopeBiasNoise        0.6019
    15           AccelerometerBiasNoise    0.6015
    15           GeomagneticVectorNoise    0.6015
    15           MagnetometerBiasNoise     0.5913
[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');