Skip to content

Commit

Permalink
ekf2: rework amsl to ellipsoid altitude conversion
Browse files Browse the repository at this point in the history
  • Loading branch information
bresch authored and dagar committed Oct 1, 2024
1 parent 5bfa6b3 commit dbf51da
Show file tree
Hide file tree
Showing 2 changed files with 35 additions and 35 deletions.
55 changes: 28 additions & 27 deletions src/modules/ekf2/EKF2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1176,7 +1176,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)

global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters
#if defined(CONFIG_EKF2_GNSS)
global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt);
global_pos.alt_ellipsoid = altAmslToEllipsoid(global_pos.alt);
#else
global_pos.alt_ellipsoid = global_pos.alt;
#endif
Expand Down Expand Up @@ -2051,29 +2051,6 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
}
#endif // CONFIG_EKF2_OPTICAL_FLOW

#if defined(CONFIG_EKF2_GNSS)
float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
{
float height_diff = static_cast<float>(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt;

if (_gps_alttitude_ellipsoid_previous_timestamp == 0) {

_wgs84_hgt_offset = height_diff;
_gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;

} else if (_gps_time_usec != _gps_alttitude_ellipsoid_previous_timestamp) {

// apply a 10 second first order low pass filter to baro offset
float dt = 1e-6f * (_gps_time_usec - _gps_alttitude_ellipsoid_previous_timestamp);
_gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;
float offset_rate_correction = 0.1f * (height_diff - _wgs84_hgt_offset);
_wgs84_hgt_offset += dt * constrain(offset_rate_correction, -0.1f, 0.1f);
}

return amsl_hgt + _wgs84_hgt_offset;
}
#endif // CONFIG_EKF2_GNSS

#if defined(CONFIG_EKF2_AIRSPEED)
void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
{
Expand Down Expand Up @@ -2433,11 +2410,14 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
return; //TODO: change and set to NAN
}

const float altitude_amsl = static_cast<float>(vehicle_gps_position.altitude_msl_m);
const float altitude_ellipsoid = static_cast<float>(vehicle_gps_position.altitude_ellipsoid_m);

gnssSample gnss_sample{
.time_us = vehicle_gps_position.timestamp,
.lat = vehicle_gps_position.latitude_deg,
.lon = vehicle_gps_position.longitude_deg,
.alt = static_cast<float>(vehicle_gps_position.altitude_msl_m),
.alt = altitude_amsl,
.vel = vel_ned,
.hacc = vehicle_gps_position.eph,
.vacc = vehicle_gps_position.epv,
Expand All @@ -2454,10 +2434,31 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)

_ekf.setGpsData(gnss_sample);

_gps_time_usec = gnss_sample.time_us;
_gps_alttitude_ellipsoid = static_cast<int32_t>(round(vehicle_gps_position.altitude_ellipsoid_m * 1e3));
const float geoid_height = altitude_ellipsoid - altitude_amsl;

if (_last_geoid_height_update_us == 0) {
_geoid_height_lpf.reset(geoid_height);
_last_geoid_height_update_us = gnss_sample.time_us;

} else if (gnss_sample.time_us > _last_geoid_height_update_us) {
const float dt = 1e-6f * (gnss_sample.time_us - _last_geoid_height_update_us);
_geoid_height_lpf.setParameters(dt, kGeoidHeightLpfTimeConstant);
_geoid_height_lpf.update(geoid_height);
_last_geoid_height_update_us = gnss_sample.time_us;
}

}
}

float EKF2::altEllipsoidToAmsl(float ellipsoid_alt) const
{
return ellipsoid_alt - _geoid_height_lpf.getState();
}

float EKF2::altAmslToEllipsoid(float amsl_alt) const
{
return amsl_alt + _geoid_height_lpf.getState();
}
#endif // CONFIG_EKF2_GNSS

#if defined(CONFIG_EKF2_MAGNETOMETER)
Expand Down
15 changes: 7 additions & 8 deletions src/modules/ekf2/EKF2.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@
#include "EKF/ekf.h"

#include "EKF2Selector.hpp"
#include "mathlib/math/filter/AlphaFilter.hpp"

#include <float.h>

Expand Down Expand Up @@ -210,10 +211,8 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps);
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GNSS)
/*
* Calculate filtered WGS84 height from estimated AMSL height
*/
float filter_altitude_ellipsoid(float amsl_hgt);
float altEllipsoidToAmsl(float ellipsoid_alt) const;
float altAmslToEllipsoid(float amsl_alt) const;

void PublishGpsStatus(const hrt_abstime &timestamp);
void PublishGnssHgtBias(const hrt_abstime &timestamp);
Expand Down Expand Up @@ -445,10 +444,10 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
#endif // CONFIG_EKF2_WIND

#if defined(CONFIG_EKF2_GNSS)
uint64_t _gps_time_usec {0};
int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid
uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt
float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84

uint64_t _last_geoid_height_update_us{0};
static constexpr float kGeoidHeightLpfTimeConstant = 10.f;
AlphaFilter<float> _geoid_height_lpf; ///< height offset between AMSL and ellipsoid

hrt_abstime _last_gps_status_published{0};

Expand Down

0 comments on commit dbf51da

Please sign in to comment.