fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: store position state as lat/lon/alt

Browse Source 
The position error state is still defined in a body-fixed NED frame but the
position state itself is latitude-longitude-altitude.
This commit is contained in:
bresch 2024-10-02 09:54:08 +02:00 committed by Mathieu Bresciani
parent cf34b9d574
commit b19a6ee3b5
33 changed files with 783 additions and 348 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/EstimatorAidSource2d.msg
View File

@ -7,7 +7,7 @@ uint32 device_id
uint64 time_last_fuse
float32[2] observation
float64[2] observation
float32[2] observation_variance
float32[2] innovation

1
src/modules/ekf2/EKF/CMakeLists.txt
View File

@ -33,6 +33,7 @@
add_subdirectory(bias_estimator)
add_subdirectory(output_predictor)
add_subdirectory(lat_lon_alt)
set(EKF_LIBS)
set(EKF_SRCS)

15
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp
View File

@ -74,11 +74,9 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
}
estimator_aid_source2d_s aid_src{};
Vector2f position;
const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used
if (ekf.global_origin_valid()) {
position = ekf.global_origin().project(sample.latitude, sample.longitude);
//const float hgt = ekf.getEkfGlobalOriginAltitude() - (float)sample.altitude;
// relax the upper observation noise limit which prevents bad measurements perturbing the position estimate
float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f);
const float pos_var = sq(pos_noise);
@ -86,12 +84,11 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
ekf.updateAidSourceStatus(aid_src,
sample.time_us, // sample timestamp
position, // observation
matrix::Vector2d(sample.latitude, sample.longitude), // observation
pos_obs_var, // observation variance
Vector2f(ekf.state().pos) - position, // innovation
innovation, // innovation
Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate
}
const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
&& ekf.control_status_flags().yaw_align;
@ -113,7 +110,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
} else {
// Try to initialize using measurement
if (ekf.setEkfGlobalOriginFromCurrentPos(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
sample.epv)) {
ekf.enableControlStatusAuxGpos();
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
@ -131,7 +128,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max)
|| (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
ekf.resetAidSourceStatusZeroInnovation(aid_src);

14
src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp
View File

@ -73,7 +73,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
if (_baro_counter <= _obs_buffer_length) {
// Initialize the pressure offset (included in the baro bias)
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
}
}
@ -106,7 +106,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
if (measurement_valid) {
bias_est.setMaxStateNoise(sqrtf(measurement_var));
bias_est.setProcessNoiseSpectralDensity(_params.baro_bias_nsd);
bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
}
// determine if we should use height aiding
@ -131,8 +131,8 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
_information_events.flags.reset_hgt_to_baro = true;
resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var);
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
resetHeightTo(_baro_lpf.getState() - bias_est.getBias(), measurement_var);
bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
// reset vertical velocity if no valid sources available
if (!isVerticalVelocityAidingActive()) {
@ -163,12 +163,12 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
_height_sensor_ref = HeightSensor::BARO;
_information_events.flags.reset_hgt_to_baro = true;
resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var);
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
resetAltitudeTo(measurement, measurement_var);
bias_est.reset();
} else {
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
}
aid_src.time_last_fuse = imu_sample.time_us;

14
src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp
View File

@ -99,7 +99,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
if (measurement_valid && quality_sufficient) {
bias_est.setMaxStateNoise(sqrtf(measurement_var));
bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd);
bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
bias_est.fuseBias(measurement + _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
}
const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
@ -117,11 +117,11 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
if (_height_sensor_ref == HeightSensor::EV) {
_information_events.flags.reset_hgt_to_ev = true;
resetVerticalPositionTo(measurement, measurement_var);
resetHeightTo(-measurement, measurement_var);
bias_est.reset();
} else {
bias_est.setBias(-_state.pos(2) + measurement);
bias_est.setBias(_gpos.altitude() + measurement);
}
aid_src.time_last_fuse = _time_delayed_us;
@ -146,8 +146,8 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
// All height sources are failing
ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME);
_information_events.flags.reset_hgt_to_ev = true;
resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var);
bias_est.setBias(-_state.pos(2) + measurement);
resetHeightTo(-measurement - bias_est.getBias(), measurement_var);
bias_est.setBias(_gpos.altitude() + measurement);
aid_src.time_last_fuse = _time_delayed_us;
@ -170,14 +170,14 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::EV)) {
ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
_information_events.flags.reset_hgt_to_ev = true;
resetVerticalPositionTo(measurement, measurement_var);
resetHeightTo(-measurement, measurement_var);
_height_sensor_ref = HeightSensor::EV;
bias_est.reset();
} else {
ECL_INFO("starting %s fusion", AID_SRC_NAME);
bias_est.setBias(-_state.pos(2) + measurement);
bias_est.setBias(_gpos.altitude() + measurement);
}
aid_src.time_last_fuse = _time_delayed_us;

16
src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp
View File

@ -137,6 +137,8 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
#endif // CONFIG_EKF2_GNSS
const Vector2f position_estimate = getLocalHorizontalPosition();
const Vector2f measurement{pos(0), pos(1)};
const Vector2f measurement_var{
@ -150,7 +152,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
if (!bias_fusion_was_active && _ev_pos_b_est.fusionActive()) {
if (quality_sufficient) {
// reset the bias estimator
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
_ev_pos_b_est.setBias(-position_estimate + measurement);
} else if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.ev_pos)) {
// otherwise stop EV position, when quality is good again it will restart with reset bias
@ -165,7 +167,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
ev_sample.time_us, // sample timestamp
position, // observation
pos_obs_var, // observation variance
Vector2f(_state.pos) - position, // innovation
position_estimate - position, // innovation
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate
@ -174,7 +176,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
if (measurement_valid && quality_sufficient) {
_ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1))));
_ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO
_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()),
_ev_pos_b_est.fuseBias(measurement - position_estimate,
measurement_var + Vector2f(getStateVariance<State::pos>()));
}
@ -213,7 +215,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem
// TODO: (_params.position_sensor_ref == PositionSensor::EV)
if (_control_status.flags.gps) {
ECL_INFO("starting %s fusion", EV_AID_SRC_NAME);
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
_ev_pos_b_est.setFusionActive();
} else {
@ -245,7 +247,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
_ev_pos_b_est.reset();
} else {
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
}
aid_src.time_last_fuse = _time_delayed_us;
@ -275,14 +277,14 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
if (_control_status.flags.gps && !pos_xy_fusion_failing) {
// reset EV position bias
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
_ev_pos_b_est.setBias(-Vector2f(getLocalHorizontalPosition()) + measurement);
} else {
_information_events.flags.reset_pos_to_vision = true;
if (_control_status.flags.gps) {
resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
} else {
resetHorizontalPositionTo(measurement, measurement_var);

8
src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp
View File

@ -51,7 +51,7 @@ void Ekf::controlFakeHgtFusion()
const float obs_var = sq(_params.pos_noaid_noise);
const float innov_gate = 3.f;
updateVerticalPositionAidStatus(aid_src, _time_delayed_us, _last_known_pos(2), obs_var, innov_gate);
updateVerticalPositionAidStatus(aid_src, _time_delayed_us, -_last_known_gpos.altitude(), obs_var, innov_gate);
const bool continuing_conditions_passing = !isVerticalAidingActive();
const bool starting_conditions_passing = continuing_conditions_passing
@ -98,7 +98,7 @@ void Ekf::controlFakeHgtFusion()
void Ekf::resetFakeHgtFusion()
{
ECL_INFO("reset fake height fusion");
_last_known_pos(2) = _state.pos(2);
_last_known_gpos.setAltitude(_gpos.altitude());
resetVerticalVelocityToZero();
resetHeightToLastKnown();
@ -109,8 +109,8 @@ void Ekf::resetFakeHgtFusion()
void Ekf::resetHeightToLastKnown()
{
_information_events.flags.reset_pos_to_last_known = true;
ECL_INFO("reset height to last known (%.3f)", (double)_last_known_pos(2));
resetVerticalPositionTo(_last_known_pos(2), sq(_params.pos_noaid_noise));
ECL_INFO("reset height to last known (%.3f)", (double)_last_known_gpos.altitude());
resetHeightTo(_last_known_gpos.altitude(), sq(_params.pos_noaid_noise));
}
void Ekf::stopFakeHgtFusion()

8
src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp
View File

@ -63,15 +63,15 @@ void Ekf::controlFakePosFusion()
obs_var(0) = obs_var(1) = sq(0.5f);
}
const Vector2f position(_last_known_pos);
const Vector2f innovation = (_gpos - _last_known_gpos).xy();
const float innov_gate = 3.f;
updateAidSourceStatus(aid_src,
_time_delayed_us,
position, // observation
Vector2f(_gpos.latitude_deg(), _gpos.longitude_deg()), // observation
obs_var, // observation variance
Vector2f(_state.pos) - position, // innovation
innovation, // innovation
Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
innov_gate); // innovation gate
@ -95,7 +95,7 @@ void Ekf::controlFakePosFusion()
void Ekf::resetFakePosFusion()
{
ECL_INFO("reset fake position fusion");
_last_known_pos.xy() = _state.pos.xy();
_last_known_gpos.setLatLon(_gpos);
resetHorizontalPositionToLastKnown();
resetHorizontalVelocityToZero();

18
src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp
View File

@ -62,9 +62,9 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
const float gnss_alt = _gps_sample_delayed.alt + pos_offset_earth(2);
const float gnss_alt = gps_sample.alt + pos_offset_earth(2);
const float measurement = gnss_alt - getEkfGlobalOriginAltitude();
const float measurement = gnss_alt;
const float measurement_var = sq(noise);
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
@ -81,13 +81,13 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
if (measurement_valid) {
bias_est.setMaxStateNoise(sqrtf(measurement_var));
bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd);
bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
}
// determine if we should use height aiding
const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
&& measurement_valid
&& _pos_ref.isInitialized()
&& _local_origin_lat_lon.isInitialized()
&& _gps_checks_passed;
const bool starting_conditions_passing = continuing_conditions_passing
@ -105,8 +105,8 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
_information_events.flags.reset_hgt_to_gps = true;
resetVerticalPositionTo(aid_src.observation, measurement_var);
bias_est.setBias(_state.pos(2) + measurement);
resetHeightTo(measurement, measurement_var);
bias_est.setBias(-_gpos.altitude() + measurement);
aid_src.time_last_fuse = _time_delayed_us;
@ -128,13 +128,13 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
_height_sensor_ref = HeightSensor::GNSS;
_information_events.flags.reset_hgt_to_gps = true;
resetVerticalPositionTo(-measurement, measurement_var);
_gpos_origin_epv = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty
resetAltitudeTo(measurement, measurement_var);
bias_est.reset();
} else {
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
bias_est.setBias(_state.pos(2) + measurement);
bias_est.setBias(-_gpos.altitude() + measurement);
}
aid_src.time_last_fuse = _time_delayed_us;

15
src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
View File

@ -57,25 +57,14 @@
void Ekf::collect_gps(const gnssSample &gps)
{
if (_filter_initialised && !_pos_ref.isInitialized() && _gps_checks_passed) {
// If we have good GPS data set the origin's WGS-84 position to the last gps fix
setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc);
// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
if (!PX4_ISFINITE(_gps_alt_ref)) {
setAltOriginFromCurrentPos(gps.alt, gps.vacc);
}
if (_filter_initialised && !_local_origin_lat_lon.isInitialized() && _gps_checks_passed) {
_information_events.flags.gps_checks_passed = true;
ECL_INFO("GPS origin set to lat=%.6f, lon=%.6f",
_pos_ref.getProjectionReferenceLat(), _pos_ref.getProjectionReferenceLon());
} else {
// a rough 2D fix is sufficient to lookup earth spin rate
const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000);
if (gps_rough_2d_fix && (_gps_checks_passed || !_pos_ref.isInitialized())) {
if (gps_rough_2d_fix && (_gps_checks_passed || !_local_origin_lat_lon.isInitialized())) {
_earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat));
}
}

