Merge pull request #78 from PX4/pr-addGpsHgtOption

EKF: Add option to use GPS for height and improve height fall-back behaviour

EKF/common.h

@@ -90,6 +90,9 @@ struct gpsSample {
 	Vector2f    pos;	// NE earth frame gps horizontal position measurement in m
 	float       hgt;	// gps height measurement in m
 	Vector3f    vel;	// NED earth frame gps velocity measurement in m/s
+	float	    hacc;	// 1-std horizontal position error m
+	float	    vacc;	// 1-std vertical position error m
+	float       sacc;	// 1-std speed error m/s
 	uint64_t    time_us;	// timestamp in microseconds
 };
 
@@ -142,6 +145,11 @@ struct flowSample {
 #define MAG_FUSE_TYPE_3D        2   // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
 #define MAG_FUSE_TYPE_2D        3   // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions.
 
+// Maximum sensor intervals in usec
+#define GPS_MAX_INTERVAL	5e5
+#define BARO_MAX_INTERVAL	2e5
+#define RNG_MAX_INTERVAL	2e5
+
 struct parameters {
 	// measurement source control
 	int fusion_mode;		// bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
@@ -176,6 +184,7 @@ struct parameters {
 	float baro_innov_gate;		// barometric height innovation consistency gate size (STD)
 	float posNE_innov_gate;		// GPS horizontal position innovation consistency gate size (STD)
 	float vel_innov_gate;		// GPS velocity innovation consistency gate size (STD)
+	float hgt_reset_lim;		// The maximum 1-sigma uncertainty in height that can be tolerated before the height state is reset (m)
 
 	// magnetometer fusion
 	float mag_heading_noise;	// measurement noise used for simple heading fusion (rad)
@@ -245,6 +254,7 @@ struct parameters {
 		baro_innov_gate = 3.0f;
 		posNE_innov_gate = 3.0f;
 		vel_innov_gate = 3.0f;
+		hgt_reset_lim = 5.0f;
 
 		// magnetometer fusion
 		mag_heading_noise = 1.7e-1f;
@@ -341,4 +351,5 @@ union filter_control_status_u {
 	} flags;
 	uint16_t value;
 };
+
 }

EKF/control.cpp

@@ -158,10 +158,85 @@ void Ekf::controlFusionModes()
 		}
 	}
 
-	// Handle the case where we have rejected height measurements for an extended period
-	// This excessive vibration levels can cause this so a reset gives the filter a chance to recover
-	// After 10 seconds without aiding we reset to the height measurement
-	if (_time_last_imu - _time_last_hgt_fuse > 10e6) {
+	/*
+	 * Handle the case where we have not fused height measurements recently and
+	 * uncertainty exceeds the max allowable. Reset using the best available height
+	 * measurement source, continue using it after the reset and declare the current
+	 * source failed if we have switched.
+	*/
+	if ((P[8][8] > sq(_params.hgt_reset_lim)) && ((_time_last_imu - _time_last_hgt_fuse) > 5e6)) {
+		// handle the case where we are using baro for height
+		if (_control_status.flags.baro_hgt) {
+			// check if GPS height is available
+			gpsSample gps_init = _gps_buffer.get_newest();
+			bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
+			bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
+			baroSample baro_init = _baro_buffer.get_newest();
+			bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
+
+			// use the gps if it is accurate or there is no baro data available
+			if (gps_hgt_available && (gps_hgt_accurate || !baro_hgt_available)) {
+				// declare the baro as unhealthy
+				_baro_hgt_faulty = true;
+				// set the height mode to the GPS
+				_control_status.flags.baro_hgt = false;
+				_control_status.flags.gps_hgt = true;
+				_control_status.flags.rng_hgt = false;
+				// adjust the height offset so we can use the GPS
+				_hgt_sensor_offset = _state.pos(2) + gps_init.hgt - _gps_alt_ref;
+				printf("EKF baro hgt timeout - switching to gps\n");
+			}
+		}
+
+		// handle the case we are using GPS for height
+		if (_control_status.flags.gps_hgt) {
+			// check if GPS height is available
+			gpsSample gps_init = _gps_buffer.get_newest();
+			bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
+			bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
+			// check the baro height source for consistency and freshness
+			baroSample baro_init = _baro_buffer.get_newest();
+			bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
+			float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
+			bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate);
+
+			// if baro data is consistent and fresh or GPS height is unavailable or inaccurate, we switch to baro for height
+			if ((baro_data_consistent && baro_data_fresh) || !gps_hgt_available || !gps_hgt_accurate) {
+				// declare the GPS height unhealthy
+				_gps_hgt_faulty = true;
+				// set the height mode to the baro
+				_control_status.flags.baro_hgt = true;
+				_control_status.flags.gps_hgt = false;
+				_control_status.flags.rng_hgt = false;
+				printf("EKF gps hgt timeout - switching to baro\n");
+			}
+		}
+
+		// handle the case we are using range finder for height
+		if (_control_status.flags.rng_hgt) {
+			// check if range finder data is available
+			rangeSample rng_init = _range_buffer.get_newest();
+			bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL);
+			// check if baro data is available
+			baroSample baro_init = _baro_buffer.get_newest();
+			bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
+			// check if baro data is consistent
+			float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
+			bool baro_data_consistent = sq(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate);
+			// switch to baro if necessary or preferable
+			bool switch_to_baro = (!rng_data_available && baro_data_available) || (baro_data_consistent && baro_data_available);
+
+			if (switch_to_baro) {
+				// declare the range finder height unhealthy
+				_rng_hgt_faulty = true;
+				// set the height mode to the baro
+				_control_status.flags.baro_hgt = true;
+				_control_status.flags.gps_hgt = false;
+				_control_status.flags.rng_hgt = false;
+				printf("EKF rng hgt timeout - switching to baro\n");
+			}
+		}
+
 		// Reset vertical position and velocity states to the last measurement
 		resetHeight();
 	}
