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