fix(ekf2): set Earth rate and gravity near the equator

The Earth rate and gravity were only evaluated once the latitude moved
more than 1 deg away from a reference initialised to 0, so within 1 deg
of the equator the filter kept a zero Earth rate and standard gravity.
Evaluate them at the first latitude known after the origin is set.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
bresch authored and Mathieu Bresciani committed 2026-10-14 17:03:05 +02:00
1 parent 6130b089a2
commit 7635d64f97
3 files changed
+8 -6

No files matched your search

+3 -1
View File
@@ -233,7 +233,9 @@ 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 (global_origin_valid()
&& (!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);
_gravity = LatLonAlt::Wgs84::gravity(_earth_rate_lat_ref_rad);
+1 -1
View File
@@ -541,7 +541,7 @@ private:
LatLonAlt _last_known_gpos{};
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)
double _earth_rate_lat_ref_rad{static_cast<double>(NAN)}; ///< latitude at which the earth rate was evaluated (radians)
float _gravity{CONSTANTS_ONE_G}; ///< latitude-dependent gravity magnitude (m/s^2)
+4 -4
View File
@@ -193,17 +193,17 @@ TEST_F(EkfBasicsTest, gravityIsStandardBeforeGlobalPosition)
EXPECT_FLOAT_EQ(_ekf->getGravityMss(), CONSTANTS_ONE_G);
}
TEST_F(EkfBasicsTest, gravityStaysStandardNearEquator)
TEST_F(EkfBasicsTest, gravityUpdatesNearEquator)
{
// GIVEN: GPS fixed within 1 deg of the equator
_sensor_simulator.setGpsLatitude(0.5);
_sensor_simulator.startGps();
_sensor_simulator.runSeconds(11);
// THEN: GPS is fused but the latitude is within the recompute threshold of the
// 0-reference, so gravity remains the standard constant (matches legacy behavior)
// THEN: gravity is evaluated at the first known latitude, however close to the equator
ASSERT_EQ(1, (int) _ekf->control_status_flags().gnss_pos);
EXPECT_FLOAT_EQ(_ekf->getGravityMss(), CONSTANTS_ONE_G);
EXPECT_FLOAT_EQ(_ekf->getGravityMss(), LatLonAlt::Wgs84::gravity(math::radians(0.5)));
EXPECT_LT(_ekf->getGravityMss(), CONSTANTS_ONE_G - 0.02f);
}
TEST_F(EkfBasicsTest, gravityUpdatesAtHighLatitude)