ekf2: compensate for coriolis and transport rate accelerations

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;

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;

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;

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
 

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<float>(r_e) + _altitude),
+			       -v_ned(0) / (static_cast<float>(r_n) + _altitude),
+			       -v_ned(1) * tanf(_latitude_rad) / (static_cast<float>(r_e) + _altitude));
+	}
+
 private:
 	// Convert between curvilinear and cartesian errors
 	static matrix::Vector2d deltaLatLonToDeltaXY(const double latitude, const float altitude)