Merge branch 'pr-ekfOptFlowFixes'

* pr-ekfOptFlowFixes:
  EKF: fix bug causing height offset when GPS use stops
  EKF: Don't reject saturated flow data when it is the only aiding source
  EKF: Prevent flow motion check false positives
  EKF: Don't assume large position uncertainty when starting optical flow nav
  EKF: relax terrain update requirements for continuing optical flow use
  EKF: Relax minimum required range finder measurement rate
  EKF: relax optical flow on ground motion checks
  EKF: range finder aiding logic fixes
  EKF: Decouple range finder use criteria checking and selection
  EKF: Don't auto select range finder for height when on ground.
  EKF: Fix false triggering of optical flow bad motion checks
  EKF: update comments
  EKF: Don't use optical flow if GPS is good and the vehicle is not using range finder for height
  EKF: Stop using EV for yaw when GPS fusion starts
  EKF: Add persistence criteria to  GPS fail check
  EKF: allow GPS fallback if quality bad and alternative aiding available
  EKF: always run GPS checks

EKF/control.cpp

@@ -151,9 +151,7 @@ void Ekf::controlExternalVisionFusion()
 
 		// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
 		// needs to be calculated and the observations rotated into the EKF frame of reference
-		if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS)
-		    && !(_params.fusion_mode & MASK_USE_EVYAW)) {
-
+		if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_yaw) {
 			// rotate EV measurements into the EKF Navigation frame
 			calcExtVisRotMat();
 		}
@@ -182,7 +180,7 @@ void Ekf::controlExternalVisionFusion()
 		}
 
 		// external vision yaw aiding selection logic
-		if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
+		if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
 			// don't start using EV data unless daa is arriving frequently
 			if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
 				// reset the yaw angle to the value from the observaton quaternion
@@ -339,13 +337,14 @@ void Ekf::controlExternalVisionFusion()
 
 void Ekf::controlOpticalFlowFusion()
 {
-	// Detect if the vehicle is on the ground and is being excessively tilted, shaken or rotated.
+	// Check if motion is un-suitable for use of optical flow
 	if (!_control_status.flags.in_air) {
-		bool motion_is_excessive = ((_accel_mag_filt > 10.8f)
-					    || (_accel_mag_filt < 8.8f)
-					    || (_ang_rate_mag_filt > 0.5f)
-					    || (_R_to_earth(2, 2) < 0.866f));
-
+		// When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation
+		float accel_norm = _accel_vec_filt.norm();
+		bool motion_is_excessive = ((accel_norm > 14.7f) // accel greater than 1.5g
+					    || (accel_norm < 4.9f) // accel less than 0.5g
+					    || (_ang_rate_mag_filt > _params.flow_rate_max) // angular rate exceeds flow sensor limit
+					    || (_R_to_earth(2,2) < 0.866f)); // tilted more than 30 degrees
 		if (motion_is_excessive) {
 			_time_bad_motion_us = _imu_sample_delayed.time_us;
 
@@ -354,11 +353,17 @@ void Ekf::controlOpticalFlowFusion()
 		}
 
 	} else {
-		_time_bad_motion_us = 0;
-		_time_good_motion_us = _imu_sample_delayed.time_us;
+		bool good_gps_aiding = _control_status.flags.gps && ((_time_last_imu - _last_gps_fail_us) > (uint64_t)6e6);
+		if (good_gps_aiding && !_range_aid_mode_enabled) {
+			// Detect the special case where we are in flight, are using good quality GPS and speed and range has exceeded
+			// limits for use of range finder for height
+			_time_bad_motion_us = _imu_sample_delayed.time_us;
+		} else {
+			_time_good_motion_us = _imu_sample_delayed.time_us;
+		}
 	}
 
-	// Inhibit flow use if on ground and motion is excessive
+	// Inhibit flow use if motion is un-suitable
 	// Apply a time based hysteresis to prevent rapid mode switching
 	if (!_inhibit_gndobs_use) {
 		if ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5) {
@@ -438,8 +443,8 @@ void Ekf::controlOpticalFlowFusion()
 			}
 		}
 