@@ -248,21 +323,20 @@ void Ekf::controlFusionModes()
 	}
 
 	// Control the soure of height measurements for the main filter
-	if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
+	if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) {
 		_control_status.flags.baro_hgt = true;
-		_control_status.flags.rng_hgt = false;
 		_control_status.flags.gps_hgt = false;
+		_control_status.flags.rng_hgt = false;
 
-	} else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
+	} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS && !_gps_hgt_faulty) || _control_status.flags.gps_hgt) {
 		_control_status.flags.baro_hgt = false;
+		_control_status.flags.gps_hgt = true;
+		_control_status.flags.rng_hgt = false;
+
+	} else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE && !_rng_hgt_faulty) {
+		_control_status.flags.baro_hgt = false;
+		_control_status.flags.gps_hgt = false;
 		_control_status.flags.rng_hgt = true;
-		_control_status.flags.gps_hgt = false;
-
-	} else {
-		// TODO functionality to fuse GPS height
-		_control_status.flags.baro_hgt = false;
-		_control_status.flags.rng_hgt = false;
-		_control_status.flags.gps_hgt = false;
 	}
 
 	// Placeholder for control of wind velocity states estimation

EKF/ekf.cpp

@@ -71,19 +71,19 @@ Ekf::Ekf():
 	_last_gps_origin_time_us(0),
 	_gps_alt_ref(0.0f),
 	_hgt_counter(0),
-	_time_last_hgt(0),
-	_hgt_sum(0.0f),
+	_hgt_filt_state(0.0f),
 	_mag_counter(0),
 	_time_last_mag(0),
-	_hgt_at_alignment(0.0f),
+	_hgt_sensor_offset(0.0f),
 	_terrain_vpos(0.0f),
 	_hagl_innov(0.0f),
 	_hagl_innov_var(0.0f),
 	_time_last_hagl_fuse(0),
