matlab: fix bugs and add replay of PX4 optical flow test data

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;

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;

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;

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

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);

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

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;

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)

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');