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');
You can also select a web site from the following list:
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)