ekf2: compensate for coriolis and transport rate accelerations

src/modules/ekf2/EKF/ekf.cpp

@@ -259,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

@@ -487,7 +487,8 @@ private:
 
 	LatLonAlt _last_known_gpos{};
 
-	Vector3f _earth_rate_NED{};	///< earth rotation vector (NED) in rad/s
+	Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s
+	double _earth_rate_lat_ref_rad{0.0}; ///< 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 @@ public:
 					-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)