From 3a7a66d360574d991564718412479cb5654e221b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 10 Jun 2017 11:35:07 +1000 Subject: [PATCH] matlab: improve EKF simulation fusion control Add ability to start before GPS checks pass. Add ability to turn GPS fusion off and on during replay. Add ability to turn Optical Flow fusion off and on during replay. Add ability to turn Visual Odometry fusion off and on during replay. Convert miscellaneous constants to parameters --- matlab/EKF_replay/Filter/InitStates.m | 30 ++-- matlab/EKF_replay/Filter/RunFilter.m | 129 +++++++++++++----- .../EKF_replay/Filter/SetParameterDefaults.m | 20 ++- matlab/EKF_replay/Filter/replay_apm_data.m | 11 +- matlab/EKF_replay/Filter/replay_px4_data.m | 9 ++ matlab/EKF_replay/readme.txt | 3 +- 6 files changed, 152 insertions(+), 50 deletions(-) diff --git a/matlab/EKF_replay/Filter/InitStates.m b/matlab/EKF_replay/Filter/InitStates.m index 249163dba9..69b4762082 100644 --- a/matlab/EKF_replay/Filter/InitStates.m +++ b/matlab/EKF_replay/Filter/InitStates.m @@ -7,9 +7,13 @@ deg2rad = pi/180; states = zeros(24,1); quat = [1;0;0;0]; -% find IMU start index that coresponds to first valid GPS data -imu_start_index = (find(imu_data.time_us > gps_data.time_us(gps_data.start_index), 1, 'first' ) - 50); -imu_start_index = max(imu_start_index,1); +if (param.control.waitForGps == 1) + % find IMU start index that coresponds to first valid GPS data + imu_start_index = (find(imu_data.time_us > gps_data.time_us(gps_data.start_index), 1, 'first' ) - 50); + imu_start_index = max(imu_start_index,1); +else + imu_start_index = 1; +end % average first 100 accel readings to reduce effect of vibration initAccel(1) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,1)); @@ -21,8 +25,8 @@ initAccel(3) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,3)); quat = AlignTilt(quat,initAccel); states(1:4) = quat; -% find magnetometer start index that coresponds to first valid GPS data -mag_start_index = (find(mag_data.time_us > gps_data.time_us(gps_data.start_index), 1, 'first' ) - 5); +% find magnetometer start index +mag_start_index = (find(mag_data.time_us > imu_data.time_us(imu_start_index), 1, 'first' ) - 5); mag_start_index = max(mag_start_index,1); % mean to reduce effect of noise in data @@ -37,12 +41,18 @@ quat = AlignHeading(quat,magBody,param.fusion.magDeclDeg*deg2rad); Tbn = Quat2Tbn(quat); states(17:19) = Tbn*magBody; -% initialise velocity and position using gps -states(5:7) = gps_data.vel_ned(gps_data.start_index,:); -states(8:9) = gps_data.pos_ned(gps_data.start_index,1:2); +if (param.control.waitForGps == 1) + % initialise velocity and position using gps + states(5:7) = gps_data.vel_ned(gps_data.start_index,:); + states(8:9) = gps_data.pos_ned(gps_data.start_index,1:2); +else + % initialise to be stationary at the origin + states(5:7) = zeros(1,3); + states(8:9) = zeros(1,2); +end -% find baro start index that coresponds to first valid GPS data -baro_start_index = (find(baro_data.time_us > gps_data.time_us(gps_data.start_index), 1, 'first' ) - 10); +% find baro start index +baro_start_index = (find(baro_data.time_us > imu_data.time_us(imu_start_index), 1, 'first' ) - 10); baro_start_index = max(baro_start_index,1); % average baro data and initialise the vertical position diff --git a/matlab/EKF_replay/Filter/RunFilter.m b/matlab/EKF_replay/Filter/RunFilter.m index 7b719d18e6..1d445a4cc2 100644 --- a/matlab/EKF_replay/Filter/RunFilter.m +++ b/matlab/EKF_replay/Filter/RunFilter.m @@ -16,24 +16,24 @@ function output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,varargin) nVarargs = length(varargin); if nVarargs >= 2 - useOpticalFlow = ~isempty(varargin{1}) && ~isempty(varargin{2}); + flowDataPresent = ~isempty(varargin{1}) && ~isempty(varargin{2}); rng_data = varargin{1}; flow_data = varargin{2}; - if useOpticalFlow + if flowDataPresent fprintf('Using optical Flow Data\n',nVarargs); end else - useOpticalFlow = 0; + flowDataPresent = 0; end if nVarargs >= 3 - useVisualOdometry = ~isempty(varargin{3}); + visoDataPresent = ~isempty(varargin{3}); viso_data = varargin{3}; - if useVisualOdometry + if visoDataPresent fprintf('Using ZED camera odometry data\n',nVarargs); end else - useVisualOdometry = 0; + visoDataPresent = 0; end @@ -43,7 +43,7 @@ end deg2rad = pi/180; gravity = 9.80665; % initial value of gravity - will be updated when WGS-84 position is known -% initialise the state vector at the first position where there is OK GPS +% initialise the state vector [states, imu_start_index] = InitStates(param,imu_data,gps_data,mag_data,baro_data); dt_imu_avg = 0.5 * (median(imu_data.gyro_dt) + median(imu_data.accel_dt)); @@ -69,6 +69,14 @@ output = struct('time_lapsed',[]',... covariance = InitCovariance(param,dt_imu_avg,1,gps_data); %% Main Loop + +% control flags +gps_use_started = boolean(false); +gps_use_blocked = boolean(false); +viso_use_blocked = boolean(false); +flow_use_blocked = boolean(false); + +% array access index variables imuIndex = imu_start_index; last_gps_index = 0; gps_fuse_index = 0; @@ -80,18 +88,27 @@ last_flow_index = 0; flow_fuse_index = 0; last_viso_index = 0; viso_fuse_index = 0; -delAngCov = [0;0;0]; -delVelCov = [0;0;0]; -dtCov = 0; -dtCovInt = 0; -covIndex = 0; +last_range_index = 0; + +% covariance prediction variables +delAngCov = [0;0;0]; % delta angle vector used by the covariance prediction (rad) +delVelCov = [0;0;0]; % delta velocity vector used by the covariance prediction (m/sec) +dtCov = 0; % time step used by the covariance prediction (sec) +dtCovInt = 0; % accumulated time step of covariance predictions (sec) +covIndex = 0; % covariance prediction frame counter + output.magFuseMethod = param.fusion.magFuseMethod; range = 0.1; -latest_range_index = 1; + +% variables used to control dead-reckoning timeout +last_drift_constrain_time = - param.control.velDriftTimeLim; +last_synthetic_velocity_fusion_time = 0; +last_valid_range_time = - param.fusion.rngTimeout; + for index = indexStart:indexStop % read IMU measurements - localTime=imu_data.time_us(imuIndex)*1e-6; + local_time=imu_data.time_us(imuIndex)*1e-6; delta_angle(:,1) = imu_data.del_ang(imuIndex,:); delta_velocity(:,1) = imu_data.del_vel(imuIndex,:); dt_imu = 0.5 * (imu_data.accel_dt(imuIndex) + imu_data.gyro_dt(imuIndex)); @@ -116,7 +133,7 @@ for index = indexStart:indexStop covIndex = covIndex + 1; % output state data - output.time_lapsed(covIndex) = localTime; + output.time_lapsed(covIndex) = local_time; output.euler_angles(covIndex,:) = QuatToEul(states(1:4)')'; output.velocity_NED(covIndex,:) = states(5:7)'; output.position_NED(covIndex,:) = states(8:10)'; @@ -131,17 +148,38 @@ for index = indexStart:indexStop output.state_variances(covIndex,i) = covariance(i,i); end - % Fuse new GPS data that has fallen beind the fusion time horizon + % Get most recent GPS data that had fallen behind the fusion time horizon latest_gps_index = find((gps_data.time_us - 1e6 * param.fusion.gpsTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); - if (latest_gps_index > last_gps_index) + + % Check if GPS use is being blocked by the user + if ((local_time < param.control.gpsOnTime) && (local_time > param.control.gpsOffTime)) + gps_use_started = false; + gps_use_blocked = true; + else + gps_use_blocked = false; + end + + % If we haven't started using GPS, check that the quality is sufficient before aligning the position and velocity states to GPS + if (~gps_use_started && ~gps_use_blocked) + if ((gps_data.spd_error(latest_gps_index) < param.control.gpsSpdErrLim) && (gps_data.pos_error(latest_gps_index) < param.control.gpsPosErrLim)) + states(5:7) = gps_data.vel_ned(latest_gps_index,:); + states(8:9) = gps_data.pos_ned(latest_gps_index,1:2); + gps_use_started = true; + last_drift_constrain_time = local_time; + end + end + + % Fuse GPS data when available if GPS use has started + if (gps_use_started && ~gps_use_blocked && (latest_gps_index > last_gps_index)) last_gps_index = latest_gps_index; gps_fuse_index = gps_fuse_index + 1; + last_drift_constrain_time = local_time; % fuse NED GPS velocity [states,covariance,velInnov,velInnovVar] = FuseVelocity(states,covariance,gps_data.vel_ned(latest_gps_index,:),param.fusion.gpsVelGate,gps_data.spd_error(latest_gps_index)); % data logging - output.innovations.vel_time_lapsed(gps_fuse_index) = localTime; + output.innovations.vel_time_lapsed(gps_fuse_index) = local_time; output.innovations.vel_innov(gps_fuse_index,:) = velInnov'; output.innovations.vel_innov_var(gps_fuse_index,:) = velInnovVar'; @@ -149,9 +187,17 @@ for index = indexStart:indexStop [states,covariance,posInnov,posInnovVar] = FusePosition(states,covariance,gps_data.pos_ned(latest_gps_index,:),param.fusion.gpsPosGate,gps_data.pos_error(latest_gps_index)); % data logging - output.innovations.pos_time_lapsed(gps_fuse_index) = localTime; + output.innovations.pos_time_lapsed(gps_fuse_index) = local_time; output.innovations.posInnov(gps_fuse_index,:) = posInnov'; output.innovations.posInnovVar(gps_fuse_index,:) = posInnovVar'; + else + % Check if drift is being corrected by some form of aiding and if not, fuse in a zero position measurement at 5Hz to prevent states diverging + if ((local_time - last_drift_constrain_time) > param.control.velDriftTimeLim) + if ((local_time - last_synthetic_velocity_fusion_time) > 0.2) + [states,covariance,~,~] = FusePosition(states,covariance,zeros(1,2),100.0,param.control.gpsPosErrLim); + last_synthetic_velocity_fusion_time = local_time; + end + end end % Fuse new Baro data that has fallen beind the fusion time horizon @@ -164,7 +210,7 @@ for index = indexStart:indexStop [states,covariance,hgtInnov,hgtInnovVar] = FuseBaroHeight(states,covariance,baro_data.height(latest_baro_index),param.fusion.baroHgtGate,param.fusion.baroHgtNoise); % data logging - output.innovations.hgt_time_lapsed(baro_fuse_index) = localTime; + output.innovations.hgt_time_lapsed(baro_fuse_index) = local_time; output.innovations.hgtInnov(baro_fuse_index) = hgtInnov; output.innovations.hgtInnovVar(baro_fuse_index) = hgtInnovVar; end @@ -183,7 +229,7 @@ for index = indexStart:indexStop [states,covariance,magInnov,magInnovVar] = FuseMagnetometer(states,covariance,mag_data.field_ga(latest_mag_index,:),param.fusion.magFieldGate, param.fusion.magFieldError^2); % data logging - output.innovations.mag_time_lapsed(mag_fuse_index) = localTime; + output.innovations.mag_time_lapsed(mag_fuse_index) = local_time; output.innovations.magInnov(mag_fuse_index,:) = magInnov; output.innovations.magInnovVar(mag_fuse_index,:) = magInnovVar; @@ -198,7 +244,7 @@ for index = indexStart:indexStop [states, covariance, hdgInnov, hdgInnovVar] = FuseMagHeading(states, covariance, mag_data.field_ga(latest_mag_index,:), param.fusion.magDeclDeg*deg2rad, param.fusion.magHdgGate, param.fusion.magHdgError^2); % log data - output.innovations.mag_time_lapsed(mag_fuse_index) = localTime; + output.innovations.mag_time_lapsed(mag_fuse_index) = local_time; output.innovations.hdgInnov(mag_fuse_index) = hdgInnov; output.innovations.hdgInnovVar(mag_fuse_index) = hdgInnovVar; @@ -206,20 +252,29 @@ for index = indexStart:indexStop end - % Attempt to use optical flow and range finder data if available - if (useOpticalFlow) + % Check if optical flow use is being blocked by the user + if ((local_time < param.control.flowOnTime) && (local_time > param.control.flowOffTime)) + flow_use_blocked = true; + else + flow_use_blocked = false; + end + + % Attempt to use optical flow and range finder data if available and not blocked + if (flowDataPresent && ~flow_use_blocked) % Get latest range finder data and gate to remove dropouts - latest_range_index = find((rng_data.time_us - 1e6 * param.fusion.rangeTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); - if (rng_data.dist(latest_range_index) < 5.0 && rng_data.dist(latest_range_index) > 0.05) - range = rng_data.dist(latest_range_index); + last_range_index = find((rng_data.time_us - 1e6 * param.fusion.rangeTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); + if (rng_data.dist(last_range_index) < param.fusion.rngValidMin && rng_data.dist(last_range_index) > param.fusion.rngValidMin) + range = rng_data.dist(last_range_index); + last_valid_range_time = local_time; end - % Fuse optical flow data that has fallen behind the fusion time horizon + % Fuse optical flow data that has fallen behind the fusion time horizon if we have a valid range measurement latest_flow_index = find((flow_data.time_us - 1e6 * param.fusion.flowTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); - if (latest_flow_index > last_flow_index) + if ((latest_flow_index > last_flow_index) && ((local_time - last_valid_range_time) < param.fusion.rngTimeout)) last_flow_index = latest_flow_index; flow_fuse_index = flow_fuse_index + 1; + last_drift_constrain_time = local_time; % fuse flow data flowRate = [flow_data.flowX(latest_flow_index);flow_data.flowY(latest_flow_index)]; @@ -227,7 +282,7 @@ for index = indexStart:indexStop [states,covariance,flowInnov,flowInnovVar] = FuseOpticalFlow(states, covariance, flowRate, bodyRate, range, param.fusion.flowRateError^2, param.fusion.flowGate); % data logging - output.innovations.flow_time_lapsed(flow_fuse_index) = localTime; + output.innovations.flow_time_lapsed(flow_fuse_index) = local_time; output.innovations.flowInnov(flow_fuse_index,:) = flowInnov; output.innovations.flowInnovVar(flow_fuse_index,:) = flowInnovVar; @@ -235,14 +290,22 @@ for index = indexStart:indexStop end - % attempt to use ZED camera visual odmetry data if available - if (useVisualOdometry) + % Check if optical flow use is being blocked by the user + if ((local_time < param.control.visoOnTime) && (local_time > param.control.visoOffTime)) + viso_use_blocked = true; + else + viso_use_blocked = false; + end + + % attempt to use ZED camera visual odmetry data if available and not blocked + if (visoDataPresent && ~viso_use_blocked) % Fuse ZED camera body frame odmometry data that has fallen behind the fusion time horizon latest_viso_index = find((viso_data.time_us - 1e6 * param.fusion.bodyVelTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); if (latest_viso_index > last_viso_index) last_viso_index = latest_viso_index; viso_fuse_index = viso_fuse_index + 1; + last_drift_constrain_time = local_time; % convert delta positon measurements to velocity relVelBodyMea = [viso_data.dVelX(latest_viso_index);viso_data.dVelY(latest_viso_index);viso_data.dVelZ(latest_viso_index)]./viso_data.dt(latest_viso_index); @@ -256,7 +319,7 @@ for index = indexStart:indexStop [states,covariance,bodyVelInnov,bodyVelInnovVar] = FuseBodyVel(states, covariance, relVelBodyMea, bodyVelError^2, param.fusion.bodyVelGate); % data logging - output.innovations.bodyVel_time_lapsed(viso_fuse_index) = localTime; + output.innovations.bodyVel_time_lapsed(viso_fuse_index) = local_time; output.innovations.bodyVelInnov(viso_fuse_index,:) = bodyVelInnov; output.innovations.bodyVelInnovVar(viso_fuse_index,:) = bodyVelInnovVar; diff --git a/matlab/EKF_replay/Filter/SetParameterDefaults.m b/matlab/EKF_replay/Filter/SetParameterDefaults.m index 75b0dcadc1..2fbbc01836 100644 --- a/matlab/EKF_replay/Filter/SetParameterDefaults.m +++ b/matlab/EKF_replay/Filter/SetParameterDefaults.m @@ -1,3 +1,14 @@ +%% Filter Control +param.control.waitForGps = 0; % set to 1 if the filter start should be delayed until GPS checks to pass +param.control.gpsSpdErrLim = 1.0; % GPS use will not start if reported GPS speed error is greater than this (m/s) +param.control.gpsPosErrLim = 5.0; % GPS use will not start if reported GPS position error is greater than this (m) +param.control.velDriftTimeLim = 5.0; % The maximum time without observations to constrain velocity drift before a zero velocity is fused to prevent the filter diverging (sec) +param.control.gpsOffTime = 0; % GPS aiding will be turned off at this time (sec) +param.control.gpsOnTime = 0; % GPS aiding will be turned back on at this time (sec) +param.control.flowOffTime = 0; % optical flow aiding will be turned off at this time (sec) +param.control.flowOnTime = 0; % optical flow aiding will be turned back on on at this time (sec) +param.control.visoOffTime = 0; % visual odometry aiding will be turned off at this time (sec) +param.control.visoOnTime = 0; % visual odometry aiding will be turned back on at this time (sec) %% GPS fusion param.fusion.gpsTimeDelay = 0.1; % GPS measurement delay relative to IMU (sec) @@ -26,11 +37,14 @@ param.fusion.rangeTimeDelay = 0.05; % range fidner sensor delay relative to IMU param.fusion.flowTimeDelay = 0.05; % Optical flow sensor time delay relative to IMU (sec) param.fusion.flowRateError = 0.5; % Observation noise 1SD for the flow sensor (rad/sec) param.fusion.flowGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD +param.fusion.rngValidMin = 0.05; % ignore range measurements smaller than this (m) +param.fusion.rngValidMin = 5.0; % ignore range measurements larger than this (m) +param.fusion.rngTimeout = 2.0; % optical flow measurements will not be used if more than this time since valid range finder data was received (sec) -%% Body frame velocity measurement fusion +%% Visual odometry body frame velocity measurement fusion param.fusion.bodyVelTimeDelay = 0.01; % Optical flow sensor time delay relative to IMU (sec) -param.fusion.bodyVelErrorMin = 0.5; % Observation noise 1SD for the odometry sensor at the highest quality value (m/sec) -param.fusion.bodyVelErrorMax = 5.0; % Observation noise 1SD for the odometry sensor at the lowest quality value (m/sec) +param.fusion.bodyVelErrorMin = 0.2; % Observation noise 1SD for the odometry sensor at the highest quality value (m/sec) +param.fusion.bodyVelErrorMax = 1.8; % Observation noise 1SD for the odometry sensor at the lowest quality value (m/sec) param.fusion.bodyVelGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD %% State prediction error growth diff --git a/matlab/EKF_replay/Filter/replay_apm_data.m b/matlab/EKF_replay/Filter/replay_apm_data.m index 56fd2fb651..2175db730d 100644 --- a/matlab/EKF_replay/Filter/replay_apm_data.m +++ b/matlab/EKF_replay/Filter/replay_apm_data.m @@ -1,13 +1,16 @@ clear all; close all; +% add required paths +addpath('../Common'); + % load compulsory data load '../TestData/APM/baro_data.mat'; load '../TestData/APM/gps_data.mat'; load '../TestData/APM/imu_data.mat'; load '../TestData/APM/mag_data.mat'; -% load data required for optical flow replay +% load optional data required for optical flow replay if exist('../TestData/APM/rng_data.mat','file') && exist('../TestData/APM/flow_data.mat','file') load '../TestData/APM/rng_data.mat'; load '../TestData/APM/flow_data.mat'; @@ -16,19 +19,23 @@ else flow_data = []; end -% load data required for ZED camera replay +% load optional data required for ZED camera replay if exist('../TestData/APM/viso_data.mat','file') load '../TestData/APM/viso_data.mat'; else viso_data = []; end +% load default parameters run('SetParameterDefaults.m'); +% run the filter replay output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,rng_data,flow_data,viso_data); +% genrate and save output plots PlotData(output); +% save output data folder = '../OutputData'; fileName = '../OutputData/ekf_replay_output.mat'; if ~exist(folder,'dir') diff --git a/matlab/EKF_replay/Filter/replay_px4_data.m b/matlab/EKF_replay/Filter/replay_px4_data.m index a93f2642c9..36a0593a71 100644 --- a/matlab/EKF_replay/Filter/replay_px4_data.m +++ b/matlab/EKF_replay/Filter/replay_px4_data.m @@ -1,16 +1,25 @@ clear all; close all; + +% add required paths +addpath('../Common'); + +% load test data load '../TestData/PX4/baro_data.mat'; load '../TestData/PX4/gps_data.mat'; load '../TestData/PX4/imu_data.mat'; load '../TestData/PX4/mag_data.mat'; +% set parameters to default values run('SetParameterDefaults.m'); +% run the filter replay output = RunFilter(param,imu_data,mag_data,baro_data,gps_data); +% generate and save output plots PlotData(output); +% save output data folder = '../OutputData'; fileName = '../OutputData/ekf_replay_output.mat'; if ~exist(folder,'dir') diff --git a/matlab/EKF_replay/readme.txt b/matlab/EKF_replay/readme.txt index 5f26ca5e58..947419e072 100644 --- a/matlab/EKF_replay/readme.txt +++ b/matlab/EKF_replay/readme.txt @@ -1,7 +1,6 @@ Instructions for running the EKF replay -1) Ensure this ‘EKF_replay’ directory is in a location you have full read and write access and add it and all its subdirectories to your path. - +1) Ensure the ‘EKF_replay’ directory is in a location you have full read and write access 2) Create a ‘TestData’ sub-directory under the ‘EKF_replay’ directory A sample dataset can be downloaded here: https://drive.google.com/file/d/0By4v2BuLAaCfSW9fWl9aSWNGbGs/view?usp=sharing