diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index c1b47c224fa7..e21ee82ff9f2 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -55,21 +55,6 @@ #define MASK_GPS_VSPD (1<<8) #define MASK_GPS_SPOOFED (1<<9) -void Ekf::collect_gps(const gnssSample &gps) -{ - if (_filter_initialised && !_local_origin_lat_lon.isInitialized() && _gps_checks_passed) { - _information_events.flags.gps_checks_passed = true; - - } else { - // a rough 2D fix is sufficient to lookup earth spin rate - const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000); - - if (gps_rough_2d_fix && (_gps_checks_passed || !_local_origin_lat_lon.isInitialized())) { - _earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat)); - } - } -} - bool Ekf::runGnssChecks(const gnssSample &gps) { _gps_check_fail_status.flags.spoofed = gps.spoofed; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index de8dad37b5e8..889af97a08ce 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -67,11 +67,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) && isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) { if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) { // First time checks are passing, latching. + if (!_gps_checks_passed) { + _information_events.flags.gps_checks_passed = true; + } + _gps_checks_passed = true; } - collect_gps(gnss_sample); - } else { // Skip this sample _gps_data_ready = false; diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index e1fb96784fd6..3661ac249b12 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -230,6 +230,11 @@ 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)) { + _earth_rate_lat_ref_rad = _gpos.latitude_rad(); + _earth_rate_NED = calcEarthRateNED((float)_earth_rate_lat_ref_rad); + } + // apply imu bias corrections const Vector3f delta_ang_bias_scaled = getGyroBias() * imu_delayed.delta_ang_dt; Vector3f corrected_delta_ang = imu_delayed.delta_ang - delta_ang_bias_scaled; @@ -254,8 +259,11 @@ void Ekf::predictState(const imuSample &imu_delayed) // calculate the increment in velocity using the current orientation _state.vel += corrected_delta_vel_ef; - // compensate for acceleration due to gravity - _state.vel(2) += CONSTANTS_ONE_G * imu_delayed.delta_vel_dt; + // compensate for acceleration due to gravity, Coriolis and transport rate + const Vector3f gravity_acceleration(0.f, 0.f, -CONSTANTS_ONE_G); // simplistic model + const Vector3f coriolis_acceleration = 2.f * _earth_rate_NED.cross(vel_last); + const Vector3f transport_rate = _gpos.computeAngularRateNavFrame(vel_last).cross(vel_last); + _state.vel -= (gravity_acceleration + coriolis_acceleration + transport_rate) * imu_delayed.delta_vel_dt; // predict position states via trapezoidal integration of velocity _gpos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index fdcb8dc59106..76cd746e5d5c 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -374,8 +374,6 @@ class Ekf final : public EstimatorInterface #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) - void collect_gps(const gnssSample &gps); - // set minimum continuous period without GPS fail required to mark a healthy GPS status void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; } @@ -490,6 +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) Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction diff --git a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp index 0fa7115df77a..bb6a384d6ac6 100644 --- a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp +++ b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp @@ -120,6 +120,21 @@ class LatLonAlt -delta_alt); } + /* + * Compute the angular rate of the local navigation frame at the current latitude and height + * with respect to an inertial frame and resolved in the navigation frame + */ + matrix::Vector3f computeAngularRateNavFrame(const matrix::Vector3f &v_ned) + { + double r_n; + double r_e; + computeRadiiOfCurvature(_latitude_rad, r_n, r_e); + return matrix::Vector3f( + v_ned(1) / (static_cast(r_e) + _altitude), + -v_ned(0) / (static_cast(r_n) + _altitude), + -v_ned(1) * tanf(_latitude_rad) / (static_cast(r_e) + _altitude)); + } + private: // Convert between curvilinear and cartesian errors static matrix::Vector2d deltaLatLonToDeltaXY(const double latitude, const float altitude)