-		// fuse the data if the terrain/distance to bottom is valid
-		if (_control_status.flags.opt_flow && get_terrain_valid()) {
+		// fuse the data if the terrain/distance to bottom is valid but use a more relaxed check to enable it to survive bad range finder data
+		if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_hagl_fuse < (uint64_t)10e6)) {
 			// Update optical flow bias estimates
 			calcOptFlowBias();
 
@@ -460,14 +465,17 @@ void Ekf::controlGpsFusion()
 
 		// Determine if we should use GPS aiding for velocity and horizontal position
 		// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
+		bool gps_checks_passing = (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6);
+		bool gps_checks_failing = (_time_last_imu - _last_gps_pass_us > (uint64_t)5e6);
 		if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
-			if (_control_status.flags.tilt_align && _NED_origin_initialised
-			    && (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6)) {
-
+			if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) {
 				// If the heading is not aligned, reset the yaw and magnetic field states
-				if (!_control_status.flags.yaw_align) {
+				// Do not use external vision for yaw if using GPS because yaw needs to be
+				// defined relative to an NED reference frame
+				if (!_control_status.flags.yaw_align || _control_status.flags.ev_yaw) {
+					_control_status.flags.yaw_align = false;
+					_control_status.flags.ev_yaw = false;
 					_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
-
 				}
 
 				// If the heading is valid start using gps aiding
@@ -490,7 +498,6 @@ void Ekf::controlGpsFusion()
 					if (_control_status.flags.gps) {
 						ECL_INFO("EKF commencing GPS fusion");
 						_time_last_gps = _time_last_imu;
-
 					}
 				}
 			}
@@ -500,6 +507,12 @@ void Ekf::controlGpsFusion()
 
 		}
 
+		// Handle the case where we are using GPS and another source of aiding and GPS is failing checks
+		if (_control_status.flags.gps  && gps_checks_failing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos)) {
+			_control_status.flags.gps = false;
+			ECL_WARN("EKF GPS data quality poor - stopping use");
+		}
+
 		// handle the case when we now have GPS, but have not been using it for an extended period
 		if (_control_status.flags.gps) {
 			// We are relying on aiding to constrain drift so after a specified time
@@ -848,11 +861,13 @@ void Ekf::controlHeightFusion()
 		_range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
 	}
 
+	rangeAidConditionsMet();
+
+	_range_aid_mode_selected = (_params.range_aid == 1) && _range_aid_mode_enabled;
 
 	if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
-		_in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode);
 
-		if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) {
+		if (_range_aid_mode_selected && _range_data_ready && !_rng_hgt_faulty) {
 			setControlRangeHeight();
 			_fuse_height = true;
 
@@ -867,10 +882,9 @@ void Ekf::controlHeightFusion()
 				}
 			}
 
-		} else if (!_in_range_aid_mode && _baro_data_ready && !_baro_hgt_faulty) {
+		} else if (!_range_aid_mode_selected && _baro_data_ready && !_baro_hgt_faulty) {
 			setControlBaroHeight();
 			_fuse_height = true;
-			_in_range_aid_mode = false;
 
 			// we have just switched to using baro height, we don't need to set a height sensor offset
 			// since we track a separate _baro_hgt_offset
@@ -892,7 +906,6 @@ void Ekf::controlHeightFusion()
 		} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_faulty) {
 			// switch to gps if there was a reset to gps
 			_fuse_height = true;
-			_in_range_aid_mode = false;
 
 			// we have just switched to using gps height, calculate height sensor offset such that current
 			// measurment matches our current height estimate
@@ -938,9 +951,8 @@ void Ekf::controlHeightFusion()
 
 	// Determine if GPS should be used as the height source
 	if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
-		_in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode);
 
-		if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) {
+		if (_range_aid_mode_selected && _range_data_ready && !_rng_hgt_faulty) {
 			setControlRangeHeight();
 			_fuse_height = true;
 
@@ -955,10 +967,9 @@ void Ekf::controlHeightFusion()
 				}
 			}
 
-		} else if (!_in_range_aid_mode && _gps_data_ready && !_gps_hgt_faulty) {
+		} else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_faulty) {
 			setControlGPSHeight();
 			_fuse_height = true;
-			_in_range_aid_mode = false;
 
 			// we have just switched to using gps height, calculate height sensor offset such that current
 			// measurment matches our current height estimate
@@ -969,7 +980,6 @@ void Ekf::controlHeightFusion()
 		} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
 			// switch to baro if there was a reset to baro
 			_fuse_height = true;
-			_in_range_aid_mode = false;
 
 			// we have just switched to using baro height, we don't need to set a height sensor offset
 			// since we track a separate _baro_hgt_offset
@@ -984,7 +994,6 @@ void Ekf::controlHeightFusion()
 		if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
 			// switch to baro if there was a reset to baro
 			_fuse_height = true;
-			_in_range_aid_mode = false;
 
 			// we have just switched to using baro height, we don't need to set a height sensor offset
 			// since we track a separate _baro_hgt_offset