26
src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp
View File

@ -82,10 +82,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
}
}
if (_pos_ref.isInitialized()) {
updateGnssPos(gnss_sample, _aid_src_gnss_pos);
}
updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel);
} else if (_control_status.flags.gps) {
@ -108,9 +105,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
const bool continuing_conditions_passing = (gnss_vel_enabled || gnss_pos_enabled)
&& _control_status.flags.tilt_align
&& _control_status.flags.yaw_align
&& _pos_ref.isInitialized();
&& _control_status.flags.yaw_align;
const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed;
const bool gpos_init_conditions_passing = gnss_pos_enabled && _gps_checks_passed;
if (_control_status.flags.gps) {
if (continuing_conditions_passing) {
@ -174,6 +171,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
}
_control_status.flags.gps = true;
} else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) {
resetHorizontalPositionToGnss(_aid_src_gnss_pos);
}
}
}
@ -221,8 +221,10 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
{
// correct position and height for offset relative to IMU
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
const Vector2f position = _pos_ref.project(gnss_sample.lat, gnss_sample.lon) - pos_offset_earth.xy();
const Vector3f pos_offset_earth = Vector3f(_R_to_earth * pos_offset_body);
const LatLonAlt measurement(gnss_sample.lat, gnss_sample.lon, gnss_sample.alt);
const LatLonAlt measurement_corrected = measurement + (-pos_offset_earth);
const Vector2f innovation = (_gpos - measurement_corrected).xy();
// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise);
@ -237,12 +239,13 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
const float pos_var = math::max(sq(pos_noise), sq(0.01f));
const Vector2f pos_obs_var(pos_var, pos_var);
const matrix::Vector2d observation(measurement_corrected.latitude_deg(), measurement_corrected.longitude_deg());
updateAidSourceStatus(aid_src,
gnss_sample.time_us, // sample timestamp
position, // observation
observation, // observation
pos_obs_var, // observation variance
Vector2f(_state.pos) - position, // innovation
innovation, // innovation
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate
}
@ -322,8 +325,9 @@ void Ekf::resetVelocityToGnss(estimator_aid_source3d_s &aid_src)
void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src)
{
_information_events.flags.reset_pos_to_gps = true;
resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
_gpos_origin_eph = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty
resetLatLonTo(aid_src.observation[0], aid_src.observation[1],
aid_src.observation_variance[0] +
aid_src.observation_variance[1]);
resetAidSourceStatusZeroInnovation(aid_src);
}

9
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
View File

@ -90,12 +90,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
if (global_origin_valid()
&& (origin_newer_than_last_mag || (isLocalHorizontalPositionValid() && isTimedOut(_wmm_mag_time_last_checked, 10e6)))
) {
// position of local NED origin in GPS / WGS84 frame
double latitude_deg;
double longitude_deg;
global_origin().reproject(_state.pos(0), _state.pos(1), latitude_deg, longitude_deg);
if (updateWorldMagneticModel(latitude_deg, longitude_deg)) {
if (updateWorldMagneticModel(_gpos.latitude_deg(), _gpos.longitude_deg())) {
wmm_updated = true;
}
@ -368,7 +363,7 @@ bool Ekf::checkHaglYawResetReq() const
// Check if height has increased sufficiently to be away from ground magnetic anomalies
// and request a yaw reset if not already requested.
static constexpr float mag_anomalies_max_hagl = 1.5f;
const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
const bool above_mag_anomalies = (getTerrainVPos() + _gpos.altitude()) > mag_anomalies_max_hagl;
return above_mag_anomalies;
}

6
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
View File

@ -233,7 +233,9 @@ void Ekf::resetFlowFusion(const flowSample &flow_sample)
// reset position, estimate is relative to initial position in this mode, so we start with zero error
if (!_control_status.flags.in_air) {
ECL_INFO("reset position to zero");
resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f);
//TODO: reset origin instead?
resetHorizontalPositionToLastKnown();
// resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f);
}
resetAidSourceStatusZeroInnovation(_aid_src_optical_flow);
@ -247,7 +249,7 @@ void Ekf::resetTerrainToFlow()
ECL_INFO("reset hagl to flow");
// TODO: use the flow data
const float new_terrain = fmaxf(0.0f, _state.pos(2));
const float new_terrain = -_gpos.altitude() + _params.rng_gnd_clearance;
const float delta_terrain = new_terrain - _state.terrain;
_state.terrain = new_terrain;
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 100.f);

3
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
View File

