diff --git a/matlab/EKF_replay/Common/convert_apm_data.m b/matlab/EKF_replay/Common/convert_apm_data.m
old mode 100644
new mode 100755
index 7f374e74f4..db0b858e74
--- a/matlab/EKF_replay/Common/convert_apm_data.m
+++ b/matlab/EKF_replay/Common/convert_apm_data.m
@@ -74,12 +74,12 @@ flow_data.bodyY = OF(:,7); % angular rate about the Y body axis (rad/sec)
 clear viso_data;
 viso_data.time_us = VISO(:,2);
 viso_data.dt = VISO(:,3); % time period the measurement was sampled across (sec)
-viso_data.dAngX = VISO(:,4); % delta angle about the X body axis
-viso_data.dAngY = VISO(:,5); % delta angle about the Y body axis
-viso_data.dAngZ = VISO(:,6); % delta angle about the Z body axis
-viso_data.dVelX = VISO(:,7); % delta velocity along the X body axis
-viso_data.dVelY = VISO(:,8); % delta velocity along the Y body axis
-viso_data.dVelZ = VISO(:,9); % delta velocity along the Z body axis
+viso_data.dAngX = VISO(:,4); % delta angle about the X body axis (rad)
+viso_data.dAngY = VISO(:,5); % delta angle about the Y body axis (rad)
+viso_data.dAngZ = VISO(:,6); % delta angle about the Z body axis (rad)
+viso_data.dPosX = VISO(:,7); % delta position along the X body axis (m)
+viso_data.dPosY = VISO(:,8); % delta position along the Y body axis (m)
+viso_data.dPosZ = VISO(:,9); % delta position along the Z body axis (m)
 viso_data.qual = VISO(:,10)/100; % quality from 0 - 1
 
 %% save data and clear workspace
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
old mode 100644
new mode 100755
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
old mode 100644
new mode 100755
diff --git a/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m b/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m
old mode 100644
new mode 100755
index f91bfb2fcf..882fafb5d5
--- a/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m
+++ b/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m
@@ -12,25 +12,18 @@ for source_index = 1:length(timestamp)
     end
 end
 
-%% convert IMU data to delta angles and velocities using trapezoidal integration
+%% convert IMU data to delta angles and velocities
+% Note: these quatntis were converted from delta angles and velocites using
+% the integral_dt values in the PX4 sensor module so we only need to
+% multiply by integral_dt to convert back
 clear imu_data;
 n_samples = length(timestamp);
-imu_data.time_us = timestamp(2:n_samples) + accelerometer_timestamp_relative(2:n_samples);
-imu_data.gyro_dt = gyro_integral_dt(2:n_samples);
-imu_data.del_ang = 0.5 * ([gyro_rad0(1:n_samples-1).*imu_data.gyro_dt, ...
-    gyro_rad1(1:n_samples-1).*imu_data.gyro_dt, ...
-    gyro_rad2(1:n_samples-1).*imu_data.gyro_dt] + ...
-    [gyro_rad0(2:n_samples).*imu_data.gyro_dt, ...
-    gyro_rad1(2:n_samples).*imu_data.gyro_dt, ...
-    gyro_rad2(2:n_samples).*imu_data.gyro_dt]);
+imu_data.time_us = timestamp + accelerometer_timestamp_relative;
+imu_data.gyro_dt = gyro_integral_dt;
+imu_data.del_ang = [gyro_rad0.*imu_data.gyro_dt, gyro_rad1.*imu_data.gyro_dt, gyro_rad2.*imu_data.gyro_dt];
 