+	_baro_hgt_faulty(false),
+	_gps_hgt_faulty(false),
+	_rng_hgt_faulty(false),
 	_baro_hgt_offset(0.0f)
 {
-	_control_status = {};
-	_control_status_prev = {};
 	_state = {};
 	_last_known_posNE.setZero();
 	_earth_rate_NED.setZero();
@@ -100,7 +100,7 @@ Ekf::Ekf():
 	_last_known_posNE.setZero();
 	_imu_down_sampled = {};
 	_q_down_sampled.setZero();
-	_mag_sum = {};
+	_mag_filt_state = {};
 	_delVel_sum = {};
 	_flow_gyro_bias = {};
 	_imu_del_ang_of = {};
@@ -154,19 +154,22 @@ bool Ekf::init(uint64_t timestamp)
 	_filter_initialised = false;
 	_terrain_initialised = false;
 
+	_control_status.value = 0;
+	_control_status_prev.value = 0;
+
 	return ret;
 }
 
 bool Ekf::update()
 {
 
-		if (!_filter_initialised) {
-			_filter_initialised = initialiseFilter();
+	if (!_filter_initialised) {
+		_filter_initialised = initialiseFilter();
 
-			if (!_filter_initialised) {
-				return false;
-			}
+		if (!_filter_initialised) {
+			return false;
 		}
+	}
 
 	// Only run the filter if IMU data in the buffer has been updated
 	if (_imu_updated) {
@@ -221,22 +224,16 @@ bool Ekf::update()
 				_fuse_height = true;
 			}
 
-		} else if ((_time_last_imu - _time_last_hgt_fuse) > 5e5 && _control_status.flags.rng_hgt) {
+		} else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) {
 			// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
 			// and are on the ground, then synthesise a measurement at the expected on ground value
 			if (!_in_air) {
 				_range_sample_delayed.rng = _params.rng_gnd_clearance;
 				_range_sample_delayed.time_us = _imu_sample_delayed.time_us;
 
-			} else {
-				// if this happens in the air, fuse the baro measurement to constrain drift
-				// use the baro for this update only
-				_control_status.flags.baro_hgt = true;
-				_control_status.flags.rng_hgt = false;
 			}
 
 			_fuse_height = true;
-
 		}
 
 		// determine if baro data has fallen behind the fuson time horizon and fuse it in the main filter if enabled
@@ -246,16 +243,25 @@ bool Ekf::update()
 
 			} else {
 				// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro  as a height reference
-				float offset_error = _baro_sample_delayed.hgt + _state.pos(2) - _baro_hgt_offset;
+				float offset_error =  _state.pos(2) + _baro_sample_delayed.hgt - _hgt_sensor_offset - _baro_hgt_offset;
 				_baro_hgt_offset += 0.02f * math::constrain(offset_error, -5.0f, 5.0f);
 			}
 		}
 
 		// If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it
-		if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) {
-			_fuse_pos = true;
-			_fuse_vert_vel = true;
-			_fuse_hor_vel = true;
+		if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
+			// Only use GPS data for position and velocity aiding if enabled
+			if (_control_status.flags.gps) {
+				_fuse_pos = true;
+				_fuse_vert_vel = true;
+				_fuse_hor_vel = true;
+			}
+
+			// only use gps height observation in the main filter if specifically enabled
+			if (_control_status.flags.gps_hgt) {
+				_fuse_height = true;
+			}
+
 		}
 
 		// If we are using optical flow aiding and data has fallen behind the fusion time horizon, then fuse it
@@ -302,6 +308,11 @@ bool Ekf::update()
 	// the output observer always runs
 	calculateOutputStates();
 
+	// check for NaN on attitude states
+	if (isnan(_state.quat_nominal(0)) || isnan(_output_new.quat_nominal(0))) {
+		return false;
+	}
+
 	// We don't have valid data to output until tilt and yaw alignment is complete
 	if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
 		return true;
@@ -320,30 +331,44 @@ bool Ekf::initialiseFilter(void)
 	_delVel_sum += imu_init.delta_vel;
 
 	// Sum the magnetometer measurements
-	magSample mag_init = _mag_buffer.get_newest();
-
-	if (mag_init.time_us != 0) {
-		_mag_counter ++;
-		_mag_sum += mag_init.mag;
-	}
-
-	if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
-		rangeSample range_init = _range_buffer.get_newest();
-
-		if (range_init.time_us != _time_last_hgt) {
-			_hgt_counter ++;
-			_hgt_sum += range_init.rng;
-			_time_last_hgt = range_init.time_us;
+	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
+		if (_mag_counter == 0) {
+			_mag_filt_state = _mag_sample_delayed.mag;
 		}
 
-	} else if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
-		// initialize vertical position with newest baro measurement
-		baroSample baro_init = _baro_buffer.get_newest();
+		_mag_counter ++;
+		_mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f;
+	}
+
+	// set the default height source from the adjustable parameter
+	if (_hgt_counter == 0) {
+		_primary_hgt_source = _params.vdist_sensor_type;
+	}
+
+	if (_primary_hgt_source == VDIST_SENSOR_RANGE) {
+		if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
+			if (_hgt_counter == 0) {
+				_control_status.flags.baro_hgt = false;
+				_control_status.flags.gps_hgt = false;
+				_control_status.flags.rng_hgt = true;
+				_hgt_filt_state = _range_sample_delayed.rng;
+			}
 
-		if (baro_init.time_us != _time_last_hgt) {
 			_hgt_counter ++;
-			_hgt_sum += baro_init.hgt;
-			_time_last_hgt = baro_init.time_us;
+			_hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _range_sample_delayed.rng;
+		}
+
+	} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) {
+		if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
+			if (_hgt_counter == 0) {
+				_control_status.flags.baro_hgt = true;
+				_control_status.flags.gps_hgt = false;
+				_control_status.flags.rng_hgt = false;
+				_hgt_filt_state = _baro_sample_delayed.hgt;
+			}
+
+			_hgt_counter ++;
+			_hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _baro_sample_delayed.hgt;
 		}
 
 	} else {
@@ -351,10 +376,14 @@ bool Ekf::initialiseFilter(void)
 	}
 
 	// check to see if we have enough measurements and return false if not