@ -67,7 +67,8 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
// recalculate the innovation using the updated state
const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias;
_aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - _aid_src_optical_flow.observation[1];
_aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - static_cast<float>
(_aid_src_optical_flow.observation[1]);
}
if (_aid_src_optical_flow.innovation_variance[index] < _aid_src_optical_flow.observation_variance[index]) {

4
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
View File

@ -148,7 +148,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
_height_sensor_ref = HeightSensor::RANGE;
_information_events.flags.reset_hgt_to_rng = true;
resetVerticalPositionTo(-aid_src.observation, aid_src.observation_variance);
resetHeightTo(aid_src.observation, aid_src.observation_variance);
_state.terrain = 0.f;
_control_status.flags.rng_hgt = true;
stopRngTerrFusion();
@ -180,7 +180,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
_information_events.flags.reset_hgt_to_rng = true;
resetVerticalPositionTo(-(aid_src.observation - _state.terrain));
resetHeightTo(aid_src.observation - _state.terrain);
// reset vertical velocity if no valid sources available
if (!isVerticalVelocityAidingActive()) {

59
src/modules/ekf2/EKF/ekf.cpp
View File

@ -74,7 +74,7 @@ void Ekf::reset()
//
#if defined(CONFIG_EKF2_TERRAIN)
// assume a ground clearance
_state.terrain = _state.pos(2) + _params.rng_gnd_clearance;
_state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance;
#endif // CONFIG_EKF2_TERRAIN
#if defined(CONFIG_EKF2_RANGE_FINDER)
@ -97,7 +97,7 @@ void Ekf::reset()
resetGpsDriftCheckFilters();
_gps_checks_passed = false;
#endif // CONFIG_EKF2_GNSS
_gps_alt_ref = NAN;
_local_origin_alt = NAN;
_output_predictor.reset();
@ -113,7 +113,7 @@ void Ekf::reset()
_time_last_heading_fuse = 0;
_time_last_terrain_fuse = 0;
_last_known_pos.setZero();
_last_known_gpos.setZero();
#if defined(CONFIG_EKF2_BAROMETER)
_baro_counter = 0;
@ -168,7 +168,7 @@ bool Ekf::update()
// control fusion of observation data
controlFusionModes(imu_sample_delayed);
_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos,
_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _gpos,
_state.gyro_bias, _state.accel_bias);
return true;
@ -205,7 +205,7 @@ bool Ekf::initialiseFilter()
initialiseCovariance();
// reset the output predictor state history to match the EKF initial values
_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);
_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _gpos);
return true;
}
@ -258,11 +258,11 @@ void Ekf::predictState(const imuSample &imu_delayed)
_state.vel(2) += CONSTANTS_ONE_G * imu_delayed.delta_vel_dt;
// predict position states via trapezoidal integration of velocity
_state.pos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f;
_gpos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f;
_state.pos(2) = -_gpos.altitude();
// constrain states
_state.vel = matrix::constrain(_state.vel, -_params.velocity_limit, _params.velocity_limit);
_state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f);
// calculate a filtered horizontal acceleration with a 1 sec time constant
@ -283,14 +283,12 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
return false;
}
if (!_pos_ref.isInitialized()) {
if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) {
if (!_local_origin_lat_lon.isInitialized()) {
if (!resetLatLonTo(latitude, longitude, sq(eph))) {
return false;
}
if (!PX4_ISFINITE(_gps_alt_ref)) {
setAltOriginFromCurrentPos(altitude, epv);
}
resetAltitudeTo(altitude, sq(epv));
return true;
}
@ -315,12 +313,20 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
pos_correction = _state.vel * dt_s;
}
{
const Vector2f hpos = _pos_ref.project(latitude, longitude) + pos_correction.xy();
LatLonAlt gpos(latitude, longitude, altitude);
bool alt_valid = true;
if (!checkAltitudeValidity(gpos.altitude())) {
gpos.setAltitude(_gpos.altitude());
alt_valid = false;
}
const LatLonAlt gpos_corrected = gpos + pos_correction;
{
const float obs_var = math::max(sq(eph), sq(0.01f));
const Vector2f innov = Vector2f(_state.pos.xy()) - hpos;
const Vector2f innov = (_gpos - gpos_corrected).xy();
const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;
const float sq_gate = sq(5.f); // magic hardcoded gate
@ -334,8 +340,8 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
ECL_INFO("reset position to external observation");
_information_events.flags.reset_pos_to_ext_obs = true;
resetHorizontalPositionTo(hpos, obs_var);
_last_known_pos.xy() = _state.pos.xy();
resetHorizontalPositionTo(gpos_corrected.latitude_deg(), gpos_corrected.longitude_deg(), obs_var);
_last_known_gpos.setLatLon(gpos_corrected);
} else {
ECL_INFO("fuse external observation as position measurement");
@ -348,24 +354,16 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
_state_reset_status.posNE_change.zero();
_time_last_hor_pos_fuse = _time_delayed_us;
_last_known_pos.xy() = _state.pos.xy();
_last_known_gpos.setLatLon(gpos_corrected);
}
}
if (checkAltitudeValidity(altitude)) {
const float altitude_corrected = altitude - pos_correction(2);
if (!PX4_ISFINITE(_gps_alt_ref)) {
setAltOriginFromCurrentPos(altitude_corrected, epv);
} else {
const float vpos = -(altitude_corrected - _gps_alt_ref);
if (alt_valid) {
const float obs_var = math::max(sq(epv), sq(0.01f));
ECL_INFO("reset height to external observation");
resetVerticalPositionTo(vpos, obs_var);
_last_known_pos(2) = _state.pos(2);
}
resetAltitudeTo(gpos_corrected.altitude(), obs_var);
_last_known_gpos.setAltitude(gpos_corrected.altitude());
}
return true;
@ -425,9 +423,10 @@ void Ekf::print_status()
(double)getStateVariance<State::vel>()(2)
);
const Vector3f position = getPosition();
printf("Position (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
State::pos.idx, State::pos.idx + State::pos.dof - 1,
(double)_state.pos(0), (double)_state.pos(1), (double)_state.pos(2),
(double)position(0), (double)position(1), (double) position(2),
(double)getStateVariance<State::pos>()(0), (double)getStateVariance<State::pos>()(1),
(double)getStateVariance<State::pos>()(2)
);

49
src/modules/ekf2/EKF/ekf.h
View File