-imu_data.accel_dt = accelerometer_integral_dt(2:n_samples);
-imu_data.del_vel = 0.5 * ([accelerometer_m_s20(1:n_samples-1).*imu_data.accel_dt, ...
-    accelerometer_m_s21(1:n_samples-1).*imu_data.accel_dt, ...
-    accelerometer_m_s22(1:n_samples-1).*imu_data.accel_dt] + ...
-    [accelerometer_m_s20(2:n_samples).*imu_data.accel_dt, ...
-    accelerometer_m_s21(2:n_samples).*imu_data.accel_dt, ...
-    accelerometer_m_s22(2:n_samples).*imu_data.accel_dt]);
+imu_data.accel_dt = accelerometer_integral_dt;
+imu_data.del_vel = [accelerometer_m_s20.*imu_data.accel_dt, accelerometer_m_s21.*imu_data.accel_dt, accelerometer_m_s22.*imu_data.accel_dt];
 
 %% convert magnetomer data
 clear mag_data;
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
old mode 100644
new mode 100755
index 2818a0843c..0385daa558
--- a/matlab/EKF_replay/Common/convert_px4_vehicle_gps_position_csv.m
+++ b/matlab/EKF_replay/Common/convert_px4_vehicle_gps_position_csv.m
@@ -7,9 +7,11 @@ 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));
+    gps_data.start_index = 1;
+    gps_data.refLLH = [1e-7*median(lat);1e-7*median(lon);0.001*median(alt)];
+else
+    gps_data.refLLH = [1e-7*lat(gps_data.start_index);1e-7*lon(gps_data.start_index);0.001*alt(gps_data.start_index)];
 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
 for index = 1:length(timestamp)
diff --git a/matlab/EKF_replay/Filter/GenerateEquations24.m b/matlab/EKF_replay/Filter/GenerateEquations24.m
old mode 100644
new mode 100755
index 100d47f67c..68146d9d2a
--- a/matlab/EKF_replay/Filter/GenerateEquations24.m
+++ b/matlab/EKF_replay/Filter/GenerateEquations24.m
@@ -83,9 +83,7 @@ nStates=numel(stateVector);
 % Define vector of process equations
 stateVectorNew = [quatNew;vNew;pNew;dAngBiasNew;dVelBiasNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew];
 
-%% derive the covariance prediction equation
-% This reduces the number of floating point operations by a factor of 6 or
-% more compared to using the standard matrix operations in code
+%% derive the state transition and state error matrix
 
 % Define the control (disturbance) vector. Error growth in the inertial
 % solution is assumed to be driven by 'noise' in the delta angles and
@@ -201,3 +199,13 @@ declination = atan(magE/magN);
 T_MAG = jacobian(declination,[magN,magE]);
 
 f = matlabFunction(T_MAG,'file','transfer_matrix.m');
+
+%% calculate Quaternion to Euler angle error transfer matrix
+% quat = [q0;q1;q2;q3];
+% syms roll pitch yaw 'real';
+roll = atan2(2*(quat(3)*quat(4)+quat(1)*quat(2)) , (quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3) + quat(4)*quat(4)));
+pitch = -asin(2*(quat(2)*quat(4)-quat(1)*quat(3)));
+yaw = atan2(2*(quat(2)*quat(3)+quat(1)*quat(4)) , (quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3) - quat(4)*quat(4)));
+euler = [roll;pitch;yaw];
+error_transfer_matrix = jacobian(euler,quat);
+matlabFunction(error_transfer_matrix,'file','quat_to_euler_error_transfer_matrix.m');
diff --git a/matlab/EKF_replay/Filter/PlotData.m b/matlab/EKF_replay/Filter/PlotData.m
old mode 100644
new mode 100755
index 865b0875d7..4ba5d5ab04
--- a/matlab/EKF_replay/Filter/PlotData.m
+++ b/matlab/EKF_replay/Filter/PlotData.m
@@ -15,7 +15,7 @@ set(h,'PaperPosition', [0 0 1 1]);
 margin = 5;
 
 subplot(3,1,1);
-plot(output.time_lapsed,output.euler_angles(:,1)*rad2deg);
+plot(output.time_lapsed,[output.euler_angles(:,1)*rad2deg,output.euler_angles(:,1)*rad2deg-2*sqrt(output.euler_variances(:,1)*rad2deg),output.euler_angles(:,1)*rad2deg+2*sqrt(output.euler_variances(:,1)*rad2deg)]);
 minVal = rad2deg*min(output.euler_angles(:,1))-margin;
 maxVal = rad2deg*max(output.euler_angles(:,1))+margin;
 ylim([minVal maxVal]);
