Merge pull request #92 from priseborough/pr-ekf2HgtResetFix

EKF: Fix vulnerability in height reset

EKF/control.cpp

@@ -184,9 +184,7 @@ void Ekf::controlFusionModes()
 				_control_status.flags.rng_hgt = false;
 				// adjust the height offset so we can use the GPS
 				_hgt_sensor_offset = _state.pos(2) + gps_init.hgt - _gps_alt_ref;
-				if (!baro_hgt_available) {
-					printf("EKF baro hgt timeout - switching to gps\n");
-				}
+				printf("EKF baro hgt timeout - switching to gps\n");
 			}
 		}
 

EKF/ekf_helper.cpp

@@ -87,11 +87,15 @@ bool Ekf::resetPosition()
 // Reset height state using the last height measurement
 void Ekf::resetHeight()
 {
+	// Get the most recent GPS data
+	gpsSample gps_newest = _gps_buffer.get_newest();
+
 	if (_control_status.flags.rng_hgt) {
 		rangeSample range_newest = _range_buffer.get_newest();
 
 		if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) {
 			_state.pos(2) = _hgt_sensor_offset - range_newest.rng;
+			P[8][8] = sq(_params.range_noise);
 
 		} else {
 			// TODO: reset to last known range based estimate
@@ -107,6 +111,7 @@ void Ekf::resetHeight()
 
 		if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) {
 			_state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset;
+			P[8][8] = sq(_params.baro_noise);
 
 		} else {
 			// TODO: reset to last known baro based estimate
@@ -114,11 +119,9 @@ void Ekf::resetHeight()
 
 	} else if (_control_status.flags.gps_hgt) {
 		// initialize vertical position and velocity with newest gps measurement
-		gpsSample gps_newest = _gps_buffer.get_newest();
-
 		if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) {
 			_state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref;
-			_state.vel(2) = gps_newest.vel(2);
+			P[8][8] = sq(gps_newest.hacc);
 
 		} else {
 			// TODO: reset to last known gps based estimate
@@ -129,6 +132,15 @@ void Ekf::resetHeight()
 		_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
 	}
 
+	// If we are using GPS, then use it to reset the vertical velocity
+	if (_control_status.flags.gps && (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL)) {
+		_state.vel(2) = gps_newest.vel(2);
+		P[5][5] = sq(1.5f * gps_newest.sacc);
+	} else {
+		P[5][5] = fminf(sq(_state.vel(2)),1000.0f);
+		_state.vel(2) = 0.0f;
+	}
+
 }
 
 // Reset heading and magnetic field states