@ -66,6 +66,8 @@
# include "aid_sources/aux_global_position/aux_global_position.hpp"
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
#include "lat_lon_alt/lat_lon_alt.hpp"
enum class Likelihood { LOW, MEDIUM, HIGH };
class ExternalVisionVel;
@ -102,8 +104,8 @@ public:
bool isTerrainEstimateValid() const { return _terrain_valid; }
// get the estimated terrain vertical position relative to the NED origin
float getTerrainVertPos() const { return _state.terrain; };
float getHagl() const { return _state.terrain - _state.pos(2); }
float getTerrainVertPos() const { return _state.terrain + getEkfGlobalOriginAltitude(); };
float getHagl() const { return _state.terrain + _gpos.altitude(); }
// get the terrain variance
float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); }
@ -192,7 +194,7 @@ public:
bool checkLatLonValidity(double latitude, double longitude);
bool checkAltitudeValidity(float altitude);
bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN);
bool setEkfGlobalOriginFromCurrentPos(double latitude, double longitude, float altitude, float eph = NAN,
bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float eph = NAN,
float epv = NAN);
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
@ -217,17 +219,14 @@ public:
void resetAccelBias();
void resetAccelBiasCov();
// return true if the global position estimate is valid
// return true if the origin is set we are not doing unconstrained free inertial navigation
// and have not started using synthetic position observations to constrain drift
bool isGlobalHorizontalPositionValid() const
{
return _pos_ref.isInitialized() && isLocalHorizontalPositionValid();
return _local_origin_lat_lon.isInitialized() && isLocalHorizontalPositionValid();
}
bool isGlobalVerticalPositionValid() const
{
return _pos_ref.isInitialized() && isLocalVerticalPositionValid();
return PX4_ISFINITE(_local_origin_alt) && isLocalVerticalPositionValid();
}
bool isLocalHorizontalPositionValid() const
@ -473,6 +472,8 @@ private:
StateSample _state{}; ///< state struct of the ekf running at the delayed time horizon
LatLonAlt _gpos{0.0, 0.0, 0.f};
bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised
uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
@ -486,7 +487,7 @@ private:
uint64_t _time_last_heading_fuse{0};
uint64_t _time_last_terrain_fuse{0};
Vector3f _last_known_pos{}; ///< last known local position vector (m)
LatLonAlt _last_known_gpos{};
Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s
@ -645,11 +646,11 @@ private:
P.slice<S.dof, S.dof>(S.idx, S.idx) = cov;
}
bool setLatLonOrigin(double latitude, double longitude, float eph = NAN);
bool setAltOrigin(float altitude, float epv = NAN);
bool setLatLonOrigin(double latitude, double longitude, float hpos_var = NAN);
bool setAltOrigin(float altitude, float vpos_var = NAN);
bool setLatLonOriginFromCurrentPos(double latitude, double longitude, float eph = NAN);
bool setAltOriginFromCurrentPos(float altitude, float epv = NAN);
bool resetLatLonTo(double latitude, double longitude, float hpos_var = NAN);
bool resetAltitudeTo(float altitude, float vpos_var = NAN);
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
@ -711,14 +712,22 @@ private:
void resetHorizontalPositionToLastKnown();
void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var);
void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); }
void resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude,
const Vector2f &new_horz_pos_var);
void resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude, const float pos_var = NAN) { resetHorizontalPositionTo(new_latitude, new_longitude, Vector2f(pos_var, pos_var)); }
void resetHorizontalPositionTo(const Vector2f &new_pos, const Vector2f &new_horz_pos_var);
Vector2f getLocalHorizontalPosition() const;
Vector2f computeDeltaHorizontalPosition(const double &new_latitude, const double &new_longitude) const;
void updateHorizontalPositionResetStatus(const Vector2f &delta);
void resetWindTo(const Vector2f &wind, const Vector2f &wind_var);
bool isHeightResetRequired() const;
void resetVerticalPositionTo(float new_vert_pos, float new_vert_pos_var = NAN);
void resetHeightTo(float new_altitude, float new_vert_pos_var = NAN);
void updateVerticalPositionResetStatus(const float delta_z);
void resetVerticalVelocityToZero();
@ -740,6 +749,7 @@ private:
void controlTerrainFakeFusion();
void updateTerrainValidity();
void updateTerrainResetStatus(const float delta_z);
# if defined(CONFIG_EKF2_RANGE_FINDER)
// update the terrain vertical position estimate using a height above ground measurement from the range finder
@ -832,6 +842,7 @@ private:
void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient,
bool reset, estimator_aid_source2d_s &aid_src);
void stopEvPosFusion();
void stopEvHgtFusion();
void stopEvVelFusion();
@ -1049,9 +1060,9 @@ private:
}
// helper used for populating and filtering estimator aid source struct for logging
template <typename T, typename S>
template <typename T, typename S, typename D>
void updateAidSourceStatus(T &status, const uint64_t &timestamp_sample,
const S &observation, const S &observation_variance,
const D &observation, const S &observation_variance,
const S &innovation, const S &innovation_variance,
float innovation_gate = 1.f) const
{
@ -1106,7 +1117,7 @@ private:
status.test_ratio[i] = test_ratio;
status.observation[i] = observation(i);
status.observation[i] = static_cast<double>(observation(i));
status.observation_variance[i] = observation_variance(i);
status.innovation[i] = innovation(i);

180
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -65,9 +65,9 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const
void Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const
{
origin_time = _pos_ref.getProjectionReferenceTimestamp();
latitude = _pos_ref.getProjectionReferenceLat();
longitude = _pos_ref.getProjectionReferenceLon();
origin_time = _local_origin_lat_lon.getProjectionReferenceTimestamp();
latitude = _local_origin_lat_lon.getProjectionReferenceLat();
longitude = _local_origin_lat_lon.getProjectionReferenceLon();
origin_alt = getEkfGlobalOriginAltitude();
}
@ -85,156 +85,170 @@ bool Ekf::checkAltitudeValidity(const float altitude)
return (PX4_ISFINITE(altitude) && ((altitude > -12'000.f) && (altitude < 100'000.f)));
}
bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph,
const float epv)
bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float hpos_var,
const float vpos_var)
{
if (!setLatLonOrigin(latitude, longitude, eph)) {
if (!setLatLonOrigin(latitude, longitude, hpos_var)) {
return false;
}
// altitude is optional
setAltOrigin(altitude, epv);
setAltOrigin(altitude, vpos_var);
return true;
}
bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float eph)
bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float hpos_var)
{
if (!checkLatLonValidity(latitude, longitude)) {
return false;
}
bool current_pos_available = false;
double current_lat = static_cast<double>(NAN);
double current_lon = static_cast<double>(NAN);
if (!_local_origin_lat_lon.isInitialized() && isLocalHorizontalPositionValid()) {
// Already navigating in a local frame, use the origin to initialize global position
const Vector2f pos_prev = getLocalHorizontalPosition();
_local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us);
double new_latitude;
double new_longitude;
_local_origin_lat_lon.reproject(pos_prev(0), pos_prev(1), new_latitude, new_longitude);
resetHorizontalPositionTo(new_latitude, new_longitude, hpos_var);
// if we are already doing aiding, correct for the change in position since the EKF started navigating
if (_pos_ref.isInitialized() && isLocalHorizontalPositionValid()) {
_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
current_pos_available = true;
}
// reinitialize map projection to latitude, longitude, altitude, and reset position
_pos_ref.initReference(latitude, longitude, _time_delayed_us);
if (PX4_ISFINITE(eph) && (eph >= 0.f)) {
_gpos_origin_eph = eph;
}
if (current_pos_available) {
// reset horizontal position if we already have a global origin
Vector2f position = _pos_ref.project(current_lat, current_lon);
resetHorizontalPositionTo(position);
} else {
// Simply move the origin and compute the change in local position
const Vector2f pos_prev = getLocalHorizontalPosition();
_local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us);
const Vector2f pos_new = getLocalHorizontalPosition();
const Vector2f delta_pos = pos_new - pos_prev;
updateHorizontalPositionResetStatus(delta_pos);
}
return true;
}
bool Ekf::setAltOrigin(const float altitude, const float epv)
bool Ekf::setAltOrigin(const float altitude, const float vpos_var)
{
if (!checkAltitudeValidity(altitude)) {
return false;
}
const float gps_alt_ref_prev = _gps_alt_ref;
_gps_alt_ref = altitude;
ECL_INFO("EKF origin altitude %.1fm -> %.1fm", (double)_local_origin_alt,
(double)altitude);
if (PX4_ISFINITE(epv) && (epv >= 0.f)) {
_gpos_origin_epv = epv;
}
if (!PX4_ISFINITE(_local_origin_alt) && isLocalVerticalPositionValid()) {
const float local_alt_prev = _gpos.altitude();
_local_origin_alt = altitude;
resetHeightTo(local_alt_prev + _local_origin_alt);
if (PX4_ISFINITE(gps_alt_ref_prev) && isLocalVerticalPositionValid()) {
// determine current z
const float z_prev = _state.pos(2);
const float current_alt = -z_prev + gps_alt_ref_prev;
#if defined(CONFIG_EKF2_GNSS)
const float gps_hgt_bias = _gps_hgt_b_est.getBias();
#endif // CONFIG_EKF2_GNSS
resetVerticalPositionTo(_gps_alt_ref - current_alt);
ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev,
(double)_state.pos(2));
#if defined(CONFIG_EKF2_GNSS)
// adjust existing GPS height bias
_gps_hgt_b_est.setBias(gps_hgt_bias);
#endif // CONFIG_EKF2_GNSS
} else {
const float delta_origin_alt = altitude - _local_origin_alt;
_local_origin_alt = altitude;
updateVerticalPositionResetStatus(-delta_origin_alt);
#if defined(CONFIG_EKF2_TERRAIN)
updateTerrainResetStatus(-delta_origin_alt);
#endif // CONFIG_EKF2_TERRAIN
}
return true;
}
bool Ekf::setEkfGlobalOriginFromCurrentPos(const double latitude, const double longitude, const float altitude,
const float eph, const float epv)
bool Ekf::resetGlobalPositionTo(const double latitude, const double longitude, const float altitude,
const float hpos_var, const float vpos_var)
{
if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) {
if (!resetLatLonTo(latitude, longitude, hpos_var)) {
return false;
}
// altitude is optional
setAltOriginFromCurrentPos(altitude, epv);
resetAltitudeTo(altitude, vpos_var);
return true;
}
bool Ekf::setLatLonOriginFromCurrentPos(const double latitude, const double longitude, const float eph)
bool Ekf::resetLatLonTo(const double latitude, const double longitude, const float hpos_var)
{
if (!checkLatLonValidity(latitude, longitude)) {
return false;
}
_pos_ref.initReference(latitude, longitude, _time_delayed_us);
Vector2f pos_prev;
if (!_local_origin_lat_lon.isInitialized()) {
MapProjection zero_ref;
zero_ref.initReference(0.0, 0.0);
pos_prev = zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg());
_local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us);
// if we are already doing aiding, correct for the change in position since the EKF started navigating
if (isLocalHorizontalPositionValid()) {
double est_lat;
double est_lon;
_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
_pos_ref.initReference(est_lat, est_lon, _time_delayed_us);
_local_origin_lat_lon.reproject(-pos_prev(0), -pos_prev(1), est_lat, est_lon);
_local_origin_lat_lon.initReference(est_lat, est_lon, _time_delayed_us);
}
if (PX4_ISFINITE(eph) && (eph >= 0.f)) {
_gpos_origin_eph = eph;
ECL_INFO("Origin set to lat=%.6f, lon=%.6f",
_local_origin_lat_lon.getProjectionReferenceLat(), _local_origin_lat_lon.getProjectionReferenceLon());
} else {
pos_prev = _local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg());
}
_gpos.setLatLonDeg(latitude, longitude);
_output_predictor.resetLatLonTo(latitude, longitude);
const Vector2f delta_horz_pos = getLocalHorizontalPosition() - pos_prev;
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - delta_horz_pos);
#endif // CONFIG_EKF2_EXTERNAL_VISION
updateHorizontalPositionResetStatus(delta_horz_pos);
if (PX4_ISFINITE(hpos_var)) {
P.uncorrelateCovarianceSetVariance<2>(State::pos.idx, math::max(sq(0.01f), hpos_var));
}
// Reset the timout timer
_time_last_hor_pos_fuse = _time_delayed_us;
return true;
}
bool Ekf::setAltOriginFromCurrentPos(const float altitude, const float epv)
bool Ekf::resetAltitudeTo(const float altitude, const float vpos_var)
{
if (!checkAltitudeValidity(altitude)) {
return false;
}
_gps_alt_ref = altitude + _state.pos(2);
if (!PX4_ISFINITE(_local_origin_alt)) {
const float local_alt_prev = _gpos.altitude();
if (PX4_ISFINITE(epv) && (epv >= 0.f)) {
_gpos_origin_epv = epv;
if (isLocalVerticalPositionValid()) {
_local_origin_alt = altitude - local_alt_prev;
} else {
_local_origin_alt = altitude;
}
ECL_INFO("Origin alt=%.3f", (double)_local_origin_alt);
}
resetHeightTo(altitude, vpos_var);
return true;
}
void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
{
float eph = INFINITY;
float epv = INFINITY;
if (global_origin_valid()) {
// report absolute accuracy taking into account the uncertainty in location of the origin
eph = sqrtf(P.trace<2>(State::pos.idx + 0) + sq(_gpos_origin_eph));
epv = sqrtf(P.trace<1>(State::pos.idx + 2) + sq(_gpos_origin_epv));
get_ekf_lpos_accuracy(ekf_eph, ekf_epv);
if (_horizontal_deadreckon_time_exceeded) {
float lpos_eph = 0.f;
float lpos_epv = 0.f;
get_ekf_lpos_accuracy(&lpos_eph, &lpos_epv);
eph = math::max(eph, lpos_eph);
epv = math::max(epv, lpos_epv);
} else {
*ekf_eph = INFINITY;
*ekf_epv = INFINITY;
}
}
*ekf_eph = eph;
*ekf_epv = epv;
}
void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
@ -726,7 +740,13 @@ void Ekf::fuse(const VectorState &K, float innovation)
_state.vel = matrix::constrain(_state.vel - K.slice<State::vel.dof, 1>(State::vel.idx, 0) * innovation, -1.e3f, 1.e3f);
// pos
_state.pos = matrix::constrain(_state.pos - K.slice<State::pos.dof, 1>(State::pos.idx, 0) * innovation, -1.e6f, 1.e6f);
const Vector3f pos_correction = K.slice<State::pos.dof, 1>(State::pos.idx, 0) * (-innovation);
// Accumulate position in global coordinates
_gpos += pos_correction;
_state.pos.zero();
// Also store altitude in the state vector as this is used for optical flow fusion
_state.pos(2) = -_gpos.altitude();
// gyro_bias
_state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx,

34
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -42,6 +42,7 @@
#ifndef EKF_ESTIMATOR_INTERFACE_H
#define EKF_ESTIMATOR_INTERFACE_H
#include "lat_lon_alt/lat_lon_alt.hpp"
#if defined(MODULE_NAME)
#include <px4_platform_common/log.h>
# define ECL_INFO PX4_DEBUG
@ -241,7 +242,26 @@ public:
Vector3f getVelocity() const { return _output_predictor.getVelocity(); }
const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); }
float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); }
Vector3f getPosition() const { return _output_predictor.getPosition(); }
Vector3f getPosition() const
{
LatLonAlt lla = _output_predictor.getLatLonAlt();
float x;
float y;
if (_local_origin_lat_lon.isInitialized()) {
_local_origin_lat_lon.project(lla.latitude_deg(), lla.longitude_deg(), x, y);
} else {
MapProjection zero_ref;
zero_ref.initReference(0.0, 0.0);
zero_ref.project(lla.latitude_deg(), lla.longitude_deg(), x, y);
}
const float z = -(lla.altitude() - getEkfGlobalOriginAltitude());
return Vector3f(x, y, z);
}
LatLonAlt getLatLonAlt() const { return _output_predictor.getLatLonAlt(); }
const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); }
#if defined(CONFIG_EKF2_MAGNETOMETER)
@ -307,9 +327,9 @@ public:
const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); }
const uint64_t &time_delayed_us() const { return _time_delayed_us; }
bool global_origin_valid() const { return _pos_ref.isInitialized(); }
const MapProjection &global_origin() const { return _pos_ref; }
float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; }
bool global_origin_valid() const { return _local_origin_lat_lon.isInitialized(); }
const MapProjection &global_origin() const { return _local_origin_lat_lon; }
float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_local_origin_alt) ? _local_origin_alt : 0.f; }
OutputPredictor &output_predictor() { return _output_predictor; };
@ -379,10 +399,8 @@ protected:
bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized
// Variables used to publish the WGS-84 location of the EKF local NED origin
MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin
float _gps_alt_ref{NAN}; ///< WGS-84 height (m)
float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin
float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin
MapProjection _local_origin_lat_lon{};
float _local_origin_alt{NAN};
#if defined(CONFIG_EKF2_GNSS)
RingBuffer<gnssSample> *_gps_buffer {nullptr};