@@ -1022,18 +1031,18 @@ void Ekf::controlHeightFusion()
 
 }
 
-bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
+void Ekf::rangeAidConditionsMet()
 {
 	// if the parameter for range aid is enabled we allow to switch from using the primary height source to using range finder as height source
 	// under the following conditions
-	// 1) we are not further than max_range_for_dual_fusion away from the ground
-	// 2) our ground speed is not higher than max_vel_for_dual_fusion
+	// 1) we are not further than max_hagl_for_range_aid away from the ground
+	// 2) our ground speed is not higher than max_vel_for_range_aid
 	// 3) Our terrain estimate is stable (needs better checks)
-	if (_params.range_aid) {
+	// 4) We are in-air
+	if (_control_status.flags.in_air) {
 		// check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching
 		bool use_range_finder;
-
-		if (in_range_aid_mode) {
+		if (_range_aid_mode_enabled) {
 			use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid();
 
 		} else {
@@ -1048,7 +1057,7 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
 		if (horz_vel_valid) {
 			float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1));
 
-			if (in_range_aid_mode) {
+			if (_range_aid_mode_enabled) {
 				use_range_finder &= ground_vel < _params.max_vel_for_range_aid;
 
 			} else {
@@ -1062,7 +1071,7 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
 		}
 
 		// use hysteresis to check for hagl
-		if (in_range_aid_mode) {
+		if (_range_aid_mode_enabled) {
 			use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f);
 
 		} else {
@@ -1071,10 +1080,10 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
 			use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 0.01f);
 		}
 
-		return use_range_finder;
+		_range_aid_mode_enabled = use_range_finder;
 
 	} else {
-		return false;
+		_range_aid_mode_enabled = false;
 	}
 }
 

EKF/covariance.cpp

@@ -163,9 +163,13 @@ void Ekf::predictCovariance()
 	float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f);
 
 	// inhibit learning of imu acccel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
-	float alpha = 1.0f - math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
-	_ang_rate_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), alpha * _ang_rate_mag_filt);
-	_accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), alpha * _accel_mag_filt);
+	float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
+	float beta = 1.0f - alpha;
+	_ang_rate_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_mag_filt);
+	_accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_mag_filt);
+	_accel_vec_filt(0) = alpha * dt_inv * _imu_sample_delayed.delta_vel(0) + beta * _accel_vec_filt(0);
+	_accel_vec_filt(1) = alpha * dt_inv * _imu_sample_delayed.delta_vel(1) + beta * _accel_vec_filt(1);
+	_accel_vec_filt(2) = alpha * dt_inv * _imu_sample_delayed.delta_vel(2) + beta * _accel_vec_filt(2);
 
 	if (_ang_rate_mag_filt > _params.acc_bias_learn_gyr_lim
 	    || _accel_mag_filt > _params.acc_bias_learn_acc_lim

EKF/ekf.h

@@ -372,6 +372,7 @@ private:
 	float _gps_velN_filt{0.0f};		///< GPS filtered North velocity (m/sec)
 	float _gps_velE_filt{0.0f};		///< GPS filtered East velocity (m/sec)
 	uint64_t _last_gps_fail_us{0};		///< last system time in usec that the GPS failed it's checks
+	uint64_t _last_gps_pass_us{0};		///< last system time in usec that the GPS passed it's checks
 
 	// Variables used to publish the WGS-84 location of the EKF local NED origin
 	uint64_t _last_gps_origin_time_us{0};	///< time the origin was last set (uSec)
@@ -401,7 +402,8 @@ private:
 
 	// variables used to inhibit accel bias learning
 	bool _accel_bias_inhibit{false};	///< true when the accel bias learning is being inhibited
-	float _accel_mag_filt{0.0f};		///< acceleration magnitude after application of a decaying envelope filter (m/sec**2)
+	Vector3f _accel_vec_filt{};		///< acceleration vector after application of a low pass filter (m/sec**2)
+	float _accel_mag_filt{0.0f};	///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
 	float _ang_rate_mag_filt{0.0f};		///< angular rate magnitude after application of a decaying envelope filter (rad/sec)
 	Vector3f _prev_dvel_bias_var;		///< saved delta velocity XYZ bias variances (m/sec)**2
 
@@ -431,7 +433,8 @@ private:
 	bool _bad_vert_accel_detected{false};	///< true when bad vertical accelerometer data has been detected
 
 	// variables used to control range aid functionality
-	bool _in_range_aid_mode{false};		///< true when range finder is to be used as the height reference instead of the primary height sensor
+	bool _range_aid_mode_enabled{false};	///< true when range finder can be used as the height reference instead of the primary height sensor
+	bool _range_aid_mode_selected{false};	///< true when range finder is being used as the height reference instead of the primary height sensor
 
 	// variables used to check for "stuck" rng data
 	float _rng_check_min_val{0.0f};		///< minimum value for new rng measurement when being stuck
@@ -583,7 +586,8 @@ private:
 	// control for combined height fusion mode (implemented for switching between baro and range height)
 	void controlHeightFusion();
 
-	bool rangeAidConditionsMet(bool in_range_aid_mode);
+	// determine if flight condition is suitable so use range finder instead of the primary height senor
+	void rangeAidConditionsMet();
 
 	// check for "stuck" range finder measurements when rng was not valid for certain period
 	void checkForStuckRange();

EKF/ekf_helper.cpp

@@ -168,7 +168,9 @@ bool Ekf::resetPosition()
 
 		}
 