-	if (_hgt_counter < 10 || _mag_counter < 10) {
+	if (_hgt_counter <= 10 || _mag_counter <= 10) {
 		return false;
 
 	} else {
+		// reset variables that are shared with post alignment GPS checks
+		_gps_drift_velD = 0.0f;
+		_gps_alt_ref = 0.0f;
+
 		// Zero all of the states
 		_state.ang_error.setZero();
 		_state.vel.setZero();
@@ -388,26 +417,24 @@ bool Ekf::initialiseFilter(void)
 		_tilt_err_length_filt = 1.0f;
 
 		// calculate the averaged magnetometer reading
-		Vector3f mag_init = _mag_sum * (1.0f / (float(_mag_counter)));
+		Vector3f mag_init = _mag_filt_state;
 
 		// calculate the initial magnetic field and yaw alignment
 		resetMagHeading(mag_init);
 
 		// calculate the averaged height reading to calulate the height of the origin
-		_hgt_at_alignment = _hgt_sum / (float)_hgt_counter;
+		_hgt_sensor_offset = _hgt_filt_state;
 
 		// if we are not using the baro height as the primary source, then calculate an offset relative to the origin
 		// so it can be used as a backup
-		if (_params.vdist_sensor_type != VDIST_SENSOR_BARO) {
+		if (!_control_status.flags.baro_hgt) {
 			baroSample baro_newest = _baro_buffer.get_newest();
-			_baro_hgt_offset = baro_newest.hgt - _hgt_at_alignment;
+			_baro_hgt_offset = baro_newest.hgt - _hgt_sensor_offset;
+
 		} else {
 			_baro_hgt_offset = 0.0f;
 		}
 
-		// set the velocity to the GPS measurement (by definition, the initial position and height is at the origin)
-		resetVelocity();
-
 		// initialise the state covariance matrix
 		initialiseCovariance();
 

EKF/ekf.h

@@ -98,12 +98,6 @@ public:
 	bool collect_gps(uint64_t time_usec, struct gps_message *gps);
 	bool collect_imu(imuSample &imu);
 
-	// this is the current status of the filter control modes
-	filter_control_status_u _control_status;
-
-	// this is the previous status of the filter control modes - used to detect mode transitions
-	filter_control_status_u _control_status_prev;
-
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt);
 
@@ -197,14 +191,13 @@ private:
 	float _gps_alt_ref;		// WGS-84 height (m)
 
 	// Variables used to initialise the filter states
-	uint8_t _hgt_counter;		// number of height samples averaged
-	uint64_t _time_last_hgt;	// measurement time of last height sample
-	float _hgt_sum;			// summed height measurement
-	uint8_t _mag_counter;		// number of magnetometer samples averaged
+	uint32_t _hgt_counter;		// number of height samples taken
+	float _hgt_filt_state;		// filtered height measurement
+	uint32_t _mag_counter;		// number of magnetometer samples taken
 	uint64_t _time_last_mag;	// measurement time of last magnetomter sample
-	Vector3f _mag_sum;		// summed magnetometer measurement
+	Vector3f _mag_filt_state;	// filtered magnetometer measurement
 	Vector3f _delVel_sum;		// summed delta velocity
-	float _hgt_at_alignment;	// baro offset relative to alignment position
+	float _hgt_sensor_offset;	// height that needs to be subtracted from the primary height sensor so that it reads zero height at the origin (m)
 
 	gps_check_fail_status_u _gps_check_fail_status;
 
@@ -216,6 +209,12 @@ private:
 	uint64_t _time_last_hagl_fuse;	// last system time in usec that the hagl measurement failed it's checks
 	bool _terrain_initialised;	// true when the terrain estimator has been intialised
 
+	// height sensor fault status
+	bool _baro_hgt_faulty;		// true if valid baro data is unavailable for use
+	bool _gps_hgt_faulty;		// true if valid gps height data is unavailable for use
+	bool _rng_hgt_faulty;		// true if valid rnage finder height data is unavailable for use
+	int _primary_hgt_source;	// priary source of height data set at initialisation
+
 	float _baro_hgt_offset;		// number of metres the baro height origin is above the local NED origin (m)
 
 	// update the real time complementary filter states. This includes the prediction

EKF/ekf_helper.cpp

@@ -90,8 +90,8 @@ void Ekf::resetHeight()
 	if (_control_status.flags.rng_hgt) {
 		rangeSample range_newest = _range_buffer.get_newest();
 
-		if (_time_last_imu - range_newest.time_us < 200000) {
-			_state.pos(2) = _hgt_at_alignment - range_newest.rng;
+		if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) {
+			_state.pos(2) = _hgt_sensor_offset - range_newest.rng;
 
 		} else {
 			// TODO: reset to last known range based estimate
@@ -105,18 +105,28 @@ void Ekf::resetHeight()
 		// initialize vertical position with newest baro measurement
 		baroSample baro_newest = _baro_buffer.get_newest();
 
-		if (_time_last_imu - baro_newest.time_us < 200000) {
-			_state.pos(2) = _hgt_at_alignment - baro_newest.hgt;
+		if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) {
+			_state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset;
 
 		} else {
 			// TODO: reset to last known baro based estimate
 		}
 
-		// the baro height offset should be zero if baro is our primary height source
-		_baro_hgt_offset = 0.0f;
+	} else if (_control_status.flags.gps_hgt) {
+		// initialize vertical position and velocity with newest gps measurement
+		gpsSample gps_newest = _gps_buffer.get_newest();
 
-	} else {
-		// TODO: reset to GPS height
+		if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) {
+			_state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref;
+			_state.vel(2) = gps_newest.vel(2);
+
+		} else {
+			// TODO: reset to last known gps based estimate
+		}
+
+		// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
+		baroSample baro_newest = _baro_buffer.get_newest();
+		_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
 	}
 
 }

EKF/estimator_interface.cpp

@@ -56,8 +56,6 @@ EstimatorInterface::EstimatorInterface():
 	_in_air(false),
 	_NED_origin_initialised(false),
 	_gps_speed_valid(false),
-	_gps_speed_accuracy(0.0f),
-	_gps_hpos_accuracy(0.0f),
 	_gps_origin_eph(0.0f),
 	_gps_origin_epv(0.0f),
 	_mag_healthy(false),
@@ -145,12 +143,8 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float *data)
 
 void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
 {
-	if (!collect_gps(time_usec, gps) || !_initialised) {
-		return;
-	}
-
-	// Only use GPS data if we have a 3D fix and limit the GPS data rate to a maximum of 14Hz
-	if (time_usec - _time_last_gps > 70000 && gps->fix_type >= 3) {
+	// Limit the GPS data rate to a maximum of 14Hz
+	if (time_usec - _time_last_gps > 70000) {
 		gpsSample gps_sample_new = {};
 		gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
 
@@ -162,15 +156,24 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
 		memcpy(gps_sample_new.vel._data[0], gps->vel_ned, sizeof(gps_sample_new.vel._data));
 
 		_gps_speed_valid = gps->vel_ned_valid;
-		_gps_speed_accuracy = gps->sacc;
-		_gps_hpos_accuracy = gps->eph;
+		gps_sample_new.sacc = gps->sacc;
+		gps_sample_new.hacc = gps->eph;
+		gps_sample_new.vacc = gps->epv;
 
-		float lpos_x = 0.0f;
-		float lpos_y = 0.0f;
-		map_projection_project(&_pos_ref, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y);
-		gps_sample_new.pos(0) = lpos_x;
-		gps_sample_new.pos(1) = lpos_y;
-		gps_sample_new.hgt = gps->alt / 1e3f;
+		gps_sample_new.hgt = (float)gps->alt * 1e-3f;
+
+		// Only calculate the relative position if the WGS-84 location of the origin is set
+		if (collect_gps(time_usec, gps)) {
+			float lpos_x = 0.0f;
+			float lpos_y = 0.0f;
+			map_projection_project(&_pos_ref, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y);
+			gps_sample_new.pos(0) = lpos_x;
+			gps_sample_new.pos(1) = lpos_y;
+
+		} else {
+			gps_sample_new.pos(0) = 0.0f;
+			gps_sample_new.pos(1) = 0.0f;
+		}
 
 		_gps_buffer.push(gps_sample_new);
 	}
@@ -291,11 +294,34 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	      _airspeed_buffer.allocate(OBS_BUFFER_LENGTH) &&
 	      _flow_buffer.allocate(OBS_BUFFER_LENGTH) &&
 	      _output_buffer.allocate(IMU_BUFFER_LENGTH))) {
-		printf("Estimator Buffer Allocation failed!");
+		printf("EKF buffer allocation failed!");
 		unallocate_buffers();
 		return false;
 	}
 
