Replace calculation of earth NED rotation vector

EKF/ekf.cpp

@@ -248,13 +248,6 @@ bool Ekf::initialiseTilt()
 
 void Ekf::predictState()
 {
-	if (!_earth_rate_initialised) {
-		if (_NED_origin_initialised) {
-			calcEarthRateNED(_earth_rate_NED, (float)_pos_ref.lat_rad);
-			_earth_rate_initialised = true;
-		}
-	}
-
 	// apply imu bias corrections
 	Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.delta_ang_bias;
 	Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias;

EKF/ekf.h

@@ -309,7 +309,6 @@ private:
 	stateSample _state{};		///< state struct of the ekf running at the delayed time horizon
 
 	bool _filter_initialised{false};	///< true when the EKF sttes and covariances been initialised
-	bool _earth_rate_initialised{false};	///< true when we know the earth rotatin rate (requires GPS)
 
 	bool _fuse_height{false};	///< true when baro height data should be fused
 	bool _fuse_pos{false};		///< true when gps position data should be fused

EKF/gps_checks.cpp

@@ -76,6 +76,7 @@ bool Ekf::collect_gps(const gps_message &gps)
 		// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
 		_gps_alt_ref = 1e-3f * (float)gps.alt + _state.pos(2);
 		_NED_origin_initialised = true;
+		calcEarthRateNED(_earth_rate_NED, (float)_pos_ref.lat_rad);
 		_last_gps_origin_time_us = _time_last_imu;
 
 		// set the magnetic field data returned by the geo library using the current GPS position