From c1829d8e9aa7ae61f13e90519aece83c2a858430 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 26 Jun 2017 14:48:03 +1000 Subject: [PATCH] matlab: fix bugs and add replay of PX4 optical flow test data --- matlab/EKF_replay/Common/convert_apm_data.m | 2 +- .../convert_px4_distance_sensor_csv_data.m | 18 +++ .../convert_px4_optical_flow_csv_data.m | 23 +++ .../convert_px4_vehicle_gps_position_csv.m | 3 + matlab/EKF_replay/Filter/InitStates.m | 4 + matlab/EKF_replay/Filter/PlotData.m | 147 +++++++++--------- matlab/EKF_replay/Filter/RunFilter.m | 7 +- .../EKF_replay/Filter/SetParameterDefaults.m | 10 +- .../Filter/replay_px4_optflow_data.m | 32 ++++ 9 files changed, 167 insertions(+), 79 deletions(-) create mode 100644 matlab/EKF_replay/Common/convert_px4_distance_sensor_csv_data.m create mode 100644 matlab/EKF_replay/Common/convert_px4_optical_flow_csv_data.m create mode 100644 matlab/EKF_replay/Filter/replay_px4_optflow_data.m diff --git a/matlab/EKF_replay/Common/convert_apm_data.m b/matlab/EKF_replay/Common/convert_apm_data.m index 84b14e1cbd..43399b3cf6 100644 --- a/matlab/EKF_replay/Common/convert_apm_data.m +++ b/matlab/EKF_replay/Common/convert_apm_data.m @@ -68,7 +68,7 @@ flow_data.qual = OF(:,3)/255; % scale quality from 0 to 1 flow_data.flowX = OF(:,4); % optical flow rate about the X body axis (rad/sec) flow_data.flowY = OF(:,5); % optical flow rate about the Y body axis (rad/sec) flow_data.bodyX = OF(:,6); % angular rate about the X body axis (rad/sec) -flow_data.bodyY = OF(:,7); % time period the measurement was sampled across (sec) +flow_data.bodyY = OF(:,7); % angular rate about the Y body axis (rad/sec) %% save visual odometry data clear viso_data; diff --git a/matlab/EKF_replay/Common/convert_px4_distance_sensor_csv_data.m b/matlab/EKF_replay/Common/convert_px4_distance_sensor_csv_data.m new file mode 100644 index 0000000000..e5de236f30 --- /dev/null +++ b/matlab/EKF_replay/Common/convert_px4_distance_sensor_csv_data.m @@ -0,0 +1,18 @@ +%% convert baro data +clear rng_data; +last_time = 0; +output_index = 1; +for source_index = 1:length(timestamp) + rng_timestamp = timestamp(source_index); + if (rng_timestamp ~= last_time) + last_time = rng_timestamp; + rng_data.time_us(output_index,1) = rng_timestamp; + rng_data.dist(output_index,1) = current_distance(source_index); + output_index = output_index + 1; + end +end + +%% save data and clear workspace +clearvars -except rng_data; + +save rng_data.mat rng_data; \ No newline at end of file diff --git a/matlab/EKF_replay/Common/convert_px4_optical_flow_csv_data.m b/matlab/EKF_replay/Common/convert_px4_optical_flow_csv_data.m new file mode 100644 index 0000000000..e04b949469 --- /dev/null +++ b/matlab/EKF_replay/Common/convert_px4_optical_flow_csv_data.m @@ -0,0 +1,23 @@ +%% convert baro data +clear flow_data; +last_time = 0; +output_index = 1; +for source_index = 1:length(timestamp) + flow_timestamp = timestamp(source_index); + if ((flow_timestamp ~= last_time) && (integration_timespan(source_index) > 0)) + last_time = flow_timestamp; + flow_data.time_us(output_index,1) = flow_timestamp; + flow_data.qual(output_index,1) = quality(source_index)/255; % scale quality from 0 to 1 + dt_inv = 1e6 / integration_timespan(source_index); + flow_data.flowX(output_index,1) = dt_inv * pixel_flow_x_integral(source_index); % optical flow rate about the X body axis (rad/sec) + flow_data.flowY(output_index,1) = dt_inv * pixel_flow_y_integral(source_index); % optical flow rate about the Y body axis (rad/sec) + flow_data.bodyX(output_index,1) = dt_inv * gyro_x_rate_integral(source_index); % angular rate about the X body axis (rad/sec) + flow_data.bodyY(output_index,1) = dt_inv * gyro_y_rate_integral(source_index); % angular rate about the Y body axis (rad/sec) + output_index = output_index + 1; + end +end + +%% save data and clear workspace +clearvars -except flow_data; + +save flow_data.mat flow_data; \ No newline at end of file diff --git a/matlab/EKF_replay/Common/convert_px4_vehicle_gps_position_csv.m b/matlab/EKF_replay/Common/convert_px4_vehicle_gps_position_csv.m index 69af83ca01..2818a0843c 100644 --- a/matlab/EKF_replay/Common/convert_px4_vehicle_gps_position_csv.m +++ b/matlab/EKF_replay/Common/convert_px4_vehicle_gps_position_csv.m @@ -6,6 +6,9 @@ gps_data.hgt_error = epv; % set reference point used to set NED origin when GPS accuracy is sufficient gps_data.start_index = max(min(find(gps_data.pos_error < 5.0)),min(find(gps_data.spd_error < 1.0))); +if isempty(gps_data.start_index) + gps_data.start_index = find(gps_data.pos_error == min(gps_data.pos_error)); +end gps_data.refLLH = [1e-7*lat(gps_data.start_index);1e-7*lon(gps_data.start_index);0.001*alt(gps_data.start_index)]; % convert GPS data to NED diff --git a/matlab/EKF_replay/Filter/InitStates.m b/matlab/EKF_replay/Filter/InitStates.m index 1687f64b23..361bec1063 100644 --- a/matlab/EKF_replay/Filter/InitStates.m +++ b/matlab/EKF_replay/Filter/InitStates.m @@ -25,6 +25,10 @@ initAccel(3) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,3))./mea quat = AlignTilt(quat,initAccel); states(1:4) = quat; +% add a roll, pitch, yaw mislignment +quat_align_err = EulToQuat([param.control.rollAlignErr,param.control.pitchAlignErr,param.control.yawAlignErr]); +quat = QuatMult(quat,quat_align_err); + % 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); diff --git a/matlab/EKF_replay/Filter/PlotData.m b/matlab/EKF_replay/Filter/PlotData.m index 3d3bf7c673..d71ddec9d0 100644 --- a/matlab/EKF_replay/Filter/PlotData.m +++ b/matlab/EKF_replay/Filter/PlotData.m @@ -290,79 +290,84 @@ if (output.magFuseMethod <= 1) title(titleText); ylabel('declination (deg)'); xlabel('time (sec)'); - + fileName='earth_field_estimates.png'; fullFileName = fullfile(folder, fileName); saveas(h,fullFileName); end %% plot velocity innovations -figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); -h=gcf; -set(h,'PaperOrientation','portrait'); -set(h,'PaperUnits','normalized'); -set(h,'PaperPosition', [0 0 1 1]); - -subplot(3,1,1); -plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,1),sqrt(output.innovations.vel_innov_var(:,1)),-sqrt(output.innovations.vel_innov_var(:,1))]); -grid on; -titleText=strcat({'Velocity Innovations and Variances'},runIdentifier); -title(titleText); -ylabel('North (m/s)'); -xlabel('time (sec)'); -legend('innovation','variance sqrt','variance sqrt'); - -subplot(3,1,2); -plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,2),sqrt(output.innovations.vel_innov_var(:,2)),-sqrt(output.innovations.vel_innov_var(:,2))]); -grid on; -ylabel('East (m/s)'); -xlabel('time (sec)'); -legend('innovation','variance sqrt','variance sqrt'); - -subplot(3,1,3); -plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,3),sqrt(output.innovations.vel_innov_var(:,3)),-sqrt(output.innovations.vel_innov_var(:,3))]); -grid on; -ylabel('Down (m/s)'); -xlabel('time (sec)'); -legend('innovation','variance sqrt','variance sqrt'); - -fileName='velocity_fusion.png'; -fullFileName = fullfile(folder, fileName); -saveas(h,fullFileName); +if isfield(output.innovations,'vel_innov') + + figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); + h=gcf; + set(h,'PaperOrientation','portrait'); + set(h,'PaperUnits','normalized'); + set(h,'PaperPosition', [0 0 1 1]); + + subplot(3,1,1); + plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,1),sqrt(output.innovations.vel_innov_var(:,1)),-sqrt(output.innovations.vel_innov_var(:,1))]); + grid on; + titleText=strcat({'Velocity Innovations and Variances'},runIdentifier); + title(titleText); + ylabel('North (m/s)'); + xlabel('time (sec)'); + legend('innovation','variance sqrt','variance sqrt'); + + subplot(3,1,2); + plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,2),sqrt(output.innovations.vel_innov_var(:,2)),-sqrt(output.innovations.vel_innov_var(:,2))]); + grid on; + ylabel('East (m/s)'); + xlabel('time (sec)'); + legend('innovation','variance sqrt','variance sqrt'); + + subplot(3,1,3); + plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,3),sqrt(output.innovations.vel_innov_var(:,3)),-sqrt(output.innovations.vel_innov_var(:,3))]); + grid on; + ylabel('Down (m/s)'); + xlabel('time (sec)'); + legend('innovation','variance sqrt','variance sqrt'); + + fileName='velocity_fusion.png'; + fullFileName = fullfile(folder, fileName); + saveas(h,fullFileName); +end %% plot position innovations -figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); -h=gcf; -set(h,'PaperOrientation','portrait'); -set(h,'PaperUnits','normalized'); -set(h,'PaperPosition', [0 0 1 1]); - -subplot(3,1,1); -plot(output.innovations.vel_time_lapsed',[output.innovations.posInnov(:,1),sqrt(output.innovations.posInnovVar(:,1)),-sqrt(output.innovations.posInnovVar(:,1))]); -grid on; -titleText=strcat({'Position Innovations and Variances'},runIdentifier); -title(titleText); -ylabel('North (m)'); -xlabel('time (sec)'); -legend('innovation','variance sqrt','variance sqrt'); - -subplot(3,1,2); -plot(output.innovations.vel_time_lapsed',[output.innovations.posInnov(:,2),sqrt(output.innovations.posInnovVar(:,2)),-sqrt(output.innovations.posInnovVar(:,2))]); -grid on; -ylabel('East (m)'); -xlabel('time (sec)'); -legend('innovation','variance sqrt','variance sqrt'); - -subplot(3,1,3); -plot(output.innovations.hgt_time_lapsed',[output.innovations.hgtInnov(:),sqrt(output.innovations.hgtInnovVar(:)),-sqrt(output.innovations.hgtInnovVar(:))]); -grid on; -ylabel('Up (m)'); -xlabel('time (sec)'); -legend('innovation','variance sqrt','variance sqrt'); - -fileName='position_fusion.png'; -fullFileName = fullfile(folder, fileName); -saveas(h,fullFileName); +if isfield(output.innovations,'pos_innov') + figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); + h=gcf; + set(h,'PaperOrientation','portrait'); + set(h,'PaperUnits','normalized'); + set(h,'PaperPosition', [0 0 1 1]); + + subplot(3,1,1); + plot(output.innovations.vel_time_lapsed',[output.innovations.posInnov(:,1),sqrt(output.innovations.posInnovVar(:,1)),-sqrt(output.innovations.posInnovVar(:,1))]); + grid on; + titleText=strcat({'Position Innovations and Variances'},runIdentifier); + title(titleText); + ylabel('North (m)'); + xlabel('time (sec)'); + legend('innovation','variance sqrt','variance sqrt'); + + subplot(3,1,2); + plot(output.innovations.vel_time_lapsed',[output.innovations.posInnov(:,2),sqrt(output.innovations.posInnovVar(:,2)),-sqrt(output.innovations.posInnovVar(:,2))]); + grid on; + ylabel('East (m)'); + xlabel('time (sec)'); + legend('innovation','variance sqrt','variance sqrt'); + + subplot(3,1,3); + plot(output.innovations.hgt_time_lapsed',[output.innovations.hgtInnov(:),sqrt(output.innovations.hgtInnovVar(:)),-sqrt(output.innovations.hgtInnovVar(:))]); + grid on; + ylabel('Up (m)'); + xlabel('time (sec)'); + legend('innovation','variance sqrt','variance sqrt'); + + fileName='position_fusion.png'; + fullFileName = fullfile(folder, fileName); + saveas(h,fullFileName); +end %% plot magnetometer innovations if isfield(output.innovations,'magInnov') @@ -447,6 +452,7 @@ if isfield(output.innovations,'flowInnov') set(h,'PaperPosition', [0 0 1 1]); subplot(2,1,1); plot(output.innovations.flow_time_lapsed,[output.innovations.flowInnov(:,1)';sqrt(output.innovations.flowInnovVar(:,1))';-sqrt(output.innovations.flowInnovVar(:,1))']); + ylim([-1.0 1.0]); grid on; title(strcat({'Optical Flow Innovations and Variances'},runIdentifier)); ylabel('X (rad/sec)'); @@ -454,6 +460,7 @@ if isfield(output.innovations,'flowInnov') legend('innovation','innovation variance sqrt','innovation variance sqrt'); subplot(2,1,2); plot(output.innovations.flow_time_lapsed,[output.innovations.flowInnov(:,2)';sqrt(output.innovations.flowInnovVar(:,2))';-sqrt(output.innovations.flowInnovVar(:,2))']); + ylim([-1.0 1.0]); grid on; ylabel('Y (rad/sec)'); xlabel('time (sec)'); @@ -471,7 +478,7 @@ if isfield(output.innovations,'bodyVelInnov') set(h,'PaperOrientation','portrait'); set(h,'PaperUnits','normalized'); set(h,'PaperPosition', [0 0 1 1]); - + subplot(3,1,1); plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,1)';sqrt(output.innovations.bodyVelInnovVar(:,1))';-sqrt(output.innovations.bodyVelInnovVar(:,1))']); grid on; @@ -479,23 +486,23 @@ if isfield(output.innovations,'bodyVelInnov') ylabel('X (m/sec)'); xlabel('time (sec)'); legend('innovation','innovation variance sqrt','innovation variance sqrt'); - + subplot(3,1,2); plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,2)';sqrt(output.innovations.bodyVelInnovVar(:,2))';-sqrt(output.innovations.bodyVelInnovVar(:,2))']); grid on; ylabel('Y (m/sec)'); xlabel('time (sec)'); legend('innovation','innovation variance sqrt','innovation variance sqrt'); - + subplot(3,1,3); plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,3)';sqrt(output.innovations.bodyVelInnovVar(:,3))';-sqrt(output.innovations.bodyVelInnovVar(:,3))']); grid on; ylabel('Z (m/sec)'); xlabel('time (sec)'); legend('innovation','innovation variance sqrt','innovation variance sqrt'); - + fileName='zed_camera_fusion.png'; fullFileName = fullfile(folder, fileName); saveas(h,fullFileName); - + end diff --git a/matlab/EKF_replay/Filter/RunFilter.m b/matlab/EKF_replay/Filter/RunFilter.m index 1d445a4cc2..ce637f162a 100644 --- a/matlab/EKF_replay/Filter/RunFilter.m +++ b/matlab/EKF_replay/Filter/RunFilter.m @@ -264,14 +264,15 @@ for index = indexStart:indexStop % Get latest range finder data and gate to remove dropouts 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); + if (rng_data.dist(last_range_index) < param.fusion.rngValidMax) + range = max(rng_data.dist(last_range_index) , param.fusion.rngValidMin); last_valid_range_time = local_time; end % 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) && ((local_time - last_valid_range_time) < param.fusion.rngTimeout)) + + if (~isempty(latest_flow_index) && (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; diff --git a/matlab/EKF_replay/Filter/SetParameterDefaults.m b/matlab/EKF_replay/Filter/SetParameterDefaults.m index 75ee723971..42553e3817 100644 --- a/matlab/EKF_replay/Filter/SetParameterDefaults.m +++ b/matlab/EKF_replay/Filter/SetParameterDefaults.m @@ -9,17 +9,18 @@ param.control.flowOffTime = 0; % optical flow aiding will be turned off at this 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) +param.control.rollAlignErr = 0.0; % initial roll misalignment (rad) +param.control.pitchAlignErr = 0.0; % initial pitch misalignment (rad) +param.control.yawAlignErr = 0.0; % initial yaw misalignment (rad) %% GPS fusion param.fusion.gpsTimeDelay = 0.1; % GPS measurement delay relative to IMU (sec) -param.fusion.gpsCheckTimeout = 5.0; % Length of time that GPS measurements will be rejected by the filter before states are reset to the GPS velocity. (sec) param.fusion.gpsVelGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD param.fusion.gpsPosGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD param.fusion.gpsCheckTimeout = 10.0; % Length of time that GPS measurements will be rejected by the filter before states are reset to the GPS velocity. (sec) %% Baro fusion param.fusion.baroTimeDelay = 0.05; % Baro measurement delay relative to IMU (sec) -param.fusion.baroCheckTimeout = 5.0; % Length of time that baro measurements will be rejected by the filter before states are reset to the baro height. (sec) param.fusion.baroHgtGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD param.fusion.baroHgtNoise = 2.0; % 1SD observation noise of the baro measurements (m) @@ -37,8 +38,8 @@ 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.rngValidMin = 0.05; % range measurements wil be constrained to be no less than this (m) +param.fusion.rngValidMax = 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) %% Visual odometry body frame velocity measurement fusion @@ -54,7 +55,6 @@ param.prediction.dAngBiasPnoise = 0.001; % IMU gyro bias 1SD rate of change (rad param.prediction.dVelBiasPnoise = 0.03; % IMU accel bias 1SD rate of change (m/sec^3) param.prediction.angRateNoise = 0.015; % IMU gyro 1SD rate process noise (rad/sec) param.prediction.accelNoise = 0.35; % IMU accelerometer 1SD error noise including switch on bias uncertainty. (m/sec^2) -param.prediction.windPnoiseNE = 0.1; % wind state process 1SD rate of change (m/sec^2) %% Initial Uncertainty param.alignment.posErrNE = 10.0; % Initial 1SD position error when aligning without GPS. (m/sec) diff --git a/matlab/EKF_replay/Filter/replay_px4_optflow_data.m b/matlab/EKF_replay/Filter/replay_px4_optflow_data.m new file mode 100644 index 0000000000..a054210128 --- /dev/null +++ b/matlab/EKF_replay/Filter/replay_px4_optflow_data.m @@ -0,0 +1,32 @@ +clear all; +close all; + +% add required paths +addpath('../Common'); + +% load test data +load '../TestData/PX4_optical_flow/baro_data.mat'; +load '../TestData/PX4_optical_flow/gps_data.mat'; +load '../TestData/PX4_optical_flow/imu_data.mat'; +load '../TestData/PX4_optical_flow/mag_data.mat'; +load '../TestData/PX4_optical_flow/rng_data.mat'; +load '../TestData/PX4_optical_flow/flow_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,rng_data,flow_data); + +% generate and save output plots +runIdentifier = ' : PX4 optical flow replay '; +folder = strcat('../OutputPlots/PX4_optical_flow'); +PlotData(output,folder,runIdentifier); + +% save output data +folder = '../OutputData/PX4_optical_flow'; +fileName = '../OutputData/PX4_optical_flow/ekf_replay_output.mat'; +if ~exist(folder,'dir') + mkdir(folder); +end +save(fileName,'output'); \ No newline at end of file