EKF: Fix bugs in position and velocity resets

The position reset was not being compensated for velocity and measurement delay
The height was being reset with the position. It has been moved into a separate reset function
The maximum accepted GPS delay of 100msec was inadequate
The states  was being incorrectly reset to the GPS position and Baro height on initial alignment.

EKF/ekf.cpp

@@ -272,9 +272,10 @@ 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,7 +85,13 @@ 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();
-	_state.pos(2) = _baro_at_alignment - baro_newest.hgt;
+
+	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
+	}
 }
 
 #if defined(__PX4_POSIX) && !defined(__PX4_QURT)