diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 51f01c02f1..a4a3d3204a 100644 --- a/EKF/ekf.cpp +++ b/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; diff --git a/EKF/ekf.h b/EKF/ekf.h index d8931d42fd..3c6b30f2e0 100644 --- a/EKF/ekf.h +++ b/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 diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index aa89933738..57c9a31b8b 100644 --- a/EKF/gps_checks.cpp +++ b/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