@@ -24,24 +24,27 @@ titleText=strcat({'Euler Angle Estimates'},runIdentifier);
 title(titleText);
 ylabel('Roll (deg)');
 xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
 
 subplot(3,1,2);
-plot(output.time_lapsed,output.euler_angles(:,2)*rad2deg);
+plot(output.time_lapsed,[output.euler_angles(:,2)*rad2deg,output.euler_angles(:,2)*rad2deg-2*sqrt(output.euler_variances(:,2)*rad2deg),output.euler_angles(:,2)*rad2deg+2*sqrt(output.euler_variances(:,2)*rad2deg)]);
 minVal = rad2deg*min(output.euler_angles(:,2))-margin;
 maxVal = rad2deg*max(output.euler_angles(:,2))+margin;
 ylim([minVal maxVal]);
 grid on;
 ylabel('Pitch (deg)');
 xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
 
 subplot(3,1,3);
-plot(output.time_lapsed,output.euler_angles(:,3)*rad2deg);
+plot(output.time_lapsed,[output.euler_angles(:,3)*rad2deg,output.euler_angles(:,3)*rad2deg-2*sqrt(output.euler_variances(:,3)*rad2deg),output.euler_angles(:,3)*rad2deg+2*sqrt(output.euler_variances(:,3)*rad2deg)]);
 minVal = rad2deg*min(output.euler_angles(:,3))-margin;
 maxVal = rad2deg*max(output.euler_angles(:,3))+margin;
 ylim([minVal maxVal]);
 grid on;
 ylabel('Yaw (deg)');
 xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
 
 fileName='euler_angle_estimates.png';
 fullFileName = fullfile(folder, fileName);
diff --git a/matlab/EKF_replay/Filter/PredictStates.m b/matlab/EKF_replay/Filter/PredictStates.m
old mode 100644
new mode 100755
index f1b06937f6..88f3d64ff9
--- a/matlab/EKF_replay/Filter/PredictStates.m
+++ b/matlab/EKF_replay/Filter/PredictStates.m
@@ -33,8 +33,9 @@ delVel = delVel - states(14:16);
 % Savage, P.G.
 % Strapdown Associates
 % 2015, WBN-14010
-correctedDelVel= delVel + ...
-    0.5*cross(prevDelAng + delAng , prevDelVel + delVel) + 1/6*cross(prevDelAng + delAng , cross(prevDelAng + delAng , prevDelVel + delVel)) +  1/12*(cross(prevDelAng , delVel) + cross(prevDelVel , delAng));
+% correctedDelVel= delVel + ...
+%     0.5*cross(prevDelAng + delAng , prevDelVel + delVel) + 1/6*cross(prevDelAng + delAng , cross(prevDelAng + delAng , prevDelVel + delVel)) +  1/12*(cross(prevDelAng , delVel) + cross(prevDelVel , delAng));
+correctedDelVel= delVel;
 
 % Calculate earth delta angle spin vector
 delAngEarth_NED(1,1) = 0.000072921 * cos(latitude) * dt;
@@ -47,7 +48,8 @@ delAngEarth_NED(3,1) = -0.000072921 * sin(latitude) * dt;
 % R. B. MILLER, 
 % Journal of Guidance, Control, and Dynamics
 % July, Vol. 6, No. 4, pp. 287-291, Eqn 11 
-correctedDelAng   = delAng - 1/12*cross(prevDelAng , delAng) - transpose(Tbn_prev)*delAngEarth_NED;
+% correctedDelAng   = delAng - 1/12*cross(prevDelAng , delAng) - transpose(Tbn_prev)*delAngEarth_NED;
+correctedDelAng   = delAng - transpose(Tbn_prev)*delAngEarth_NED;
 
 % Save current measurements
 prevDelAng = delAng;