1
src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt Normal file
View File

@ -0,0 +1 @@
px4_add_unit_gtest(SRC test_lat_lon_alt.cpp LINKLIBS geo)

154
src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp Normal file
View File

@ -0,0 +1,154 @@
/****************************************************************************
*
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "mathlib/math/Limits.hpp"
#include <matrix/math.hpp>
class LatLonAlt
{
public:
LatLonAlt() = default;
LatLonAlt(const LatLonAlt &lla)
{
_latitude_rad = lla.latitude_rad();
_longitude_rad = lla.longitude_rad();
_altitude = lla.altitude();
}
LatLonAlt(const double latitude_deg, const double longitude_deg, const float altitude_m)
{
_latitude_rad = math::radians(latitude_deg);
_longitude_rad = math::radians(longitude_deg);
_altitude = altitude_m;
}
void setZero() { _latitude_rad = 0.0; _longitude_rad = 0.0; _altitude = 0.f; }
double latitude_deg() const { return math::degrees(latitude_rad()); }
double longitude_deg() const { return math::degrees(longitude_rad()); }
const double &latitude_rad() const { return _latitude_rad; }
const double &longitude_rad() const { return _longitude_rad; }
float altitude() const { return _altitude; }
void setLatitudeDeg(const double &latitude_deg) { _latitude_rad = math::radians(latitude_deg); }
void setLongitudeDeg(const double &longitude_deg) { _longitude_rad = math::radians(longitude_deg); }
void setAltitude(const float altitude) { _altitude = altitude; }
void setLatLon(const LatLonAlt &lla) { _latitude_rad = lla.latitude_rad(); _longitude_rad = lla.longitude_rad(); }
void setLatLonDeg(const double latitude, const double longitude) { _latitude_rad = math::radians(latitude); _longitude_rad = math::radians(longitude); }
void setLatLonRad(const double latitude, const double longitude) { _latitude_rad = latitude; _longitude_rad = longitude; }
void print() const { printf("latitude = %f (deg), longitude = %f (deg), altitude = %f (m)\n", _latitude_rad, _longitude_rad, (double)_altitude); }
void operator+=(const matrix::Vector3f &delta_pos)
{
matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude);
_latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast<double>(delta_pos(0)) / d_lat_lon_to_d_xy(0));
_longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast<double>(delta_pos(1)) / d_lat_lon_to_d_xy(1));
_altitude -= delta_pos(2);
}
void operator+=(const matrix::Vector2f &delta_pos)
{
matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude);
_latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast<double>(delta_pos(0)) / d_lat_lon_to_d_xy(0));
_longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast<double>(delta_pos(1)) / d_lat_lon_to_d_xy(1));
}
LatLonAlt operator+(const matrix::Vector3f &delta_pos) const
{
matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(latitude_rad(), altitude());
const double latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast<double>(delta_pos(0)) / d_lat_lon_to_d_xy(0));
const double longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast<double>(delta_pos(1)) / d_lat_lon_to_d_xy(1));
const float altitude = _altitude - delta_pos(2);
LatLonAlt lla_new;
lla_new.setLatLonRad(latitude_rad, longitude_rad);
lla_new.setAltitude(altitude);
return lla_new;
}
void operator=(const LatLonAlt &lla)
{
_latitude_rad = lla.latitude_rad();
_longitude_rad = lla.longitude_rad();
_altitude = lla.altitude();
}
matrix::Vector3f operator-(const LatLonAlt &lla) const
{
const double delta_lat = matrix::wrap_pi(_latitude_rad - lla.latitude_rad());
const double delta_lon = matrix::wrap_pi(_longitude_rad - lla.longitude_rad());
const float delta_alt = _altitude - lla.altitude();
const matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude);
return matrix::Vector3f(static_cast<float>(delta_lat * d_lat_lon_to_d_xy(0)),
static_cast<float>(delta_lon * d_lat_lon_to_d_xy(1)),
-delta_alt);
}
private:
// Convert between curvilinear and cartesian errors
static matrix::Vector2d deltaLatLonToDeltaXY(const double latitude, const float altitude)
{
double r_n;
double r_e;
computeRadiiOfCurvature(latitude, r_n, r_e);
const double dn_dlat = r_n + static_cast<double>(altitude);
const double de_dlon = (r_e + static_cast<double>(altitude)) * cos(latitude);
return matrix::Vector2d(dn_dlat, de_dlon);
}
static void computeRadiiOfCurvature(const double latitude, double &meridian_radius_of_curvature,
double &transverse_radius_of_curvature)
{
const double tmp = 1.0 - pow(Wgs84::eccentricity * sin(latitude), 2);
const double sqrt_tmp = std::sqrt(tmp);
meridian_radius_of_curvature = Wgs84::meridian_radius_of_curvature_numerator / (tmp * tmp * sqrt_tmp);
transverse_radius_of_curvature = Wgs84::equatorial_radius / sqrt_tmp;
}
struct Wgs84 {
static constexpr double equatorial_radius = 6378137.0;
static constexpr double eccentricity = 0.0818191908425;
static constexpr double meridian_radius_of_curvature_numerator = equatorial_radius * (1.0 - eccentricity *eccentricity);
};
double _latitude_rad{0.0};
double _longitude_rad{0.0};
float _altitude{0.0};
};

107
src/modules/ekf2/EKF/lat_lon_alt/test_lat_lon_alt.cpp Normal file
View File

@ -0,0 +1,107 @@
/****************************************************************************
*
* Copyright (C) 2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <gtest/gtest.h>
#include <matrix/math.hpp>
#include <lib/geo/geo.h>
#include "lat_lon_alt.hpp"
using namespace matrix;
using math::radians;
using math::degrees;
TEST(TestLatLonAlt, init)
{
LatLonAlt lla(5.7, -2.3, 420);
ASSERT_FLOAT_EQ(lla.latitude_deg(), 5.7);
ASSERT_FLOAT_EQ(lla.longitude_deg(), -2.3);
ASSERT_EQ(lla.altitude(), 420);
}
TEST(TestLatLonAlt, set)
{
LatLonAlt lla(0.0, 0.0, 0);
ASSERT_EQ(lla.latitude_rad(), 0.0);
ASSERT_EQ(lla.longitude_rad(), 0.0);
ASSERT_EQ(lla.altitude(), 0);
lla.setLatLonRad(0.1, -0.5);
lla.setAltitude(420);
ASSERT_EQ(lla.latitude_rad(), 0.1);
ASSERT_EQ(lla.longitude_rad(), -0.5);
ASSERT_EQ(lla.altitude(), 420);
}
TEST(TestLatLonAlt, copy)
{
LatLonAlt lla(-0.8, -0.1, 500);
LatLonAlt lla_copy = lla;
ASSERT_EQ(lla_copy.latitude_deg(), -0.8);
ASSERT_EQ(lla_copy.longitude_deg(), -0.1);
ASSERT_EQ(lla_copy.altitude(), 500);
}
TEST(TestLatLonAlt, addDeltaPos)
{
MapProjection pos_ref(60.0, 5.0);
LatLonAlt lla(pos_ref.getProjectionReferenceLat(), pos_ref.getProjectionReferenceLon(), 400.f);
Vector3f delta_pos(5.f, -2.f, 3.f);
lla += delta_pos;
double lat_new, lon_new;
pos_ref.reproject(delta_pos(0), delta_pos(1), lat_new, lon_new);
EXPECT_NEAR(lla.latitude_deg(), lat_new, 1e-6);
EXPECT_NEAR(lla.longitude_deg(), lon_new, 1e-6);
EXPECT_EQ(lla.altitude(), 397.f);
}
TEST(TestLatLonAlt, subLatLonAlt)
{
MapProjection pos_ref(60.0, 5.0);
LatLonAlt lla(pos_ref.getProjectionReferenceLat(), pos_ref.getProjectionReferenceLon(), 0.f);
const Vector3f delta_pos_true(1.f, -2.f, 3.f);
double lat_new, lon_new;
pos_ref.reproject(delta_pos_true(0), delta_pos_true(1), lat_new, lon_new);
LatLonAlt lla_new(lat_new, lon_new, -3.f);
Vector3f delta_pos = lla_new - lla;
EXPECT_NEAR(delta_pos(0), delta_pos_true(0), 1e-2);
EXPECT_NEAR(delta_pos(1), delta_pos_true(1), 1e-2);
EXPECT_EQ(delta_pos(2), delta_pos_true(2));
}

49
src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
View File

@ -69,7 +69,7 @@ void OutputPredictor::print_status()
_output_vert_buffer.entries(), _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size());
}
void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state)
void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const LatLonAlt &gpos_state)
{
const outputSample &output_delayed = _output_buffer.get_oldest();
@ -77,9 +77,12 @@ void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f
Quatf q_delta{quat_state * output_delayed.quat_nominal.inversed()};
q_delta.normalize();
// calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon
// calculate the velocity delta between the output and EKF at the EKF fusion time horizon
const Vector3f vel_delta = vel_state - output_delayed.vel;
const Vector3f pos_delta = pos_state - output_delayed.pos;
// zero the position error at delayed time and reset the global reference
const Vector3f pos_delta = -output_delayed.pos;
_global_ref = gpos_state;
// loop through the output filter state history and add the deltas
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
@ -156,29 +159,15 @@ void OutputPredictor::resetVerticalVelocityTo(float delta_vert_vel)
_output_vert_new.vert_vel += delta_vert_vel;
}
void OutputPredictor::resetHorizontalPositionTo(const Vector2f &delta_horz_pos)
void OutputPredictor::resetLatLonTo(const double &new_latitude, const double &new_longitude)
{
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
_output_buffer[index].pos.xy() += delta_horz_pos;
}
_output_new.pos.xy() += delta_horz_pos;
_global_ref.setLatitudeDeg(new_latitude);
_global_ref.setLongitudeDeg(new_longitude);
}
void OutputPredictor::resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change)
void OutputPredictor::resetAltitudeTo(const float new_altitude, const float vert_pos_change)
{
// apply the change in height / height rate to our newest height / height rate estimate
// which have already been taken out from the output buffer
_output_new.pos(2) += vert_pos_change;
// add the reset amount to the output observer buffered data
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
_output_buffer[i].pos(2) += vert_pos_change;
_output_vert_buffer[i].vert_vel_integ += vert_pos_change;
}
// add the reset amount to the output observer vertical position state
_output_vert_new.vert_vel_integ = new_vert_pos;
_global_ref.setAltitude(new_altitude);
}
void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector3f &delta_angle,
@ -261,7 +250,7 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector
}
void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,
const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, const matrix::Vector3f &gyro_bias,
const Quatf &quat_state, const Vector3f &vel_state, const LatLonAlt &gpos_state, const matrix::Vector3f &gyro_bias,
const matrix::Vector3f &accel_bias)
{
// calculate an average filter update time
@ -317,6 +306,8 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,
const float vel_gain = _dt_correct_states_avg / math::constrain(_vel_tau, _dt_correct_states_avg, 10.f);
const float pos_gain = _dt_correct_states_avg / math::constrain(_pos_tau, _dt_correct_states_avg, 10.f);
const Vector3f pos_state = gpos_state - _global_ref;
// calculate down velocity and position tracking errors
const float vert_vel_err = (vel_state(2) - output_vert_delayed.vert_vel);
const float vert_vel_integ_err = (pos_state(2) - output_vert_delayed.vert_vel_integ);
@ -325,7 +316,7 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,
// using a PD feedback tuned to a 5% overshoot
const float vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * vel_gain * 1.1f;
applyCorrectionToVerticalOutputBuffer(vert_vel_correction);
applyCorrectionToVerticalOutputBuffer(vert_vel_correction, pos_state(2));
// calculate velocity and position tracking errors
const Vector3f vel_err(vel_state - output_delayed.vel);
@ -342,10 +333,14 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,
_pos_err_integ += pos_err;
const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
applyCorrectionToOutputBuffer(vel_correction, pos_correction);
// as the reference changes, adjust the position correction to keep a constant global position
const Vector3f pos_correction_with_ref_change = pos_correction - pos_state;
applyCorrectionToOutputBuffer(vel_correction, pos_correction_with_ref_change);
_global_ref = gpos_state;
}
void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_correction)
void OutputPredictor::applyCorrectionToVerticalOutputBuffer(const float vert_vel_correction, const float pos_ref_change)
{
// loop through the vertical output filter state history starting at the oldest and apply the corrections to the
// vert_vel states and propagate vert_vel_integ forward using the corrected vert_vel
@ -367,7 +362,7 @@ void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_corre
// position is propagated forward using the corrected velocity and a trapezoidal integrator
next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f *
next_state.dt;
next_state.dt - pos_ref_change;
// advance the index
index = (index + 1) % size;

18
src/modules/ekf2/EKF/output_predictor/output_predictor.h
View File

@ -37,6 +37,7 @@
#include <matrix/math.hpp>
#include "../RingBuffer.h"
#include "../lat_lon_alt/lat_lon_alt.hpp"
#include <lib/geo/geo.h>
@ -52,7 +53,7 @@ public:
// modify output filter to match the the EKF state at the fusion time horizon
void alignOutputFilter(const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state,
const matrix::Vector3f &pos_state);
const LatLonAlt &gpos_state);
/*
* Implement a strapdown INS algorithm using the latest IMU data at the current time horizon.
* Buffer the INS states and calculate the difference with the EKF states at the delayed fusion time horizon.
@ -67,7 +68,7 @@ public:
const matrix::Vector3f &delta_velocity, const float delta_velocity_dt);
void correctOutputStates(const uint64_t time_delayed_us,
const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state,
const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const LatLonAlt &gpos_state,
const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias);
void resetQuaternion(const matrix::Quatf &quat_change);
@ -75,8 +76,8 @@ public:
void resetHorizontalVelocityTo(const matrix::Vector2f &delta_horz_vel);
void resetVerticalVelocityTo(float delta_vert_vel);
void resetHorizontalPositionTo(const matrix::Vector2f &delta_horz_pos);
void resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change);
void resetLatLonTo(const double &new_latitude, const double &new_longitude);
void resetAltitudeTo(float new_altitude, float vert_pos_change);
void print_status();
@ -106,13 +107,12 @@ public:
// get the derivative of the vertical position of the body frame origin in local NED earth frame
float getVerticalPositionDerivative() const { return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); }
// get the position of the body frame origin in local earth frame
matrix::Vector3f getPosition() const
LatLonAlt getLatLonAlt() const
{
// rotate the position of the IMU relative to the boy origin into earth frame
const matrix::Vector3f pos_offset_earth{_R_to_earth_now * _imu_pos_body};
// subtract from the EKF position (which is at the IMU) to get position at the body origin
return _output_new.pos - pos_offset_earth;
return _global_ref + (_output_new.pos - pos_offset_earth);
}
// return an array containing the output predictor angular, velocity and position tracking
@ -133,7 +133,7 @@ private:
* This provides an alternative vertical velocity output that is closer to the first derivative
* of the position but does degrade tracking relative to the EKF state.
*/
void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction);
void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction, const float pos_ref_change);
/*
* Calculate corrections to be applied to vel and pos output state history.
@ -159,6 +159,8 @@ private:
float dt{0.f}; ///< delta time (sec)
};
LatLonAlt _global_ref{0.0, 0.0, 0.f};
RingBuffer<outputSample> _output_buffer{12};
RingBuffer<outputVert> _output_vert_buffer{12};

151
src/modules/ekf2/EKF/position_fusion.cpp
View File

@ -36,7 +36,7 @@
void Ekf::updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us,
const float observation, const float observation_variance, const float innovation_gate) const
{
float innovation = _state.pos(2) - observation;
float innovation = -_gpos.altitude() - observation;
float innovation_variance = getStateVariance<State::pos>()(2) + observation_variance;
updateAidSourceStatus(aid_src, time_us,
@ -93,10 +93,20 @@ bool Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
return aid_src.fused;
}
void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var)
void Ekf::resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude,
const Vector2f &new_horz_pos_var)
{
const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}};
_state.pos.xy() = new_horz_pos;
const Vector2f delta_horz_pos = computeDeltaHorizontalPosition(new_latitude, new_longitude);
updateHorizontalPositionResetStatus(delta_horz_pos);
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - delta_horz_pos);
#endif // CONFIG_EKF2_EXTERNAL_VISION
//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
_gpos.setLatLonDeg(new_latitude, new_longitude);
_output_predictor.resetLatLonTo(new_latitude, new_longitude);
if (PX4_ISFINITE(new_horz_pos_var(0))) {
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0)));
@ -106,54 +116,89 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1)));
}
_output_predictor.resetHorizontalPositionTo(delta_horz_pos);
// record the state change
if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
_state_reset_status.posNE_change = delta_horz_pos;
} else {
// there's already a reset this update, accumulate total delta
_state_reset_status.posNE_change += delta_horz_pos;
}
_state_reset_status.reset_count.posNE++;
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change);
#endif // CONFIG_EKF2_EXTERNAL_VISION
//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
// Reset the timout timer
_time_last_hor_pos_fuse = _time_delayed_us;
}
void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
Vector2f Ekf::computeDeltaHorizontalPosition(const double &new_latitude, const double &new_longitude) const
{
const float old_vert_pos = _state.pos(2);
_state.pos(2) = new_vert_pos;
Vector2f pos;
Vector2f pos_new;
if (_local_origin_lat_lon.isInitialized()) {
_local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg(), pos(0), pos(1));
_local_origin_lat_lon.project(new_latitude, new_longitude, pos_new(0), pos_new(1));
} else {
MapProjection zero_ref;
zero_ref.initReference(0.0, 0.0);
zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg(), pos(0), pos(1));
zero_ref.project(new_latitude, new_longitude, pos_new(0), pos_new(1));
}
return pos_new - pos;
}
Vector2f Ekf::getLocalHorizontalPosition() const
{
if (_local_origin_lat_lon.isInitialized()) {
return _local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg());
} else {
MapProjection zero_ref;
zero_ref.initReference(0.0, 0.0);
return zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg());
}
}
void Ekf::updateHorizontalPositionResetStatus(const Vector2f &delta)
{
if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
_state_reset_status.posNE_change = delta;
} else {
// there's already a reset this update, accumulate total delta
_state_reset_status.posNE_change += delta;
}
_state_reset_status.reset_count.posNE++;
}
void Ekf::resetHorizontalPositionTo(const Vector2f &new_pos,
const Vector2f &new_horz_pos_var)
{
double new_latitude;
double new_longitude;
if (_local_origin_lat_lon.isInitialized()) {
_local_origin_lat_lon.reproject(new_pos(0), new_pos(1), new_latitude, new_longitude);
} else {
MapProjection zero_ref;
zero_ref.initReference(0.0, 0.0);
zero_ref.reproject(new_pos(0), new_pos(1), new_latitude, new_longitude);
}
resetHorizontalPositionTo(new_latitude, new_longitude, new_horz_pos_var);
}
void Ekf::resetHeightTo(const float new_altitude, float new_vert_pos_var)
{
const float old_altitude = _gpos.altitude();
_gpos.setAltitude(new_altitude);
if (PX4_ISFINITE(new_vert_pos_var)) {
// the state variance is the same as the observation
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var));
}
const float delta_z = new_vert_pos - old_vert_pos;
const float delta_z = -(new_altitude - old_altitude);
// apply the change in height / height rate to our newest height / height rate estimate
// which have already been taken out from the output buffer
_output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z);
_output_predictor.resetAltitudeTo(new_altitude, delta_z);
// record the state change
if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
_state_reset_status.posD_change = delta_z;
} else {
// there's already a reset this update, accumulate total delta
_state_reset_status.posD_change += delta_z;
}
_state_reset_status.reset_count.posD++;
updateVerticalPositionResetStatus(delta_z);
#if defined(CONFIG_EKF2_BAROMETER)
_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
@ -166,9 +211,29 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_TERRAIN)
updateTerrainResetStatus(delta_z);
_state.terrain += delta_z;
#endif // CONFIG_EKF2_TERRAIN
// record the state change
// Reset the timout timer
_time_last_hgt_fuse = _time_delayed_us;
}
void Ekf::updateVerticalPositionResetStatus(const float delta_z)
{
if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
_state_reset_status.posD_change = delta_z;
} else {
// there's already a reset this update, accumulate total delta
_state_reset_status.posD_change += delta_z;
}
_state_reset_status.reset_count.posD++;
}
void Ekf::updateTerrainResetStatus(const float delta_z)
{
if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
_state_reset_status.hagl_change = delta_z;
@ -178,17 +243,15 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
}
_state_reset_status.reset_count.hagl++;
#endif // CONFIG_EKF2_TERRAIN
// Reset the timout timer
_time_last_hgt_fuse = _time_delayed_us;
}
void Ekf::resetHorizontalPositionToLastKnown()
{
ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_gpos.latitude_deg(),
(double)_last_known_gpos.longitude_deg());
_information_events.flags.reset_pos_to_last_known = true;
// Used when falling back to non-aiding mode of operation
resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise));
resetHorizontalPositionTo(_last_known_gpos.latitude_deg(), _last_known_gpos.longitude_deg(),
sq(_params.pos_noaid_noise));
}

4
src/modules/ekf2/EKF/terrain_control.cpp
View File

@ -43,7 +43,7 @@
void Ekf::initTerrain()
{
// assume a ground clearance
_state.terrain = _state.pos(2) + _params.rng_gnd_clearance;
_state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance;
// use the ground clearance value as our uncertainty
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.rng_gnd_clearance));
@ -53,7 +53,7 @@ void Ekf::controlTerrainFakeFusion()
{
// If we are on ground, store the local position and time to use as a reference
if (!_control_status.flags.in_air) {
_last_on_ground_posD = _state.pos(2);
_last_on_ground_posD = -_gpos.altitude();
_control_status.flags.rng_fault = false;
} else if (!_control_status_prev.flags.in_air) {

10
src/modules/ekf2/EKF2.cpp
View File

@ -1166,17 +1166,17 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
{
if (_ekf.global_origin_valid() && _ekf.control_status().flags.yaw_align) {
const Vector3f position{_ekf.getPosition()};
// generate and publish global position data
vehicle_global_position_s global_pos{};
global_pos.timestamp_sample = timestamp;
// Position of local NED origin in GPS / WGS84 frame
_ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon);
// Position GPS / WGS84 frame
const LatLonAlt lla = _ekf.getLatLonAlt();
global_pos.lat = lla.latitude_deg();
global_pos.lon = lla.longitude_deg();
global_pos.lat_lon_valid = _ekf.isGlobalHorizontalPositionValid();
global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters
global_pos.alt = lla.altitude();
global_pos.alt_valid = _ekf.isGlobalVerticalPositionValid();
#if defined(CONFIG_EKF2_GNSS)

19
src/modules/ekf2/test/test_EKF_airspeed.cpp
View File

@ -180,15 +180,31 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning)
_ekf->set_vehicle_at_rest(false);
_ekf->set_is_fixed_wing(true);
const Vector3f pos_prev = _ekf->getPosition();
const double latitude_new = -15.0000005;
const double longitude_new = -115.0000005;
const float altitude_new = 1500.0;
const float eph = 50.f;
const float epv = 10.f;
_ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new);
_ekf->resetGlobalPosToExternalObservation(latitude_new, longitude_new, altitude_new, eph, epv, 0);
const Vector3f pos = _ekf->getPosition();
// lat/lon is initialized so the local horizontal position remains constant
EXPECT_NEAR(pos(0), pos_prev(0), 1e-3f);
EXPECT_NEAR(pos(1), pos_prev(1), 1e-3f);
// alt is updated as the local altitude origin was already set
EXPECT_NEAR(pos(2), pos_prev(2) - (altitude_new - _ekf->getEkfGlobalOriginAltitude()), 1e-3f);
const LatLonAlt lla = _ekf->getLatLonAlt();
EXPECT_NEAR(lla.latitude_deg(), latitude_new, 1e-6f);
EXPECT_NEAR(lla.longitude_deg(), longitude_new, 1e-6f);
EXPECT_NEAR(lla.altitude(), altitude_new, 1e-3f);
// Simulate the fact that the sideslip can start immediately, without
// waiting for a measurement sample.
_ekf_wrapper.enableBetaFusion();
@ -227,7 +243,6 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset)
const float eph = 50.f;
const float epv = 1.f;
_ekf->setEkfGlobalOrigin(latitude, longitude, altitude);
_ekf->resetGlobalPosToExternalObservation(latitude, longitude, altitude, eph, epv, 0);
_ekf_wrapper.enableBetaFusion();

15
src/modules/ekf2/test/test_EKF_basics.cpp
View File

@ -219,19 +219,22 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized)
_altitude_new = 100.0;
_sensor_simulator.startGps();
_ekf->set_min_required_gps_health_time(1e6);
_sensor_simulator.runSeconds(1);
_ekf_wrapper.enableGpsHeightFusion();
_sensor_simulator.setGpsLatitude(_latitude_new);
_sensor_simulator.setGpsLongitude(_longitude_new);
_sensor_simulator.setGpsAltitude(_altitude_new);
_ekf->set_min_required_gps_health_time(1e6);
_sensor_simulator.runSeconds(1);
_sensor_simulator.runSeconds(5);
_ekf->getEkfGlobalOrigin(_origin_time, _latitude, _longitude, _altitude);
EXPECT_DOUBLE_EQ(_latitude, _latitude_new);
EXPECT_DOUBLE_EQ(_longitude, _longitude_new);
EXPECT_NEAR(_altitude, _altitude_new, 0.01f);
// In baro height ref the origin is set using baro data and not GNSS altitude
EXPECT_NEAR(_altitude, _sensor_simulator._baro.getData(), 0.01f);
// Note: we cannot reset too far since the local position is limited to 1e6m
_latitude_new = 14.0000005;
@ -261,11 +264,13 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized)
TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
{
_ekf->getEkfGlobalOrigin(_origin_time, _latitude_new, _longitude_new, _altitude_new);
_ekf->getEkfGlobalOrigin(_origin_time, _latitude, _longitude, _altitude);
EXPECT_DOUBLE_EQ(_latitude, _latitude_new);
EXPECT_DOUBLE_EQ(_longitude, _longitude_new);
EXPECT_FLOAT_EQ(_altitude, _altitude_new);
// In baro height ref the origin is set using baro data and not GNSS altitude
EXPECT_NEAR(_altitude, _sensor_simulator._baro.getData(), 0.01f);
EXPECT_FALSE(_ekf->global_origin_valid());

46
src/modules/ekf2/test/test_EKF_flow.cpp
View File

@ -345,3 +345,49 @@ TEST_F(EkfFlowTest, yawMotionNoMagFusion)
_ekf->state().vector().print();
_ekf->covariances().print();
}
TEST_F(EkfFlowTest, deadReckoning)
{
ResetLoggingChecker reset_logging_checker(_ekf);
// WHEN: simulate being 5m above ground
const float simulated_distance_to_ground = 5.f;
_sensor_simulator._trajectory[2].setCurrentPosition(-simulated_distance_to_ground);
startRangeFinderFusion(simulated_distance_to_ground);
_ekf->set_in_air_status(true);
_ekf->set_vehicle_at_rest(false);
// WHEN: moving a couple of meters while doing flow dead_reckoning
Vector3f simulated_velocity(0.5f, -0.2f, 0.f);
_sensor_simulator._trajectory[0].setCurrentVelocity(simulated_velocity(0));
_sensor_simulator._trajectory[1].setCurrentVelocity(simulated_velocity(1));
_sensor_simulator.setTrajectoryTargetVelocity(simulated_velocity);
_ekf_wrapper.enableFlowFusion();
_sensor_simulator.startFlow();
_sensor_simulator.runTrajectorySeconds(5.f);
simulated_velocity = Vector3f(0.f, 0.f, 0.f);
_sensor_simulator.setTrajectoryTargetVelocity(simulated_velocity);
_sensor_simulator.runTrajectorySeconds(_sensor_simulator._trajectory[0].getTotalTime());
EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
const Vector3f lpos_before_reset = _ekf->getPosition();
const float altitude_ref_prev = _ekf->getEkfGlobalOriginAltitude();
const double latitude_new = -15.0000005;
const double longitude_new = -115.0000005;
const float altitude_new = 1500.0;
const float eph = 50.f;
const float epv = 10.f;
_ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new, eph, epv);
const Vector3f lpos_after_reset = _ekf->getPosition();
EXPECT_NEAR(lpos_after_reset(0), lpos_before_reset(0), 1e-3);
EXPECT_NEAR(lpos_after_reset(1), lpos_before_reset(1), 1e-3);
EXPECT_NEAR(lpos_after_reset(2), lpos_before_reset(2) + (altitude_new - altitude_ref_prev), 1e-3);
}

2
src/modules/ekf2/test/test_EKF_gps.cpp
View File

@ -164,7 +164,7 @@ TEST_F(EkfGpsTest, resetToGpsPosition)
// AND: simulate jump in position
_sensor_simulator.startGps();
const Vector3f simulated_position_change(2.0f, -1.0f, 0.f);
const Vector3f simulated_position_change(20.0f, -1.0f, 0.f);
_sensor_simulator._gps.stepHorizontalPositionByMeters(
Vector2f(simulated_position_change));
_sensor_simulator.runSeconds(6);

19
src/modules/ekf2/test/test_EKF_height_fusion.cpp
View File

@ -123,7 +123,7 @@ TEST_F(EkfHeightFusionTest, baroRef)
/* EXPECT_EQ(status.bias, _sensor_simulator._baro.getData()); */ // This is the real bias, but the estimator isn't running so the status isn't updated
EXPECT_EQ(baro_status.bias, 0.f);
const BiasEstimator::status &gps_status = _ekf->getGpsHgtBiasEstimatorStatus();
EXPECT_NEAR(gps_status.bias, -baro_increment, 0.2f);
EXPECT_NEAR(gps_status.bias, _sensor_simulator._gps.getData().alt - _sensor_simulator._baro.getData(), 0.2f);
const float terrain = _ekf->getTerrainVertPos();
EXPECT_NEAR(terrain, -baro_increment, 1.2f);
@ -170,8 +170,13 @@ TEST_F(EkfHeightFusionTest, gpsRef)
EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeightFusion());
// AND WHEN: the baro data increases
const float baro_initial = _sensor_simulator._baro.getData();
const BiasEstimator::status &baro_status_initial = _ekf->getBaroBiasEstimatorStatus();
const float baro_rel_initial = baro_initial - _sensor_simulator._gps.getData().alt;
EXPECT_NEAR(baro_status_initial.bias, baro_rel_initial, 0.6f);
// AND WHEN: the baro data increases
const float baro_increment = 5.f;
_sensor_simulator._baro.setData(baro_initial + baro_increment);
_sensor_simulator.runSeconds(100);
@ -180,7 +185,7 @@ TEST_F(EkfHeightFusionTest, gpsRef)
// the GPS height value and the baro gets its bias estimated
EXPECT_NEAR(_ekf->getPosition()(2), 0.f, 1.f);
const BiasEstimator::status &baro_status = _ekf->getBaroBiasEstimatorStatus();
EXPECT_NEAR(baro_status.bias, baro_initial + baro_increment, 1.3f);
EXPECT_NEAR(baro_status.bias, baro_rel_initial + baro_increment, 1.3f);
const float terrain = _ekf->getTerrainVertPos();
EXPECT_NEAR(terrain, 0.f, 1.1f); // TODO: why?
@ -196,7 +201,7 @@ TEST_F(EkfHeightFusionTest, gpsRef)
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion());
EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_initial + baro_increment - gps_step, 0.2f);
EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_rel_initial + baro_increment - gps_step, 0.2f);
// and the innovations are close to zero
EXPECT_NEAR(_ekf->aid_src_baro_hgt().innovation, 0.f, 0.2f);
@ -367,6 +372,7 @@ TEST_F(EkfHeightFusionTest, changeEkfOriginAlt)
ResetLoggingChecker reset_logging_checker(_ekf);
reset_logging_checker.capturePreResetState();
const float baro_bias_prev = _ekf->getBaroBiasEstimatorStatus().bias;
const float alt_increment = 4478.f;
_ekf->setEkfGlobalOrigin(lat, lon, alt + alt_increment);
@ -376,9 +382,12 @@ TEST_F(EkfHeightFusionTest, changeEkfOriginAlt)
EXPECT_NEAR(_ekf->getPosition()(2), alt_increment, 1.f);
reset_logging_checker.capturePostResetState();
EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, _sensor_simulator._baro.getData() + alt_increment, 0.2f);
// An origin reset doesn't change the baro bias as it is relative to the height reference (GNSS)
EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_bias_prev, 0.3f);
EXPECT_NEAR(_ekf->getTerrainVertPos(), alt_increment, 1.f);
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0));
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
}

