EKF: Correct for sensor noise and baro offset during alignment

EKF/ekf.cpp

@@ -272,10 +272,9 @@ bool Ekf::initialiseFilter(void)
 		// calculate the averaged barometer reading
 		_baro_at_alignment = _baro_sum / (float)_baro_counter;
 
-		// set the velocity to the GPS measurement (by definition, the initial position and height is at the origin)
 		resetVelocity();
+		resetPosition();
 
-		// initialise the state covariance matrix
 		initialiseCovariance();
 
 		return true;

EKF/ekf_helper.cpp

@@ -85,13 +85,7 @@ void Ekf::resetHeight()
 {
 	// if we have a valid height measurement, use it to initialise the vertical position state
 	baroSample baro_newest = _baro_buffer.get_newest();
-
-	if (_time_last_imu - baro_newest.time_us < 200000) {
-		_state.pos(2) = _baro_at_alignment - baro_newest.hgt;
-
-	} else {
-		// XXX use the value of the last known position
-	}
+	_state.pos(2) = _baro_at_alignment - baro_newest.hgt;
 }
 
 #if defined(__PX4_POSIX) && !defined(__PX4_QURT)