EKF: Add position, height and velocity reset for EV aiding

EKF/common.h

@@ -145,6 +145,7 @@ struct extVisionSample {
 #define VDIST_SENSOR_BARO  0	// Use baro height
 #define VDIST_SENSOR_GPS   1	// Use GPS height
 #define VDIST_SENSOR_RANGE 2	// Use range finder height
+#define VDIST_SENSOR_EV    3    // USe external vision
 
 // Bit locations for mag_declination_source
 #define MASK_USE_GEO_DECL   (1<<0)  // set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
@@ -167,6 +168,7 @@ struct extVisionSample {
 #define GPS_MAX_INTERVAL	5e5
 #define BARO_MAX_INTERVAL	2e5
 #define RNG_MAX_INTERVAL	2e5
+#define EV_MAX_INTERVAL		2e5
 
 struct parameters {
 	// measurement source control

EKF/control.cpp

@@ -56,6 +56,25 @@ void Ekf::controlFusionModes()
 		_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
 	}
 
+	// external vision position aiding selection logic
+	if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) {
+		// check for a exernal vision measurement that has fallen behind the fusion time horizon
+		if (_ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed)) {
+			// turn on use of external vision measurements for position
+			_control_status.flags.ev_pos = true;
+			// reset the position, height and velocity
+			resetPosition();
+			resetVelocity();
+			resetHeight();
+		}
+	}
+
+	// external vision yaw aiding selection logic
+	if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align
+			&& (_time_last_imu - _time_last_ext_vision) < 5e5) {
+		//TODO
+	}
+
 	// optical flow fusion mode selection logic
 	// to start using optical flow data we need angular alignment complete, and fresh optical flow and height above terrain data
 	if ((_params.fusion_mode & MASK_USE_OF) && !_control_status.flags.opt_flow && _control_status.flags.tilt_align

EKF/ekf.cpp

@@ -390,7 +390,11 @@ bool Ekf::initialiseFilter(void)
 
 	// set the default height source from the adjustable parameter
 	if (_hgt_counter == 0) {
-		_primary_hgt_source = _params.vdist_sensor_type;
+		if (_params.fusion_mode & MASK_USE_EVPOS) {
+			_primary_hgt_source = VDIST_SENSOR_EV;
+		} else {
+			_primary_hgt_source = _params.vdist_sensor_type;
+		}
 	}
 
 	if (_primary_hgt_source == VDIST_SENSOR_RANGE) {
@@ -409,7 +413,7 @@ bool Ekf::initialiseFilter(void)
 			}
 		}
 
-	} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) {
+	} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS || _primary_hgt_source == VDIST_SENSOR_EV) {
 		// if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS
 		// later when it passes checks.
 		if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {

EKF/ekf_helper.cpp

@@ -51,15 +51,22 @@
 // gps measurement then use for velocity initialisation
 bool Ekf::resetVelocity()
 {
-	// if we have a valid GPS measurement use it to initialise velocity states
-	gpsSample gps_newest = _gps_buffer.get_newest();
+	if (_control_status.flags.gps) {
+		// if we have a valid GPS measurement use it to initialise velocity states
+		gpsSample gps_newest = _gps_buffer.get_newest();
 
-	if (_time_last_imu - gps_newest.time_us < 400000) {
-		_state.vel = gps_newest.vel;
-		return true;
+		if (_time_last_imu - gps_newest.time_us < 2*GPS_MAX_INTERVAL) {
+			_state.vel = gps_newest.vel;
+			return true;
 
+		} else {
+			// XXX use the value of the last known velocity
+			return false;
+		}
+	} else if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) {
+		_state.vel.setZero();
+		return true;
 	} else {
-		// XXX use the value of the last known velocity
 		return false;
 	}
 }
@@ -68,18 +75,48 @@ bool Ekf::resetVelocity()
 // gps measurement then use for position initialisation
 bool Ekf::resetPosition()
 {
-	// if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay
-	gpsSample gps_newest = _gps_buffer.get_newest();
+	if (_control_status.flags.gps) {
+		// if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay
+		gpsSample gps_newest = _gps_buffer.get_newest();
 
-	float time_delay = 1e-6f * (float)(_time_last_imu - gps_newest.time_us);
+		float time_delay = 1e-6f * (float)(_imu_sample_delayed.time_us - gps_newest.time_us);
+		float max_time_delay = 1e-6f * (float)GPS_MAX_INTERVAL;
 
-	if (time_delay < 0.4f) {
-		_state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay;
-		_state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay;
+		if (time_delay < max_time_delay) {
+			_state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay;
+			_state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay;
+			return true;
+
+		} else {
+			// XXX use the value of the last known position
+			return false;
+		}
+	} else if (_control_status.flags.opt_flow) {
+		_state.pos(0) = 0.0f;
+		_state.pos(1) = 0.0f;
 		return true;
+	} else if (_control_status.flags.ev_pos) {
+		// if we have fresh data, reset the position to the measurement
+		extVisionSample ev_newest = _ext_vision_buffer.get_newest();
+		if (_time_last_imu - ev_newest.time_us < 2*EV_MAX_INTERVAL) {
+			// use the most recent data if it's time offset from the fusion time horizon is smaller
+			int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
+			int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
+			if (abs(dt_newest) < abs(dt_delayed)) {
+				_state.pos(0) = ev_newest.posNED(0);
+				_state.pos(1) = ev_newest.posNED(1);
+			} else {
+				_state.pos(0) = _ev_sample_delayed.posNED(0);
+				_state.pos(1) = _ev_sample_delayed.posNED(1);
+			}
+			return true;
+
+		} else {
+			// XXX use the value of the last known position
+			return false;
+		}
 
 	} else {
-		// XXX use the value of the last known position
 		return false;
 	}
 }
@@ -116,13 +153,14 @@ void Ekf::resetHeight()
 
 			vert_pos_reset = true;
 
+			// 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 {
 			// TODO: reset to last known range 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);
 
 	} else if (_control_status.flags.baro_hgt) {
 		// initialize vertical position with newest baro measurement
@@ -158,13 +196,24 @@ void Ekf::resetHeight()
 
 			vert_pos_reset = true;
 
+			// 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 {
 			// 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);
+	} else if (_control_status.flags.ev_pos) {
+		// initialize vertical position with newest measurement
+		extVisionSample ev_newest = _ext_vision_buffer.get_newest();
+
+		if (_time_last_imu - ev_newest.time_us < 2 * EV_MAX_INTERVAL) {
+			_state.pos(2) = ev_newest.posNED(2);
+
+		} else {
+			// TODO: reset to last known baro based estimate
+		}
 	}
 
 	// reset the vertical velocity covariance values

EKF/estimator_interface.h

@@ -248,6 +248,7 @@ protected:
 	rangeSample _range_sample_delayed;
 	airspeedSample _airspeed_sample_delayed;
 	flowSample _flow_sample_delayed;
+	extVisionSample _ev_sample_delayed;
 
 	outputSample _output_sample_delayed;	// filter output on the delayed time horizon
 	outputSample _output_new;	// filter output on the non-delayed time horizon