13
src/modules/ekf2/test/test_EKF_yaw_estimator.cpp
View File

@ -40,7 +40,6 @@
#include "EKF/ekf.h"
#include "sensor_simulator/sensor_simulator.h"
#include "sensor_simulator/ekf_wrapper.h"
#include "test_helper/reset_logging_checker.h"
class EKFYawEstimatorTest : public ::testing::Test
{
@ -91,8 +90,6 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment)
// WHEN: the vehicle starts accelerating in the horizontal plane
_sensor_simulator.setTrajectoryTargetVelocity(Vector3f(2.f, -2.f, -1.f));
_ekf->set_in_air_status(true);
ResetLoggingChecker reset_logging_checker(_ekf);
reset_logging_checker.capturePreResetState();
_sensor_simulator.runTrajectorySeconds(3.f);
@ -106,10 +103,12 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment)
EXPECT_NEAR(yaw_est, yaw, tolerance_rad);
EXPECT_LT(yaw_est_var, tolerance_rad);
// 1 reset when starting GNSS aiding
reset_logging_checker.capturePostResetState();
EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1));
EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(1));
EXPECT_LT(_ekf->aid_src_gnss_pos().test_ratio[0], 0.5f);
EXPECT_LT(_ekf->aid_src_gnss_pos().test_ratio[1], 0.5f);
EXPECT_LT(_ekf->aid_src_gnss_vel().test_ratio[0], 0.1f);
EXPECT_LT(_ekf->aid_src_gnss_vel().test_ratio[1], 0.1f);
EXPECT_LT(_ekf->aid_src_gnss_vel().test_ratio[2], 0.1f);
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid());