mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 22:50:49 +08:00
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:
1 parent
6130b089a2
commit
7635d64f97
3 files changed
+8
-6
No files matched your search
@@ -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);
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
Reference in new issue
Block a user