diff --git a/matlab/EKF_replay/Filter/RunFilter.m b/matlab/EKF_replay/Filter/RunFilter.m
old mode 100644
new mode 100755
index ce637f162a..37afb47f2f
--- a/matlab/EKF_replay/Filter/RunFilter.m
+++ b/matlab/EKF_replay/Filter/RunFilter.m
@@ -148,6 +148,13 @@ for index = indexStart:indexStop
             output.state_variances(covIndex,i) = covariance(i,i);
         end
         
+        % output equivalent euler angle variances
+        error_transfer_matrix = quat_to_euler_error_transfer_matrix(states(1),states(2),states(3),states(4));
+        euler_covariance_matrix = error_transfer_matrix * covariance(1:4,1:4) * transpose(error_transfer_matrix);
+        for i=1:3
+            output.euler_variances(covIndex,i) = euler_covariance_matrix(i,i);
+        end
+        
         % 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' );
 
@@ -309,7 +316,7 @@ for index = indexStart:indexStop
                 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);
+                relVelBodyMea = [viso_data.dPosX(latest_viso_index);viso_data.dPosY(latest_viso_index);viso_data.dPosZ(latest_viso_index)]./viso_data.dt(latest_viso_index);
                 
                 % convert quality metric to equivalent observation error
                 % (this is a guess)
diff --git a/matlab/EKF_replay/Filter/SetParameterDefaults.m b/matlab/EKF_replay/Filter/SetParameterDefaults.m
old mode 100644
new mode 100755
index dc4e99e887..fe73cabce1
--- a/matlab/EKF_replay/Filter/SetParameterDefaults.m
+++ b/matlab/EKF_replay/Filter/SetParameterDefaults.m
@@ -36,7 +36,7 @@ param.fusion.magDeclDeg = 10.5; % Magnetic declination in deg
 %% Optical flow measurement fusion
 param.fusion.rangeTimeDelay = 0.05; % range fidner sensor delay relative to IMU (sec)
 param.fusion.flowTimeDelay = 0.05; % Optical flow sensor time delay relative to IMU (sec)
-param.fusion.flowRateError = 0.1; % Observation noise 1SD for the flow sensor (rad/sec)
+param.fusion.flowRateError = 0.15; % 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; % range measurements wil be constrained to be  no less than this (m)
 param.fusion.rngValidMax = 5.0; % ignore range measurements larger than this (m)
@@ -44,8 +44,8 @@ param.fusion.rngTimeout = 2.0; % optical flow measurements will not be used if m
 
 %% 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.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.bodyVelErrorMin = 0.1; % Observation noise 1SD for the odometry sensor at the highest quality value (m/sec)