+	// zero the data in the observation buffers
+	for (int index=0; index < OBS_BUFFER_LENGTH; index++) {
+		gpsSample gps_sample_init = {};
+		_gps_buffer.push(gps_sample_init);
+		magSample mag_sample_init = {};
+		_mag_buffer.push(mag_sample_init);
+		baroSample baro_sample_init = {};
+		_baro_buffer.push(baro_sample_init);
+		rangeSample range_sample_init = {};
+		_range_buffer.push(range_sample_init);
+		airspeedSample airspeed_sample_init = {};
+		_airspeed_buffer.push(airspeed_sample_init);
+		flowSample flow_sample_init = {};
+		_flow_buffer.push(flow_sample_init);
+	}
+
+	// zero the data in the imu data and output observer state buffers
+	for (int index=0; index < IMU_BUFFER_LENGTH; index++) {
+		imuSample imu_sample_init = {};
+		_imu_buffer.push(imu_sample_init);
+		outputSample output_sample_init = {};
+		_output_buffer.push(output_sample_init);
+	}
 
 	_dt_imu_avg = 0.0f;
 

EKF/estimator_interface.h

@@ -222,8 +222,6 @@ protected:
 
 	bool _NED_origin_initialised = false;
 	bool _gps_speed_valid = false;
-	float _gps_speed_accuracy = 0.0f; // GPS receiver reported 1-sigma speed accuracy (m/s)
-	float _gps_hpos_accuracy = 0.0f; // GPS receiver reported 1-sigma horizontal accuracy (m)
 	float _gps_origin_eph = 0.0f; // horizontal position uncertainty of the GPS origin
 	float _gps_origin_epv = 0.0f; // vertical position uncertainty of the GPS origin
 	struct map_projection_reference_s _pos_ref = {};    // Contains WGS-84 position latitude and longitude (radians)
