Refactor position resets (#822)

EKF/control.cpp

@@ -201,7 +201,7 @@ void Ekf::controlExternalVisionFusion()
 				// turn on use of external vision measurements for position
 				if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) {
 					_control_status.flags.ev_pos = true;
-					resetPosition();
+					resetHorizontalPosition();
 					ECL_INFO_TIMESTAMPED("starting vision pos fusion");
 				}
 
@@ -308,7 +308,7 @@ void Ekf::controlExternalVisionFusion()
 						resetVelocity();
 					}
 
-					resetPosition();
+					resetHorizontalPosition();
 				}
 			}
 
@@ -457,7 +457,7 @@ void Ekf::controlOpticalFlowFusion()
 					// we will use _flow_compensated_XY_rad in the resetVelocity() function,
 					// but _flow_compensated_XY_rad is this zero as it gets updated for the first time below here.
 					resetVelocity();
-					resetPosition();
+					resetHorizontalPosition();
 
 					// align the output observer to the EKF states
 					alignOutputFilter();
@@ -475,7 +475,7 @@ void Ekf::controlOpticalFlowFusion()
 
 			if (do_reset) {
 				resetVelocity();
-				resetPosition();
+				resetHorizontalPosition();
 			}
 		}
 
@@ -577,12 +577,12 @@ void Ekf::controlGpsFusion()
 					_control_status.flags.gps = true;
 
 					if (!_control_status.flags.opt_flow) {
-						if (!resetPosition() || !resetVelocity()) {
+						if (!resetHorizontalPosition() || !resetVelocity()) {
 							_control_status.flags.gps = false;
 
 						}
 
-					} else if (!resetPosition()) {
+					} else if (!resetHorizontalPosition()) {
 						_control_status.flags.gps = false;
 
 					}
@@ -604,7 +604,7 @@ void Ekf::controlGpsFusion()
 			stopGpsFusion();
 			// Reset position state to external vision if we are going to use absolute values
 			if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) {
-				resetPosition();
+				resetHorizontalPosition();
 			}
 			ECL_WARN_TIMESTAMPED("GPS quality poor - stopping use");
 		}
@@ -669,7 +669,7 @@ void Ekf::controlGpsFusion()
 				}
 
 				resetVelocity();
-				resetPosition();
+				resetHorizontalPosition();
 				_velpos_reset_request = false;
 				ECL_WARN_TIMESTAMPED("GPS fusion timeout - reset to GPS");
 
@@ -1264,7 +1264,7 @@ void Ekf::controlFakePosFusion()
 
 			// Reset position and velocity states if we re-commence this aiding method
 			if (isTimedOut(_time_last_fake_pos, (uint64_t)4e5)) {
-				resetPosition();
+				resetHorizontalPosition();
 				resetVelocity();
 				_fuse_hpos_as_odom = false;
 

EKF/ekf.h

@@ -588,8 +588,16 @@ private:
 
 	inline void resetVerticalVelocityTo(float new_vert_vel);
 
+	bool resetHorizontalPosition();
+
+	inline void resetHorizontalPositionToGps();
+
+	inline void resetHorizontalPositionToVision();
+
 	inline void resetHorizontalPositionTo(const Vector2f &new_horz_pos);
 
+	void resetHeight();
+
 	// fuse optical flow line of sight rate measurements
 	void fuseOptFlow();
 
@@ -633,12 +641,6 @@ private:
 	// Return the magnetic declination in radians to be used by the alignment and fusion processing
 	float getMagDeclination();
 
-	// reset position states of the ekf (only horizontal position)
-	bool resetPosition();
-
-	// reset height state of the ekf
-	void resetHeight();
-
 	// modify output filter to match the the EKF state at the fusion time horizon
 	void alignOutputFilter();
 

EKF/ekf_helper.cpp

@@ -62,7 +62,6 @@ bool Ekf::resetVelocity()
 	} else {
 		resetHorizontalVelocityToZero();
 	}
-
 	return true;
 }
 
@@ -153,30 +152,18 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel) {
 
 // Reset position states. If we have a recent and valid
 // gps measurement then use for position initialisation
-bool Ekf::resetPosition()
+bool Ekf::resetHorizontalPosition()
 {
 	// 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) {
-		ECL_INFO_TIMESTAMPED("reset position to GPS");
-
 		// this reset is only called if we have new gps data at the fusion time horizon
-		resetHorizontalPositionTo(Vector2f(_gps_sample_delayed.pos));
+		resetHorizontalPositionToGps();
-
-		// use GPS accuracy to reset variances
-		P.uncorrelateCovarianceSetVariance<2>(7, sq(_gps_sample_delayed.hacc));
 
 	} else if (_control_status.flags.ev_pos) {
-		ECL_INFO_TIMESTAMPED("reset position to ev position");
-
 		// this reset is only called if we have new ev data at the fusion time horizon
-		Vector3f _ev_pos = _ev_sample_delayed.pos;
+		resetHorizontalPositionToVision();
-		if(_params.fusion_mode & MASK_ROTATE_EV){
-			_ev_pos = _R_ev_to_ekf *_ev_sample_delayed.pos;
-		}
-		resetHorizontalPositionTo(Vector2f(_ev_pos));
-		P.uncorrelateCovarianceSetVariance<2>(7, _ev_sample_delayed.posVar.slice<2, 1>(0, 0));
 
 	} else if (_control_status.flags.opt_flow) {
 		ECL_INFO_TIMESTAMPED("reset position to last known position");
@@ -201,6 +188,22 @@ bool Ekf::resetPosition()
 	return true;
 }
 
+void Ekf::resetHorizontalPositionToGps() {
+	ECL_INFO_TIMESTAMPED("reset position to GPS");
+	resetHorizontalPositionTo(_gps_sample_delayed.pos);
+	P.uncorrelateCovarianceSetVariance<2>(7, sq(_gps_sample_delayed.hacc));
+}
+
+void Ekf::resetHorizontalPositionToVision() {
+	ECL_INFO_TIMESTAMPED("reset position to ev position");
+	Vector3f _ev_pos = _ev_sample_delayed.pos;
+	if(_params.fusion_mode & MASK_ROTATE_EV){
+		_ev_pos = _R_ev_to_ekf *_ev_sample_delayed.pos;
+	}
+	resetHorizontalPositionTo(Vector2f(_ev_pos));
+	P.uncorrelateCovarianceSetVariance<2>(7, _ev_sample_delayed.posVar.slice<2, 1>(0, 0));
+}
+
 void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos) {
 	const Vector2f delta_horz_pos = new_horz_pos - Vector2f(_state.pos);
 	_state.pos.xy() = new_horz_pos;
@@ -1790,7 +1793,7 @@ bool Ekf::resetYawToEKFGSF()
 
 		// reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
 		resetVelocity();
-		resetPosition();
+		resetHorizontalPosition();
 
 		// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
 		_flt_mag_align_start_time = _imu_sample_delayed.time_us;

EKF/mag_control.cpp

@@ -184,7 +184,7 @@ void Ekf::runVelPosReset()
 {
 	if (_velpos_reset_request) {
 		resetVelocity();
-		resetPosition();
+		resetHorizontalPosition();
 		_velpos_reset_request = false;
 	}
 }