+param.fusion.bodyVelErrorMax = 0.9; % 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/SetParameters.m b/matlab/EKF_replay/Filter/SetParameters.m
new file mode 100755
index 0000000000..fe73cabce1
--- /dev/null
+++ b/matlab/EKF_replay/Filter/SetParameters.m
@@ -0,0 +1,69 @@
+%% 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)
+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.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.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)
+
+%% Magnetometer measurement fusion
+param.fusion.magTimeDelay = 0.0; % Magnetomer time delay relative to IMU (sec)
+param.fusion.magFuseMethod = 1; % 0: 3-Axis field fusion with free declination, 1: 3-Axis field fusion with constrained declination, 2: magnetic heading fusion. (None)
+param.fusion.magFieldError = 0.05; % Magnetic field measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 0 or 1. (gauss)
+param.fusion.magHdgError = 0.1745; % Magnetic heading measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 2. (rad)
+param.fusion.magFieldGate = 5.0; % Size of the magnetic field innovation consistency check gate in SD
+param.fusion.magHdgGate = 5.0; % Size of the magnetic heading innovation consistency check gate in SD
+param.fusion.magDeclDeg = 10.5; % Magnetic declination in deg
+
+%% Optical flow measurement fusion
+param.fusion.rangeTimeDelay = 0.05; % range fidner sensor delay relative to IMU (sec)
+param.fusion.flowTimeDelay = 0.05; % Optical flow sensor time delay relative to IMU (sec)
+param.fusion.flowRateError = 0.15; % 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; % 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
+param.fusion.bodyVelTimeDelay = 0.01; % Optical flow sensor time delay relative to IMU (sec)
+param.fusion.bodyVelErrorMin = 0.1; % Observation noise 1SD for the odometry sensor at the highest quality value (m/sec)
+param.fusion.bodyVelErrorMax = 0.9; % 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
+param.prediction.magPnoiseNED = 1e-3; % Earth magnetic field 1SD rate of change. (gauss/sec)
+param.prediction.magPnoiseXYZ = 1e-3; % Body magnetic field 1SD rate of change. (gauss/sec)
+param.prediction.dAngBiasPnoise = 0.001; % IMU gyro bias 1SD rate of change (rad/sec^2)
+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)
+
+%% Initial Uncertainty
+param.alignment.posErrNE = 10.0; % Initial 1SD position error when aligning without GPS. (m/sec)
+param.alignment.velErrNE = 5.0; % Initial 1SD velocity error when aligning without GPS. (m/sec)
+param.alignment.velErrD = 1.0; % Initial 1SD vertical velocity error when aligning without GPS. (m/sec)
+param.alignment.delAngBiasErr = 0.05*pi/180; % Initial 1SD rate gyro bias uncertainty. (rad/sec)
+param.alignment.delVelBiasErr = 0.07; % Initial 1SD accelerometer bias uncertainty. (m/sec^2)
+param.alignment.quatErr = 0.1; % Initial 1SD uncertainty in quaternion.
+param.alignment.magErrXYZ = 0.01; % Initial 1SD uncertainty in body frame XYZ magnetic field states. (gauss)
+param.alignment.magErrNED = 0.5; % Initial 1SD uncertainty in earth frame NED magnetic field states. (gauss)
+param.alignment.hgtErr = 0.5; % Initial 1SD uncertainty in height. (m)
+param.alignment.windErrNE = 5.0; % Initial 1SD error in wind states. (m/sec)
diff --git a/matlab/EKF_replay/Filter/find_best_gps_delay.m b/matlab/EKF_replay/Filter/find_best_gps_delay.m
new file mode 100755
index 0000000000..a320153591
--- /dev/null
+++ b/matlab/EKF_replay/Filter/find_best_gps_delay.m
@@ -0,0 +1,50 @@
+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');
+
+% Loop through range of GPS time delays and capture the RMS velocity
+% innovation and corresponding delay each time
+for index = 1:1:21
+    index
+    param.fusion.gpsTimeDelay = 0.01*(index - 1);
+    output = RunFilter(param,imu_data,mag_data,baro_data,gps_data);
+    gps_delay(index) = param.fusion.gpsTimeDelay;
+    gps_vel_innov_rms(index) = sqrt(sum(output.innovations.vel_innov(:,1).^2) + sum(output.innovations.vel_innov(:,2).^2))/sqrt(length(output.innovations.vel_innov));
+end
+
+% plot the innvation level vs time delay
+figure;
+plot(gps_delay,gps_vel_innov_rms);
+grid on;
+xlabel('GPS delay (sec)');
+ylabel('RMS velocity innovation (m/s)');
+
+% find the smallest value and re-run the replay
+param.fusion.gpsTimeDelay = gps_delay(gps_delay == min(gps_delay))
+
+% run the filter replay
+output = RunFilter(param,imu_data,mag_data,baro_data,gps_data);
+
+% generate and save output plots
+runIdentifier = ' : PX4 data replay ';
+folder = strcat('../OutputPlots/PX4');
+PlotData(output,folder,runIdentifier);
+
+% save output data
+folder = '../OutputData/PX4';
+fileName = '../OutputData/PX4/ekf_replay_output.mat';
+if ~exist(folder,'dir')
+    mkdir(folder);
+end
+save(fileName,'output');
\ No newline at end of file
diff --git a/matlab/EKF_replay/Filter/quat_to_euler_error_transfer_matrix.m b/matlab/EKF_replay/Filter/quat_to_euler_error_transfer_matrix.m
new file mode 100644
index 0000000000..ffcb87d8dd
--- /dev/null
+++ b/matlab/EKF_replay/Filter/quat_to_euler_error_transfer_matrix.m
@@ -0,0 +1,38 @@
+function error_transfer_matrix = quat_to_euler_error_transfer_matrix(q0,q1,q2,q3)
+%QUAT_TO_EULER_ERROR_TRANSFER_MATRIX
+%    ERROR_TRANSFER_MATRIX = QUAT_TO_EULER_ERROR_TRANSFER_MATRIX(Q0,Q1,Q2,Q3)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    13-Jul-2017 08:41:41
+
+t8 = q0.*q1.*2.0;
+t9 = q2.*q3.*2.0;
+t2 = t8+t9;
+t4 = q0.^2;
+t5 = q1.^2;
+t6 = q2.^2;
+t7 = q3.^2;
+t3 = t4-t5-t6+t7;
+t10 = t3.^2;
+t11 = t2.^2;
+t12 = t10+t11;
+t13 = 1.0./t12;
+t14 = 1.0./t3;
+t15 = 1.0./t3.^2;
+t17 = q0.*q2.*2.0;
+t18 = q1.*q3.*2.0;
+t16 = t17-t18;
+t19 = t16.^2;
+t20 = -t19+1.0;
+t21 = 1.0./sqrt(t20);
+t24 = q0.*q3.*2.0;
+t25 = q1.*q2.*2.0;
+t22 = t24+t25;
+t23 = t4+t5-t6-t7;
+t26 = t23.^2;
+t27 = t22.^2;
+t28 = t26+t27;
+t29 = 1.0./t28;
+t30 = 1.0./t23;
+t31 = 1.0./t23.^2;
+error_transfer_matrix = reshape([t10.*t13.*(q1.*t14.*2.0-q0.*t2.*t15.*2.0),q2.*t21.*2.0,t26.*t29.*(q3.*t30.*2.0-q0.*t22.*t31.*2.0),t10.*t13.*(q0.*t14.*2.0+q1.*t2.*t15.*2.0),q3.*t21.*-2.0,t26.*t29.*(q2.*t30.*2.0-q1.*t22.*t31.*2.0),t10.*t13.*(q3.*t14.*2.0+q2.*t2.*t15.*2.0),q0.*t21.*2.0,t26.*t29.*(q1.*t30.*2.0+q2.*t22.*t31.*2.0),t10.*t13.*(q2.*t14.*2.0-q3.*t2.*t15.*2.0),q1.*t21.*-2.0,t26.*t29.*(q0.*t30.*2.0+q3.*t22.*t31.*2.0)],[3, 4]);
diff --git a/matlab/EKF_replay/Filter/replay_apm_data.m b/matlab/EKF_replay/Filter/replay_apm_data.m
old mode 100644
new mode 100755
index 7165cfc232..af76d37d62
--- a/matlab/EKF_replay/Filter/replay_apm_data.m
+++ b/matlab/EKF_replay/Filter/replay_apm_data.m
@@ -27,7 +27,7 @@ else
 end
 
 % load default parameters
-run('SetParameterDefaults.m');
+run('SetParameters.m');
 
 % run the filter replay 
 output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,rng_data,flow_data,viso_data);
diff --git a/matlab/EKF_replay/Filter/replay_px4_data.m b/matlab/EKF_replay/Filter/replay_px4_data.m
old mode 100644
new mode 100755
index c542a773fd..ce5a15c31c
--- a/matlab/EKF_replay/Filter/replay_px4_data.m
+++ b/matlab/EKF_replay/Filter/replay_px4_data.m
@@ -11,7 +11,7 @@ load '../TestData/PX4/imu_data.mat';
 load '../TestData/PX4/mag_data.mat';
 
 % set parameters to default values
-run('SetParameterDefaults.m');
+run('SetParameters.m');
 
 % run the filter replay
 output = RunFilter(param,imu_data,mag_data,baro_data,gps_data);