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.
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);
The insfilterAsync
filter fuses data from multiple sensors operating asynchronously
filtUntuned = insfilterAsync;
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);
sensorData
with an Untuned FilterUse 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);
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);
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');