@@ -262,4 +260,11 @@ protected:
 
 	float _mag_declination_gps;         // magnetic declination returned by the geo library using the last valid GPS position (rad)
 	float _mag_declination_to_save_deg; // magnetic declination to save to EKF2_MAG_DECL (deg)
+
+	// this is the current status of the filter control modes
+	filter_control_status_u _control_status;
+
+	// this is the previous status of the filter control modes - used to detect mode transitions
+	filter_control_status_u _control_status_prev;
+
 };

EKF/gps_checks.cpp

@@ -59,7 +59,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
 	if (!_NED_origin_initialised) {
 		// we have good GPS data so can now set the origin's WGS-84 position
 		if (gps_is_good(gps) && !_NED_origin_initialised) {
-			printf("gps is good -  setting EKF origin\n");
+			printf("EKF gps is good - setting origin\n");
 			// Set the origin's WGS-84 position to the last gps fix
 			double lat = gps->lat / 1.0e7;
 			double lon = gps->lon / 1.0e7;
@@ -81,6 +81,16 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
 			// 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) {
+				printf("EKF switching to GPS height\n");
+				_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;
+			}
 		}
 	}
 
@@ -133,7 +143,7 @@ bool Ekf::gps_is_good(struct gps_message *gps)
 
 	} else {
 		map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
-		_gps_alt_ref = gps->alt * 1e-3f;
+		_gps_alt_ref = 1e-3f * (float)gps->alt;
 	}
 
 	// Calculate time lapsed since last update, limit to prevent numerical errors and calculate the lowpass filter coefficient
@@ -166,10 +176,10 @@ bool Ekf::gps_is_good(struct gps_message *gps)
 
 	// Calculate the vertical drift velocity and limit to 10x the threshold
 	vel_limit = 10.0f * _params.req_vdrift;
