diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index d883d83ff8..c3d4434a71 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index c2277606b1..47c6e10f94 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/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 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 0fa7115df7..bb6a384d6a 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 @@ 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(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)