Merge pull request #311 from PX4/pr-ekfExtVis

EKF: Add ability to use EV and GPS data together

EKF/control.cpp

@@ -115,7 +115,6 @@ void Ekf::controlFusionModes()
 
 	// control use of observations for aiding
 	controlMagFusion();
-	controlExternalVisionFusion();
 	controlOpticalFlowFusion();
 	controlGpsFusion();
 	controlAirDataFusion();
@@ -123,10 +122,13 @@ void Ekf::controlFusionModes()
 	controlDragFusion();
 	controlHeightFusion();
 
-	// for efficiency, fusion of direct state observations for position and velocity is performed sequentially
-	// in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc)
+	// For efficiency, fusion of direct state observations for position and velocity is performed sequentially
+	// in a single function using sensor data from multiple sources (GPS, baro, range finder, etc)
 	controlVelPosFusion();
 
+	// Additional data from an external vision sensor can also be fused.
+	controlExternalVisionFusion();
+
 	// report dead reckoning if we are no longer fusing measurements that constrain velocity drift
 	_is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > _params.no_aid_timeout_max)
 			&& (_time_last_imu - _time_last_vel_fuse > _params.no_aid_timeout_max)
@@ -143,20 +145,27 @@ void Ekf::controlExternalVisionFusion()
 		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 (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
-				// turn on use of external vision measurements for position and height
-				setControlEVHeight();
+				// turn on use of external vision measurements for position
+				_control_status.flags.ev_pos = true;
 				ECL_INFO("EKF commencing external vision position fusion");
-				// reset the position, height and velocity
-				resetPosition();
-				resetVelocity();
-				resetHeight();
-				_control_status.flags.ev_pos=true;
+
+				// reset the position if we are not already aiding using GPS, else use a relative position
+				// method for fusing the position data
+				if (_control_status.flags.gps) {
+					_hpos_odometry = true;
+
+				} else {
+					resetPosition();
+					resetVelocity();
+					_hpos_odometry = false;
+
+				}
 			}
 		}
 
 		// external vision yaw aiding selection logic
 		if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
-			// check for a exernal vision measurement that has fallen behind the fusion time horizon
+			// 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
 				// get the roll, pitch, yaw estimates from the quaternion states
@@ -207,11 +216,18 @@ void Ekf::controlExternalVisionFusion()
 			}
 		}
 
-		// determine if we should use the height observation
+		// determine if we should start using the height observations
 		if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
-			setControlEVHeight();
-			_fuse_height = true;
+			// don't start using EV data unless daa is arriving frequently
+			if (!_control_status.flags.ev_hgt && (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL)) {
+				setControlEVHeight();
+				resetHeight();
+			}
+		}
 
+		// determine if we should use the vertical position observation
+		if (_control_status.flags.ev_hgt) {
+			_fuse_height = true;
 		}
 
 		// determine if we should use the horizontal position observations
@@ -224,11 +240,24 @@ void Ekf::controlExternalVisionFusion()
 			_ev_sample_delayed.posNED(0) -= pos_offset_earth(0);
 			_ev_sample_delayed.posNED(1) -= pos_offset_earth(1);
 			_ev_sample_delayed.posNED(2) -= pos_offset_earth(2);
+
+			// if GPS data is being used, then use an incremental position fusion method for EV data
+			if (_control_status.flags.gps) {
+				_hpos_odometry = true;
+			}
+		}
+
+		// Fuse available NED position data into the main filter
+		if (_fuse_height || _fuse_pos) {
+			fuseVelPosHeight();
+			_fuse_pos = _fuse_height = false;
+
 		}
 
 		// determine if we should use the yaw observation
 		if (_control_status.flags.ev_yaw) {
 			fuseHeading();
+
 		}
 	}
 
@@ -245,7 +274,6 @@ void Ekf::controlExternalVisionFusion()
 
 		}
 	}
-
 }
 
 void Ekf::controlOpticalFlowFusion()

EKF/ekf.h

@@ -238,6 +238,12 @@ private:
 	bool _fuse_hor_vel{false};	///< true when gps horizontal velocity measurement should be fused
 	bool _fuse_vert_vel{false};	///< true when gps vertical velocity measurement should be fused
 
+	// variables used when position data is being fused using a relative position odometry model
+	bool _hpos_odometry{false};		///< true when the NE position data is being fused using an odometry assumption
+	Vector2f _hpos_meas_prev;		///< previous value of NE position measurement fused using odometry assumption (m)
+	Vector2f _hpos_pred_prev;		///< previous value of NE position state used by odometry fusion (m)
+	bool _hpos_prev_available{false};	///< true when previous values of the estimate and measurement are available for use
+
 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
 	bool _mag_data_ready{false};	///< true when new magnetometer data has fallen behind the fusion time horizon and is available to be fused

EKF/ekf_helper.cpp

@@ -98,6 +98,9 @@ bool Ekf::resetPosition()
 	posNE_before_reset(0) = _state.pos(0);
 	posNE_before_reset(1) = _state.pos(1);
 
+	// let the next odometry update know that the previous value of states cannot be used to calculate the change in position
+	_hpos_prev_available = false;
+
 	if (_control_status.flags.gps) {
 		// this reset is only called if we have new gps data at the fusion time horizon
 		_state.pos(0) = _gps_sample_delayed.pos(0);

EKF/gps_checks.cpp

@@ -68,7 +68,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
 			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) {
+			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);

EKF/vel_pos_fusion.cpp

@@ -99,10 +99,34 @@ void Ekf::fuseVelPosHeight()
 
 
 		} else if (_control_status.flags.ev_pos) {
-			// we are using external vision measurements
+			// calculate innovations
+			if(_hpos_odometry) {
+				if(!_hpos_prev_available) {
+					// no previous observation available to calculate position change
+					fuse_map[3] = fuse_map[4] = false;
+					_hpos_prev_available = true;
+
+				} else {
+					// use the change in position since the last measurement
+					_vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - _ev_sample_delayed.posNED(0) + _hpos_meas_prev(0);
+					_vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - _ev_sample_delayed.posNED(1) + _hpos_meas_prev(1);
+
+				}
+
+				// record observation and estimate for use next time
+				_hpos_meas_prev(0) = _ev_sample_delayed.posNED(0);
+				_hpos_meas_prev(1) = _ev_sample_delayed.posNED(1);
+				_hpos_pred_prev(0) = _state.pos(0);
+				_hpos_pred_prev(1) = _state.pos(1);
+
+			} else {
+				// use the absolute position
+				_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
+				_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
+			}
+
+			// observation 1-STD error
 			R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
-			_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
-			_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
 
 			// innovation gate size
 			gate_size[3] = fmaxf(_params.ev_innov_gate, 1.0f);