-		setDiag(P, 7, 8, sq(_params.pos_noaid_noise));
+		// estimate is relative to initial positon in this mode, so we start with zero error.
+		zeroCols(P,7,8);
+		zeroRows(P,7,8);
 
 	} else {
 		// Used when falling back to non-aiding mode of operation
@@ -567,7 +569,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 		Dcmf R_to_earth(euler321);
 
 		// calculate the observed yaw angle
-		if (_params.fusion_mode & MASK_USE_EVYAW) {
+		if (_control_status.flags.ev_yaw) {
 			// convert the observed quaternion to a rotation matrix
 			Dcmf R_to_earth_ev(_ev_sample_delayed.quat);	// transformation matrix from body to world frame
 			// calculate the yaw angle for a 312 sequence
@@ -621,7 +623,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 		R_to_earth(2, 1) = s1;
 
 		// calculate the observed yaw angle
-		if (_params.fusion_mode & MASK_USE_EVYAW) {
+		if (_control_status.flags.ev_yaw) {
 			// convert the observed quaternion to a rotation matrix
 			Dcmf R_to_earth_ev(_ev_sample_delayed.quat);	// transformation matrix from body to world frame
 			// calculate the yaw angle for a 312 sequence
@@ -703,14 +705,12 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 		_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
 
 		// reset the rotation from the EV to EKF frame of reference if it is being used
-		if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS)
-		    && !(_params.fusion_mode & MASK_USE_EVYAW)) {
-
+		if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_yaw) {
 			resetExtVisRotMat();
 		}
 
 		// update the yaw angle variance using the variance of the measurement
-		if (_params.fusion_mode & MASK_USE_EVYAW) {
+		if (!_control_status.flags.ev_yaw) {
 			// using error estimate from external vision data
 			angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
 

EKF/estimator_interface.cpp

@@ -364,20 +364,24 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 
 
 		// check magnitude is within sensor limits
+		// use this to prevent use of a saturated flow sensor when there are other aiding sources available
 		float flow_rate_magnitude;
 		bool flow_magnitude_good = true;
-
 		if (delta_time_good) {
 			flow_rate_magnitude = flow->flowdata.norm() / delta_time;
 			flow_magnitude_good = (flow_rate_magnitude <= _params.flow_rate_max);
 		}
 
+		bool relying_on_flow =  _control_status.flags.opt_flow
+				&& !_control_status.flags.gps
+				&& !_control_status.flags.ev_pos;
+
 		// check quality metric
 		bool flow_quality_good = (flow->quality >= _params.flow_qual_min);
 
 		// Always use data when on ground to allow for bad quality due to unfocussed sensors and operator handling
 		// If flow quality fails checks on ground, assume zero flow rate after body rate compensation
-		if ((delta_time_good && flow_quality_good && flow_magnitude_good) || !_control_status.flags.in_air) {
+		if ((delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow)) || !_control_status.flags.in_air) {
 			flowSample optflow_sample_new;
 			// calculate the system time-stamp for the mid point of the integration period
 			optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000 - flow->dt / 2;

EKF/gps_checks.cpp

@@ -58,46 +58,41 @@
 
 bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
 {
-	// Run GPS checks whenever the WGS-84 origin is not set or the vehicle is not using GPS
-	// Also run checks if the vehicle is on-ground as the check data can be used by vehicle pre-flight checks
-	if (!_control_status.flags.in_air || !_NED_origin_initialised || !_control_status.flags.gps) {
-		bool gps_checks_pass = gps_is_good(gps);
-
-		if (!_NED_origin_initialised && gps_checks_pass) {
-			// If we have good GPS data set the origin's WGS-84 position to the last gps fix
-			double lat = gps->lat / 1.0e7;
-			double lon = gps->lon / 1.0e7;
-			map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
-
-			// if we are already doing aiding, corect for the change in posiiton since the EKF started navigating
-			if (_control_status.flags.opt_flow || _control_status.flags.gps || _control_status.flags.ev_pos) {
-				double est_lat, est_lon;
-				map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon);
-				map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);
-			}
-
-			// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
-			_gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2);
-			_NED_origin_initialised = true;
-			_last_gps_origin_time_us = _time_last_imu;
-			// set the magnetic declination returned by the geo library using the current GPS position
-			_mag_declination_gps = math::radians(get_mag_declination(lat, lon));
-			// save the horizontal and vertical position uncertainty of the origin
-			_gps_origin_eph = gps->eph;
-			_gps_origin_epv = gps->epv;
-
-			// if the user has selected GPS as the primary height source, switch across to using it
-			if (_primary_hgt_source == VDIST_SENSOR_GPS) {
-				ECL_INFO("EKF GPS checks passed (WGS-84 origin set, using GPS height)");
-				_control_status.flags.baro_hgt = false;
-				_control_status.flags.gps_hgt = true;
-				_control_status.flags.rng_hgt = false;
-				// zero the sensor offset
-				_hgt_sensor_offset = 0.0f;
-
-			} else {
-				ECL_INFO("EKF GPS checks passed (WGS-84 origin set)");
-			}
+	// Run GPS checks always
+	bool gps_checks_pass = gps_is_good(gps);
+	if (!_NED_origin_initialised && gps_checks_pass) {
+		// If we have good GPS data set the origin's WGS-84 position to the last gps fix
+		double lat = gps->lat / 1.0e7;
+		double lon = gps->lon / 1.0e7;
+		map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
+
+		// if we are already doing aiding, corect for the change in posiiton since the EKF started navigating
+		if (_control_status.flags.opt_flow || _control_status.flags.gps || _control_status.flags.ev_pos) {
+			double est_lat, est_lon;
+			map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon);
+			map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);
+		}
+
+		// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
+		_gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2);
+		_NED_origin_initialised = true;
+		_last_gps_origin_time_us = _time_last_imu;
+		// set the magnetic declination returned by the geo library using the current GPS position
+		_mag_declination_gps = math::radians(get_mag_declination(lat, lon));
+		// save the horizontal and vertical position uncertainty of the origin
+		_gps_origin_eph = gps->eph;
+		_gps_origin_epv = gps->epv;
+
+		// if the user has selected GPS as the primary height source, switch across to using it
+		if (_primary_hgt_source == VDIST_SENSOR_GPS) {
+			ECL_INFO("EKF GPS checks passed (WGS-84 origin set, using GPS height)");
+			_control_status.flags.baro_hgt = false;
+			_control_status.flags.gps_hgt = true;
+			_control_status.flags.rng_hgt = false;
+			// zero the sensor offset
+			_hgt_sensor_offset = 0.0f;
+		} else {
+			ECL_INFO("EKF GPS checks passed (WGS-84 origin set)");
 		}
 	}
 
@@ -233,6 +228,8 @@ bool Ekf::gps_is_good(struct gps_message *gps)
 		(_gps_check_fail_status.flags.vspeed  && (_params.gps_check_mask & MASK_GPS_VSPD))
 	) {
 		_last_gps_fail_us = _time_last_imu;
+	} else {
+		_last_gps_pass_us = _time_last_imu;
 	}
 
 	// continuous period without fail of 10 seconds required to return a healthy status

EKF/terrain_estimator.cpp

@@ -207,16 +207,17 @@ void Ekf::get_hagl_innov_var(float *hagl_innov_var)
 // check that the range finder data is continuous
 void Ekf::checkRangeDataContinuity()
 {
-	// update range data continuous flag (2Hz ie 500 ms)
+	// update range data continuous flag (1Hz ie 1000 ms)
 	/* Timing in micro seconds */
 
-	/* Apply a 1.0 sec low pass filter to the time delta from the last range finder updates */
-	_dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - _dt_update) + _dt_update *
+	/* Apply a 2.0 sec low pass filter to the time delta from the last range finder updates */
+	float alpha = 0.5f * _dt_update;
+	_dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - alpha) + alpha *
 					(_time_last_imu - _time_last_range);
 
-	_dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 1e6f);
+	_dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 2e6f);
 
-	if (_dt_last_range_update_filt_us < 5e5f) {
+	if (_dt_last_range_update_filt_us < 1e6f) {
 		_range_data_continuous = true;
 
 	} else {