diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index dc9caadebc3e..65dd40adff36 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -230,7 +230,8 @@ bool Ekf::initialiseTilt() void Ekf::predictState(const imuSample &imu_delayed) { - if (std::fabs(_gpos.latitude_rad() - _earth_rate_lat_ref_rad) > math::radians(1.0)) { + if (!PX4_ISFINITE(_earth_rate_lat_ref_rad) + || std::fabs(_gpos.latitude_rad() - _earth_rate_lat_ref_rad) > math::radians(1.0)) { _earth_rate_lat_ref_rad = _gpos.latitude_rad(); _earth_rate_NED = calcEarthRateNED((float)_earth_rate_lat_ref_rad); } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f3f449f64db6..51a25b8598fa 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -488,7 +488,7 @@ class Ekf final : public EstimatorInterface LatLonAlt _last_known_gpos{}; Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s - double _earth_rate_lat_ref_rad; ///< latitude at which the earth rate was evaluated (radians) + double _earth_rate_lat_ref_rad{NAN}; ///< latitude at which the earth rate was evaluated (radians) Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction