Merge pull request #76 from PX4/pr-fixRngHgtMode

EKF: Fix bugs preventing use of range finder as primary height source

Flight testing completed and I have checked that the other pending PR #75 rebases cleanly on it.

EKF/control.cpp

@@ -125,6 +125,7 @@ void Ekf::controlFusionModes()
 			if (_control_status.flags.yaw_align) {
 				_control_status.flags.gps = true;
 				_time_last_gps = _time_last_imu;
+
 				// if we are not already aiding with optical flow, then we need to reset the position and velocity
 				if (!_control_status.flags.opt_flow) {
 					_control_status.flags.gps = resetPosition();
@@ -159,8 +160,8 @@ 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 provided the data is fresh
-	if ((_time_last_imu - _time_last_hgt_fuse > 10e6) && (_time_last_imu - _time_last_baro < 5e5)) {
+	// After 10 seconds without aiding we reset to the height measurement
+	if (_time_last_imu - _time_last_hgt_fuse > 10e6) {
 		// Reset vertical position and velocity states to the last measurement
 		resetHeight();
 	}
@@ -247,10 +248,22 @@ void Ekf::controlFusionModes()
 	}
 
 	// Control the soure of height measurements for the main filter
-	_control_status.flags.baro_hgt = true;
-	_control_status.flags.rng_hgt = false;
-	_control_status.flags.gps_hgt = false;
+	if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
+		_control_status.flags.baro_hgt = true;
+		_control_status.flags.rng_hgt = false;
+		_control_status.flags.gps_hgt = false;
+
+	} else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
+		_control_status.flags.baro_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
 	// TODO add methods for true airspeed and/or sidelsip fusion or some type of drag force measurement

EKF/ekf.cpp

@@ -40,6 +40,7 @@
  */
 
 #include "ekf.h"
+#include "mathlib.h"
 
 Ekf::Ekf():
 	_filter_initialised(false),
@@ -70,13 +71,16 @@ Ekf::Ekf():
 	_last_gps_origin_time_us(0),
 	_gps_alt_ref(0.0f),
 	_hgt_counter(0),
+	_time_last_hgt(0),
 	_hgt_sum(0.0f),
 	_mag_counter(0),
+	_time_last_mag(0),
 	_hgt_at_alignment(0.0f),
 	_terrain_vpos(0.0f),
 	_hagl_innov(0.0f),
 	_hagl_innov_var(0.0f),
-	_time_last_hagl_fuse(0)
+	_time_last_hagl_fuse(0),
+	_baro_hgt_offset(0.0f)
 {
 	_control_status = {};
 	_control_status_prev = {};
@@ -99,7 +103,7 @@ Ekf::Ekf():
 	_mag_sum = {};
 	_delVel_sum = {};
 	_flow_gyro_bias = {};
-	_imu_del_ang_of = {};	
+	_imu_del_ang_of = {};
 }
 
 Ekf::~Ekf()
@@ -156,8 +160,6 @@ bool Ekf::init(uint64_t timestamp)
 bool Ekf::update()
 {
 
-	// Only run the filter if IMU data in the buffer has been updated
-	if (_imu_updated) {
 		if (!_filter_initialised) {
 			_filter_initialised = initialiseFilter();
 
@@ -166,6 +168,9 @@ bool Ekf::update()
 			}
 		}
 
+	// Only run the filter if IMU data in the buffer has been updated
+	if (_imu_updated) {
+
 		// perform state and covariance prediction for the main filter
 		predictState();
 		predictCovariance();
@@ -207,21 +212,43 @@ bool Ekf::update()
 		// determine if range finder data has fallen behind the fusin time horizon fuse it if we are
 		// not tilted too much to use it
 		if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
-			   && (_R_prev(2, 2) > 0.7071f)) {
+		    && (_R_prev(2, 2) > 0.7071f)) {
 			// if we have range data we always try to estimate terrain height
 			_fuse_hagl_data = true;
 
 			// only use range finder as a height observation in the main filter if specifically enabled
-			if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
+			if (_control_status.flags.rng_hgt) {
 				_fuse_height = true;
 			}
 
+		} else if ((_time_last_imu - _time_last_hgt_fuse) > 5e5 && _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
-		if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)
-			   && _params.vdist_sensor_type == VDIST_SENSOR_BARO) {
-			_fuse_height = true;
+		if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
+			if (_control_status.flags.baro_hgt) {
+				_fuse_height = true;
+
+			} 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;
+				_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
@@ -233,15 +260,15 @@ bool Ekf::update()
 
 		// If we are using optical flow aiding and data has fallen behind the fusion time horizon, then fuse it
 		if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
-			   && _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5
-			   && (_R_prev(2, 2) > 0.7071f)) {
+		    && _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5
+		    && (_R_prev(2, 2) > 0.7071f)) {
 			_fuse_flow = true;
 		}
 
 		// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
 		// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
 		if (!_control_status.flags.gps && !_control_status.flags.opt_flow
-			   && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
+		    && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
 			_fuse_pos = true;
 			_gps_sample_delayed.pos(0) = _last_known_posNE(0);
 			_gps_sample_delayed.pos(1) = _last_known_posNE(1);
@@ -303,22 +330,27 @@ bool Ekf::initialiseFilter(void)
 	if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
 		rangeSample range_init = _range_buffer.get_newest();
 
-		if (range_init.time_us != 0) {
+		if (range_init.time_us != _time_last_hgt) {
 			_hgt_counter ++;
 			_hgt_sum += range_init.rng;
+			_time_last_hgt = range_init.time_us;
 		}
 
-	} else {
+	} else if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
 		// initialize vertical position with newest baro measurement
 		baroSample baro_init = _baro_buffer.get_newest();
 
-		if (baro_init.time_us != 0) {
+		if (baro_init.time_us != _time_last_hgt) {
 			_hgt_counter ++;
 			_hgt_sum += baro_init.hgt;
+			_time_last_hgt = baro_init.time_us;
 		}
+
+	} else {
+		return false;
 	}
 
-	// check to see if we have enough measruements and return false if not
+	// check to see if we have enough measurements and return false if not
 	if (_hgt_counter < 10 || _mag_counter < 10) {
 		return false;
 
@@ -361,9 +393,14 @@ bool Ekf::initialiseFilter(void)
 		// calculate the initial magnetic field and yaw alignment
 		resetMagHeading(mag_init);
 
-		// calculate the averaged barometer reading
+		// calculate the averaged height reading to calulate the height of the origin
 		_hgt_at_alignment = _hgt_sum / (float)_hgt_counter;
 
+		// 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
+		baroSample baro_newest = _baro_buffer.get_newest();
+		_baro_hgt_offset = baro_newest.hgt;
+
 		// set the velocity to the GPS measurement (by definition, the initial position and height is at the origin)
 		resetVelocity();
 

EKF/ekf.h

@@ -197,9 +197,11 @@ private:
 	float _gps_alt_ref;		// WGS-84 height (m)
 
 	// Variables used to initialise the filter states
-	uint8_t _hgt_counter;		// number of baro samples averaged
-	float _hgt_sum;			// summed baro measurement
+	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
+	uint64_t _time_last_mag;	// measurement time of last magnetomter sample
 	Vector3f _mag_sum;		// summed magnetometer measurement
 	Vector3f _delVel_sum;		// summed delta velocity
 	float _hgt_at_alignment;	// baro offset relative to alignment position
@@ -214,6 +216,8 @@ 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
 
+	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
 	// and the correction step
 	void calculateOutputStates();

EKF/ekf_helper.cpp

@@ -87,7 +87,7 @@ bool Ekf::resetPosition()
 // Reset height state using the last height measurement
 void Ekf::resetHeight()
 {
-	if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
+	if (_control_status.flags.rng_hgt) {
 		rangeSample range_newest = _range_buffer.get_newest();
 
 		if (_time_last_imu - range_newest.time_us < 200000) {
@@ -96,7 +96,12 @@ void Ekf::resetHeight()
 		} else {
 			// TODO: reset to last known range based estimate
 		}
-	} else {
+
+		// 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);
+
+	} else if (_control_status.flags.baro_hgt) {
 		// initialize vertical position with newest baro measurement
 		baroSample baro_newest = _baro_buffer.get_newest();
 
@@ -106,6 +111,12 @@ void Ekf::resetHeight()
 		} 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 {
+		// TODO: reset to GPS height
 	}
 
 }

EKF/gps_checks.cpp

@@ -56,7 +56,7 @@
 bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
 {
 	// If we have defined the WGS-84 position of the NED origin, run gps quality checks until they pass, then define the origins WGS-84 position using the last GPS fix
-	if (!_NED_origin_initialised ) {
+	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");
@@ -64,12 +64,14 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
 			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) {
 				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;

EKF/terrain_estimator.cpp

@@ -78,7 +78,8 @@ void Ekf::predictHagl()
 	_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);
 
 	// process noise due to terrain gradient
-	_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(1)));
+	_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(
+				1)));
 
 	// limit the variance to prevent it becoming badly conditioned
 	_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);

EKF/vel_pos_fusion.cpp

@@ -110,7 +110,7 @@ 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);
+			_vel_pos_innov[5] = _state.pos(2) - (_hgt_at_alignment - _baro_sample_delayed.hgt + _baro_hgt_offset);
 			// observation variance - user parameter defined
 			R[5] = fmaxf(_params.baro_noise, 0.01f);
 			R[5] = R[5] * R[5];
@@ -120,7 +120,7 @@ void Ekf::fuseVelPosHeight()
 		} else if (_control_status.flags.rng_hgt && (_R_prev(2, 2) > 0.7071f)) {
 			fuse_map[5] = true;
 			// use range finder with tilt correction
-			_vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng *_R_prev(2, 2),
+			_vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_prev(2, 2),
 							     _params.rng_gnd_clearance));
 			// observation variance - user parameter defined
 			R[5] = fmaxf(_params.range_noise, 0.01f);