-	float velD = fminf(fmaxf((_gps_alt_ref - gps->alt * 1e-3f) / dt, -vel_limit), vel_limit);
+	float velD = fminf(fmaxf((_gps_alt_ref - 1e-3f * (float)gps->alt) / dt, -vel_limit), vel_limit);
 
 	// Save the current height as the reference for next time
-	_gps_alt_ref = gps->alt * 1e-3f;
+	_gps_alt_ref = 1e-3f * (float)gps->alt;
 
 	// Apply a low pass filter to the vertical velocity
 	_gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef);

EKF/mag_fusion.cpp

@@ -1007,7 +1007,6 @@ void Ekf::fuseMag2D()
 		// we allow to use it when on the ground because the large innovation could be caused
 		// by interference or a large initial gyro bias
 		if (_control_status.flags.in_air) {
-			printf("return 5\n");
 			return;
 
 		} else {

EKF/vel_pos_fusion.cpp

@@ -60,7 +60,7 @@ void Ekf::fuseVelPosHeight()
 		_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
 		// observation variance - use receiver reported accuracy with parameter setting the minimum value
 		R[0] = fmaxf(_params.gps_vel_noise, 0.01f);
-		R[0] = fmaxf(R[0], _gps_speed_accuracy);
+		R[0] = fmaxf(R[0], _gps_sample_delayed.sacc);
 		R[0] = R[0] * R[0];
 		R[1] = R[0];
 		// innovation gate sizes
@@ -75,7 +75,7 @@ void Ekf::fuseVelPosHeight()
 		// observation variance - use receiver reported accuracy with parameter setting the minimum value
 		R[2] = fmaxf(_params.gps_vel_noise, 0.01f);
 		// use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP
-		R[2] = 1.5f * fmaxf(R[2], _gps_speed_accuracy);
+		R[2] = 1.5f * fmaxf(R[2], _gps_sample_delayed.sacc);
 		R[2] = R[2] * R[2];
 		// innovation gate size
 		gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f);
@@ -89,13 +89,18 @@ void Ekf::fuseVelPosHeight()
 
 		// observation variance - user parameter defined
 		// if we are in flight and not using GPS, then use a specific parameter
-		if (!_control_status.flags.gps && _control_status.flags.in_air) {
-			R[3] = fmaxf(_params.pos_noaid_noise, 0.5f);
+		if (!_control_status.flags.gps) {
+			if (_control_status.flags.in_air) {
+				R[3] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
+
+			} else {
+				R[3] = _params.gps_pos_noise;
+			}
 
 		} else {
 			float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
 			float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
-			R[3] = math::constrain(_gps_hpos_accuracy, lower_limit, upper_limit);
+			R[3] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
 
 		}
 
@@ -110,13 +115,26 @@ void Ekf::fuseVelPosHeight()
 		if (_control_status.flags.baro_hgt) {
 			fuse_map[5] = true;
 			// vertical position innovation - baro measurement has opposite sign to earth z axis
-			_vel_pos_innov[5] = _state.pos(2) - (_hgt_at_alignment - _baro_sample_delayed.hgt + _baro_hgt_offset);
+			_vel_pos_innov[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;
 			// observation variance - user parameter defined
 			R[5] = fmaxf(_params.baro_noise, 0.01f);
 			R[5] = R[5] * R[5];
 			// innovation gate size
 			gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
 
+		} else if (_control_status.flags.gps_hgt) {
+			fuse_map[5] = true;
+			// vertical position innovation - gps measurement has opposite sign to earth z axis
+			_vel_pos_innov[5] = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
+			// observation variance - receiver defined and parameter limited
+			// use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
+			float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
+			float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
+			R[5] = 1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit);
+			R[5] = R[5] * R[5];
+			// innovation gate size
+			gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
+
 		} else if (_control_status.flags.rng_hgt && (_R_prev(2, 2) > 0.7071f)) {
 			fuse_map[5] = true;
 			// use range finder with tilt correction
@@ -128,6 +146,7 @@ void Ekf::fuseVelPosHeight()
 			// innovation gate size
 			gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f);
 		}
+
 	}
 
 	// calculate innovation test ratios
@@ -244,5 +263,4 @@ void Ekf::fuseVelPosHeight()
 		makeSymmetrical();
 		limitCov();
 	}
-
 }