EKF: Fix reversion from GPS to no-aiding mode (PX4#412)

* EKF: Do not delay reversion to no-aiding mode if parameter initiated * EKF: Move no-aid reversion resets to helper functions * EKF: Prevent unwanted fusion of velocity data during no aiding mode
TSC21 · Mar 22, 2018 · 8a01243 · 8a01243
1 parent 95c4777
commit 8a01243
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 5 deletions.
diff --git a/EKF/control.cpp b/EKF/control.cpp
@@ -1339,6 +1339,9 @@ void Ekf::controlVelPosFusion()
 {
 	// 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 (!(_params.fusion_mode & MASK_USE_GPS)) {
+		_control_status.flags.gps = false;
+	}
 	if (!_control_status.flags.gps &&
 			!_control_status.flags.opt_flow &&
 			!_control_status.flags.ev_pos &&
@@ -1347,9 +1350,8 @@ void Ekf::controlVelPosFusion()
 	{
 		// Reset position and velocity states if we re-commence this aiding method
 		if ((_time_last_imu - _time_last_fake_gps) > (uint64_t)4e5) {
-			_last_known_posNE(0) = _state.pos(0);
-			_last_known_posNE(1) = _state.pos(1);
-			_state.vel.setZero();
+			resetPosition();
+			resetVelocity();
 			_fuse_hpos_as_odom = false;
 			if (_time_last_fake_gps != 0) {
 				ECL_WARN("EKF stopping navigation");
@@ -1358,13 +1360,18 @@ void Ekf::controlVelPosFusion()
 		}
 
 		_fuse_pos = true;
+		_fuse_hor_vel = false;
+		_fuse_vert_vel = false;
 		_time_last_fake_gps = _time_last_imu;
 
 		if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
 			_posObsNoiseNE = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
 		} else {
 			_posObsNoiseNE = 0.5f;
 		}
+		_vel_pos_innov[0] = 0.0f;
+		_vel_pos_innov[1] = 0.0f;
+		_vel_pos_innov[2] = 0.0f;
 		_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
 		_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
 

diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
@@ -64,7 +64,10 @@ bool Ekf::resetVelocity()
 		zeroOffDiag(P,4,6);
 
 	} else {
-		return false;
+		// Used when falling back to non-aiding mode of operation
+		_state.vel(0) = 0.0f;
+		_state.vel(1) = 0.0f;
+		setDiag(P,4,5,25.0f);
 	}
 
 	// calculate the change in velocity and apply to the output predictor state history
@@ -122,7 +125,10 @@ bool Ekf::resetPosition()
 		setDiag(P,7,8,sq(_ev_sample_delayed.posErr));
 
 	} else {
-		return false;
+		// Used when falling back to non-aiding mode of operation
+		_state.pos(0) = _last_known_posNE(0);
+		_state.pos(1) = _last_known_posNE(1);
+		setDiag(P,7,8,sq(_params.pos_noaid_noise));
 	}
 
 	// calculate the change in position and apply to the output predictor state history