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 3.9157
1 GyroscopeNoise 3.9157
1 MagnetometerNoise 3.8618
1 GPSPositionNoise 3.8162
1 GPSVelocityNoise 3.6551
1 QuaternionNoise 3.6522
1 AngularVelocityNoise 3.4632
1 PositionNoise 3.4632
1 VelocityNoise 3.4632
1 AccelerationNoise 3.4420
1 GyroscopeBiasNoise 3.4420
1 AccelerometerBiasNoise 3.4389
1 GeomagneticVectorNoise 3.4389
1 MagnetometerBiasNoise 3.4389
2 AccelerometerNoise 3.4389
2 GyroscopeNoise 3.4389
2 MagnetometerNoise 3.3792
2 GPSPositionNoise 3.3792
2 GPSVelocityNoise 3.2526
2 QuaternionNoise 3.2506
2 AngularVelocityNoise 2.6514
2 PositionNoise 2.6514
2 VelocityNoise 2.6514
2 AccelerationNoise 2.6514
2 GyroscopeBiasNoise 2.6514
2 AccelerometerBiasNoise 2.6496
2 GeomagneticVectorNoise 2.6496
2 MagnetometerBiasNoise 2.6386
3 AccelerometerNoise 2.6351
3 GyroscopeNoise 2.6351
3 MagnetometerNoise 2.6165
3 GPSPositionNoise 2.6165
3 GPSVelocityNoise 2.5219
3 QuaternionNoise 2.4994
3 AngularVelocityNoise 2.4183
3 PositionNoise 2.4183
3 VelocityNoise 2.4183
3 AccelerationNoise 2.4032
3 GyroscopeBiasNoise 2.4032
3 AccelerometerBiasNoise 2.3922
3 GeomagneticVectorNoise 2.3922
3 MagnetometerBiasNoise 2.3922
4 AccelerometerNoise 2.3888
4 GyroscopeNoise 2.3888
4 MagnetometerNoise 2.3888
4 GPSPositionNoise 2.3888
4 GPSVelocityNoise 2.2745
4 QuaternionNoise 2.2641
4 AngularVelocityNoise 2.1749
4 PositionNoise 2.1749
4 VelocityNoise 2.1749
4 AccelerationNoise 2.1628
4 GyroscopeBiasNoise 2.1628
4 AccelerometerBiasNoise 2.1624
4 GeomagneticVectorNoise 2.1624
4 MagnetometerBiasNoise 2.1589
5 AccelerometerNoise 2.1587
5 GyroscopeNoise 2.1587
5 MagnetometerNoise 2.1587
5 GPSPositionNoise 2.1573
5 GPSVelocityNoise 2.0520
5 QuaternionNoise 2.0518
5 AngularVelocityNoise 2.0044
5 PositionNoise 2.0044
5 VelocityNoise 2.0044
5 AccelerationNoise 2.0013
5 GyroscopeBiasNoise 2.0013
5 AccelerometerBiasNoise 1.9900
5 GeomagneticVectorNoise 1.9900
5 MagnetometerBiasNoise 1.9900
6 AccelerometerNoise 1.9893
6 GyroscopeNoise 1.9893
6 MagnetometerNoise 1.9893
6 GPSPositionNoise 1.9893
6 GPSVelocityNoise 1.8894
6 QuaternionNoise 1.8894
6 AngularVelocityNoise 1.8273
6 PositionNoise 1.8273
6 VelocityNoise 1.8273
6 AccelerationNoise 1.8189
6 GyroscopeBiasNoise 1.8189
6 AccelerometerBiasNoise 1.8189
6 GeomagneticVectorNoise 1.8188
6 MagnetometerBiasNoise 1.8188
7 AccelerometerNoise 1.8188
7 GyroscopeNoise 1.8188
7 MagnetometerNoise 1.8177
7 GPSPositionNoise 1.8157
7 GPSVelocityNoise 1.7122
7 QuaternionNoise 1.7122
7 AngularVelocityNoise 1.6420
7 PositionNoise 1.6420
7 VelocityNoise 1.6420
7 AccelerationNoise 1.6420
7 GyroscopeBiasNoise 1.6420
7 AccelerometerBiasNoise 1.6412
7 GeomagneticVectorNoise 1.6412
7 MagnetometerBiasNoise 1.6412
8 AccelerometerNoise 1.6412
8 GyroscopeNoise 1.6411
8 MagnetometerNoise 1.6411
8 GPSPositionNoise 1.6383
8 GPSVelocityNoise 1.5216
8 QuaternionNoise 1.5216
8 AngularVelocityNoise 1.5004
8 PositionNoise 1.5004
8 VelocityNoise 1.5004
8 AccelerationNoise 1.5000
8 GyroscopeBiasNoise 1.5000
8 AccelerometerBiasNoise 1.5000
8 GeomagneticVectorNoise 1.5000
8 MagnetometerBiasNoise 1.4998
9 AccelerometerNoise 1.4998
9 GyroscopeNoise 1.4998
9 MagnetometerNoise 1.4998
9 GPSPositionNoise 1.4998
9 GPSVelocityNoise 1.4106
9 QuaternionNoise 1.4106
9 AngularVelocityNoise 1.3916
9 PositionNoise 1.3916
9 VelocityNoise 1.3916
9 AccelerationNoise 1.3916
9 GyroscopeBiasNoise 1.3916
9 AccelerometerBiasNoise 1.3911
9 GeomagneticVectorNoise 1.3911
9 MagnetometerBiasNoise 1.3911
10 AccelerometerNoise 1.3911
10 GyroscopeNoise 1.3910
10 MagnetometerNoise 1.3910
10 GPSPositionNoise 1.3892
10 GPSVelocityNoise 1.2977
10 QuaternionNoise 1.2977
10 AngularVelocityNoise 1.2977
10 PositionNoise 1.2977
10 VelocityNoise 1.2977
10 AccelerationNoise 1.2975
10 GyroscopeBiasNoise 1.2975
10 AccelerometerBiasNoise 1.2975
10 GeomagneticVectorNoise 1.2975
10 MagnetometerBiasNoise 1.2974
11 AccelerometerNoise 1.2973
11 GyroscopeNoise 1.2973
11 MagnetometerNoise 1.2973
11 GPSPositionNoise 1.2973
11 GPSVelocityNoise 1.2031
11 QuaternionNoise 1.2030
11 AngularVelocityNoise 1.1998
11 PositionNoise 1.1998
11 VelocityNoise 1.1998
11 AccelerationNoise 1.1998
11 GyroscopeBiasNoise 1.1998
11 AccelerometerBiasNoise 1.1995
11 GeomagneticVectorNoise 1.1995
11 MagnetometerBiasNoise 1.1994
12 AccelerometerNoise 1.1993
12 GyroscopeNoise 1.1992
12 MagnetometerNoise 1.1992
12 GPSPositionNoise 1.1982
12 GPSVelocityNoise 1.1009
12 QuaternionNoise 1.1008
12 AngularVelocityNoise 1.0972
12 PositionNoise 1.0972
12 VelocityNoise 1.0972
12 AccelerationNoise 1.0972
12 GyroscopeBiasNoise 1.0972
12 AccelerometerBiasNoise 1.0968
12 GeomagneticVectorNoise 1.0968
12 MagnetometerBiasNoise 1.0967
13 AccelerometerNoise 1.0966
13 GyroscopeNoise 1.0965
13 MagnetometerNoise 1.0965
13 GPSPositionNoise 1.0955
13 GPSVelocityNoise 0.9830
13 QuaternionNoise 0.9829
13 AngularVelocityNoise 0.9829
13 PositionNoise 0.9829
13 VelocityNoise 0.9829
13 AccelerationNoise 0.9829
13 GyroscopeBiasNoise 0.9829
13 AccelerometerBiasNoise 0.9825
13 GeomagneticVectorNoise 0.9825
13 MagnetometerBiasNoise 0.9824
14 AccelerometerNoise 0.9822
14 GyroscopeNoise 0.9822
14 MagnetometerNoise 0.9822
14 GPSPositionNoise 0.9812
14 GPSVelocityNoise 0.8868
14 QuaternionNoise 0.8867
14 AngularVelocityNoise 0.8816
14 PositionNoise 0.8816
14 VelocityNoise 0.8816
14 AccelerationNoise 0.8815
14 GyroscopeBiasNoise 0.8815
14 AccelerometerBiasNoise 0.8812
14 GeomagneticVectorNoise 0.8812
14 MagnetometerBiasNoise 0.8811
15 AccelerometerNoise 0.8809
15 GyroscopeNoise 0.8809
15 MagnetometerNoise 0.8809
15 GPSPositionNoise 0.8799
15 GPSVelocityNoise 0.7916
15 QuaternionNoise 0.7916
15 AngularVelocityNoise 0.7916
15 PositionNoise 0.7916
15 VelocityNoise 0.7916
15 AccelerationNoise 0.7916
15 GyroscopeBiasNoise 0.7916
15 AccelerometerBiasNoise 0.7913
15 GeomagneticVectorNoise 0.7913
15 MagnetometerBiasNoise 0.7913
[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');
