From c29b4ff87ed9e68990975687ab93ae226db2a1df Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 10 Jul 2024 10:40:29 -0400 Subject: [PATCH] ekf2: apply astyle formatting and enforce --- Tools/astyle/files_to_check_code_style.sh | 3 +- .../aid_sources/airspeed/airspeed_fusion.cpp | 15 +- .../aux_global_position.cpp | 12 +- .../aux_global_position.hpp | 4 +- .../EKF/aid_sources/auxvel/auxvel_fusion.cpp | 12 +- .../ekf2/EKF/aid_sources/drag/drag_fusion.cpp | 12 +- .../external_vision/ev_control.cpp | 6 +- .../external_vision/ev_height_control.cpp | 2 + .../external_vision/ev_pos_control.cpp | 18 +- .../external_vision/ev_yaw_control.cpp | 14 +- .../ekf2/EKF/aid_sources/fake_pos_control.cpp | 18 +- .../ekf2/EKF/aid_sources/gnss/gps_checks.cpp | 4 +- .../ekf2/EKF/aid_sources/gnss/gps_control.cpp | 24 +-- .../EKF/aid_sources/gnss/gps_yaw_fusion.cpp | 12 +- .../aid_sources/gravity/gravity_fusion.cpp | 24 +-- .../aid_sources/magnetometer/mag_control.cpp | 47 ++--- .../aid_sources/magnetometer/mag_fusion.cpp | 12 +- .../optical_flow/optical_flow_fusion.cpp | 3 +- .../range_finder/range_height_control.cpp | 18 +- .../range_finder/sensor_range_finder.hpp | 3 +- .../aid_sources/sideslip/sideslip_fusion.cpp | 12 +- .../zero_innovation_heading_update.cpp | 5 +- src/modules/ekf2/EKF/common.h | 163 +++++++++++------- src/modules/ekf2/EKF/covariance.cpp | 18 +- src/modules/ekf2/EKF/ekf.cpp | 37 ++-- src/modules/ekf2/EKF/ekf.h | 73 +++++--- src/modules/ekf2/EKF/ekf_helper.cpp | 148 +++++++++++----- src/modules/ekf2/EKF/estimator_interface.cpp | 32 ++-- src/modules/ekf2/EKF/estimator_interface.h | 26 +-- src/modules/ekf2/EKF/height_control.cpp | 8 + .../EKF/output_predictor/output_predictor.cpp | 6 +- .../EKF/output_predictor/output_predictor.h | 3 +- src/modules/ekf2/EKF/position_fusion.cpp | 15 +- src/modules/ekf2/EKF/velocity_fusion.cpp | 15 +- .../ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp | 3 +- src/modules/ekf2/EKF/yaw_fusion.cpp | 2 + 36 files changed, 527 insertions(+), 302 deletions(-) diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index 97a2d8c6fc86..c94132867ee1 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -18,7 +18,8 @@ exec find boards msg src platforms test \ -path src/lib/events/libevents -prune -o \ -path src/lib/parameters/uthash -prune -o \ -path src/lib/wind_estimator/python/generated -prune -o \ - -path src/modules/ekf2/EKF -prune -o \ + -path src/modules/ekf2/EKF/python/ekf_derivation/generated -prune -o \ + -path src/modules/ekf2/EKF/yaw_estimator/derivation/generated -prune -o \ -path src/modules/gyro_fft/CMSIS_5 -prune -o \ -path src/modules/mavlink/mavlink -prune -o \ -path test/mavsdk_tests/catch2 -prune -o \ diff --git a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp index 69895181b1ed..c9822d4829bf 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -89,7 +89,8 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) updateAirspeed(airspeed_sample, _aid_src_airspeed); - _innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag + _innov_check_fail_status.flags.reject_airspeed = + _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing @@ -155,12 +156,12 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so &innov, &innov_var); updateAidSourceStatus(aid_src, - airspeed_sample.time_us, // sample timestamp - airspeed_sample.true_airspeed, // observation - R, // observation variance - innov, // innovation - innov_var, // innovation variance - math::max(_params.tas_innov_gate, 1.f)); // innovation gate + airspeed_sample.time_us, // sample timestamp + airspeed_sample.true_airspeed, // observation + R, // observation variance + innov, // innovation + innov_var, // innovation variance + math::max(_params.tas_innov_gate, 1.f)); // innovation gate } void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index fe313315b597..6efd4fabe66b 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -85,12 +85,12 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed const Vector2f pos_obs_var(pos_var, pos_var); ekf.updateAidSourceStatus(aid_src, - sample.time_us, // sample timestamp - position, // observation - pos_obs_var, // observation variance - Vector2f(ekf.state().pos) - position, // innovation - Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance - math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate + sample.time_us, // sample timestamp + position, // observation + pos_obs_var, // observation variance + Vector2f(ekf.state().pos) - position, // 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) diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp index bd63f7245605..e5bc78026a68 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp @@ -96,8 +96,8 @@ class AuxGlobalPosition : public ModuleParams uint64_t _time_last_buffer_push{0}; enum class Ctrl : uint8_t { - HPOS = (1<<0), - VPOS = (1<<1) + HPOS = (1 << 0), + VPOS = (1 << 1) }; enum class State { diff --git a/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp index 132aaa10559b..14d333f3642d 100644 --- a/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp @@ -41,12 +41,12 @@ void Ekf::controlAuxVelFusion() if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &sample)) { updateAidSourceStatus(_aid_src_aux_vel, - sample.time_us, // sample timestamp - sample.vel, // observation - sample.velVar, // observation variance - Vector2f(_state.vel.xy()) - sample.vel, // innovation - Vector2f(getStateVariance()) + sample.velVar, // innovation variance - math::max(_params.auxvel_gate, 1.f)); // innovation gate + sample.time_us, // sample timestamp + sample.vel, // observation + sample.velVar, // observation variance + Vector2f(_state.vel.xy()) - sample.vel, // innovation + Vector2f(getStateVariance()) + sample.velVar, // innovation variance + math::max(_params.auxvel_gate, 1.f)); // innovation gate if (isHorizontalAidingActive()) { fuseHorizontalVelocity(_aid_src_aux_vel); diff --git a/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp index c55ed14aaf4d..439cbdb44651 100644 --- a/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp @@ -169,12 +169,12 @@ void Ekf::fuseDrag(const dragSample &drag_sample) } updateAidSourceStatus(_aid_src_drag, - drag_sample.time_us, // sample timestamp - observation, // observation - observation_variance, // observation variance - innovation, // innovation - innovation_variance, // innovation variance - innov_gate); // innovation gate + drag_sample.time_us, // sample timestamp + observation, // observation + observation_variance, // observation variance + innovation, // innovation + innovation_variance, // innovation variance + innov_gate); // innovation gate if (fused[0] && fused[1]) { _aid_src_drag.fused = true; diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp index f757b59a788e..43f61a79d057 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp @@ -55,8 +55,10 @@ void Ekf::controlExternalVisionFusion() const bool starting_conditions_passing = quality_sufficient && ((ev_sample.time_us - _ev_sample_prev.time_us) < EV_MAX_INTERVAL) - && ((_params.ev_quality_minimum <= 0) || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient - && ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient + && ((_params.ev_quality_minimum <= 0) + || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient + && ((_params.ev_quality_minimum <= 0) + || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient && isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL); updateEvAttitudeErrorFilter(ev_sample, ev_reset); diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index e4308c4fcf76..a69906eef489 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp @@ -77,10 +77,12 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com float measurement_var = math::max(pos_cov(2, 2), sq(_params.ev_pos_noise), sq(0.01f)); #if defined(CONFIG_EKF2_GNSS) + // increase minimum variance if GPS active if (_control_status.flags.gps_hgt) { measurement_var = math::max(measurement_var, sq(_params.gps_pos_noise)); } + #endif // CONFIG_EKF2_GNSS const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp index 1e6e7904ddce..53ee21e09b8c 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp @@ -161,12 +161,12 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common const Vector2f pos_obs_var = measurement_var + _ev_pos_b_est.getBiasVar(); updateAidSourceStatus(aid_src, - ev_sample.time_us, // sample timestamp - position, // observation - pos_obs_var, // observation variance - Vector2f(_state.pos) - position, // innovation - Vector2f(getStateVariance()) + pos_obs_var, // innovation variance - math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate + ev_sample.time_us, // sample timestamp + position, // observation + pos_obs_var, // observation variance + Vector2f(_state.pos) - position, // innovation + Vector2f(getStateVariance()) + pos_obs_var, // innovation variance + math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate // update the bias estimator before updating the main filter but after // using its current state to compute the vertical position innovation @@ -205,7 +205,8 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common } } -void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src) +void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, + estimator_aid_source2d_s &aid_src) { // activate fusion // TODO: (_params.position_sensor_ref == PositionSensor::EV) @@ -229,7 +230,8 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem _control_status.flags.ev_pos = true; } -void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src) +void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, + bool reset, estimator_aid_source2d_s &aid_src) { if (reset) { diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp index d974a5e87b81..a285d3e4556a 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp @@ -53,12 +53,12 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common computeYawInnovVarAndH(obs_var, innov_var, H_YAW); updateAidSourceStatus(aid_src, - ev_sample.time_us, // sample timestamp - obs, // observation - obs_var, // observation variance - innov, // innovation - innov_var, // innovation variance - math::max(_params.heading_innov_gate, 1.f)); // innovation gate + ev_sample.time_us, // sample timestamp + obs, // observation + obs_var, // observation variance + innov, // innovation + innov_var, // innovation variance + math::max(_params.heading_innov_gate, 1.f)); // innovation gate if (ev_reset) { _control_status.flags.ev_yaw_fault = false; @@ -191,9 +191,11 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common void Ekf::stopEvYawFusion() { #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_yaw) { _control_status.flags.ev_yaw = false; } + #endif // CONFIG_EKF2_EXTERNAL_VISION } diff --git a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp index 9ea6485e64fd..1ba1dd309b02 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp @@ -68,17 +68,17 @@ void Ekf::controlFakePosFusion() const float innov_gate = 3.f; updateAidSourceStatus(aid_src, - _time_delayed_us, - position, // observation - obs_var, // observation variance - Vector2f(_state.pos) - position, // innovation - Vector2f(getStateVariance()) + obs_var, // innovation variance - innov_gate); // innovation gate + _time_delayed_us, + position, // observation + obs_var, // observation variance + Vector2f(_state.pos) - position, // innovation + Vector2f(getStateVariance()) + obs_var, // innovation variance + innov_gate); // innovation gate const bool enable_conditions_passing = !isHorizontalAidingActive() - && ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest) - && (!(_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest) - && _horizontal_deadreckon_time_exceeded; + && ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest) + && (!(_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest) + && _horizontal_deadreckon_time_exceeded; if (_control_status.flags.fake_pos) { if (enable_conditions_passing) { diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index a598f1078b66..652069caf574 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -126,6 +126,7 @@ void Ekf::collect_gps(const gnssSample &gps) _wmm_gps_time_last_set = _time_delayed_us; } } + #endif // CONFIG_EKF2_MAGNETOMETER _earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat)); @@ -157,7 +158,8 @@ bool Ekf::runGnssChecks(const gnssSample &gps) // Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient constexpr float filt_time_const = 10.0f; - const float dt = math::constrain(float(int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) * 1e-6f, 0.001f, filt_time_const); + const float dt = math::constrain(float(int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) + * 1e-6f, 0.001f, filt_time_const); const float filter_coef = dt / filt_time_const; // The following checks are only valid when the vehicle is at rest diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index afff48dcb7da..0c8c98ab26b6 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -190,12 +190,12 @@ void Ekf::updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s const float innovation_gate = math::max(_params.gps_vel_innov_gate, 1.f); updateAidSourceStatus(aid_src, - gnss_sample.time_us, // sample timestamp - velocity, // observation - vel_obs_var, // observation variance - _state.vel - velocity, // innovation - getVelocityVariance() + vel_obs_var, // innovation variance - innovation_gate); // innovation gate + gnss_sample.time_us, // sample timestamp + velocity, // observation + vel_obs_var, // observation variance + _state.vel - velocity, // innovation + getVelocityVariance() + vel_obs_var, // innovation variance + innovation_gate); // innovation gate // vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable, // but limit innovation to prevent spikes that could destabilise the filter @@ -228,12 +228,12 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s const Vector2f pos_obs_var(pos_var, pos_var); updateAidSourceStatus(aid_src, - gnss_sample.time_us, // sample timestamp - position, // observation - pos_obs_var, // observation variance - Vector2f(_state.pos) - position, // innovation - Vector2f(getStateVariance()) + pos_obs_var, // innovation variance - math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate + gnss_sample.time_us, // sample timestamp + position, // observation + pos_obs_var, // observation variance + Vector2f(_state.pos) - position, // innovation + Vector2f(getStateVariance()) + pos_obs_var, // innovation variance + math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate } void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp index 2e135f357f20..d66fbac7c6f3 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp @@ -62,12 +62,12 @@ void Ekf::updateGpsYaw(const gnssSample &gps_sample) &heading_pred, &heading_innov_var, &H); updateAidSourceStatus(_aid_src_gnss_yaw, - gps_sample.time_us, // sample timestamp - measured_hdg, // observation - R_YAW, // observation variance - wrap_pi(heading_pred - measured_hdg), // innovation - heading_innov_var, // innovation variance - math::max(_params.heading_innov_gate, 1.f)); // innovation gate + gps_sample.time_us, // sample timestamp + measured_hdg, // observation + R_YAW, // observation variance + wrap_pi(heading_pred - measured_hdg), // innovation + heading_innov_var, // innovation variance + math::max(_params.heading_innov_gate, 1.f)); // innovation gate } void Ekf::fuseGpsYaw(float antenna_yaw_offset) diff --git a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp index 40127a37548e..e6961ed17c31 100644 --- a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp @@ -54,7 +54,8 @@ void Ekf::controlGravityFusion(const imuSample &imu) const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f; const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f; - const bool accel_lpf_norm_good = (_accel_magnitude_filt > lower_accel_limit) && (_accel_magnitude_filt < upper_accel_limit); + const bool accel_lpf_norm_good = (_accel_magnitude_filt > lower_accel_limit) + && (_accel_magnitude_filt < upper_accel_limit); // fuse gravity observation if our overall acceleration isn't too big _control_status.flags.gravity_vector = (_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) @@ -70,12 +71,12 @@ void Ekf::controlGravityFusion(const imuSample &imu) // fill estimator aid source status updateAidSourceStatus(_aid_src_gravity, - imu.time_us, // sample timestamp - measurement, // observation - Vector3f{measurement_var, measurement_var, measurement_var}, // observation variance - innovation, // innovation - innovation_variance, // innovation variance - 0.25f); // innovation gate + imu.time_us, // sample timestamp + measurement, // observation + Vector3f{measurement_var, measurement_var, measurement_var}, // observation variance + innovation, // innovation + innovation_variance, // innovation variance + 0.25f); // innovation gate bool fused[3] {false, false, false}; @@ -90,14 +91,16 @@ void Ekf::controlGravityFusion(const imuSample &imu) sym::ComputeGravityYInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H); // recalculate innovation using the updated state - _aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f))(index) - measurement(index); + _aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, + -1.f))(index) - measurement(index); } else if (index == 2) { // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) sym::ComputeGravityZInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H); // recalculate innovation using the updated state - _aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f))(index) - measurement(index); + _aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, + -1.f))(index) - measurement(index); } VectorState K = P * H / _aid_src_gravity.innovation_variance[index]; @@ -105,7 +108,8 @@ void Ekf::controlGravityFusion(const imuSample &imu) const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]; if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) { - fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], _aid_src_gravity.innovation[index]); + fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], + _aid_src_gravity.innovation[index]); } } diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 74d017add5f9..979807f116a5 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -109,12 +109,12 @@ void Ekf::controlMagFusion() sym::ComputeMagInnovInnovVarAndHx(_state.vector(), P, mag_sample.mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H); updateAidSourceStatus(aid_src, - mag_sample.time_us, // sample timestamp - mag_sample.mag, // observation - Vector3f(R_MAG, R_MAG, R_MAG), // observation variance - mag_innov, // innovation - innov_var, // innovation variance - math::max(_params.mag_innov_gate, 1.f)); // innovation gate + mag_sample.time_us, // sample timestamp + mag_sample.mag, // observation + Vector3f(R_MAG, R_MAG, R_MAG), // observation variance + mag_innov, // innovation + innov_var, // innovation variance + math::max(_params.mag_innov_gate, 1.f)); // innovation gate // Perform an innovation consistency check and report the result _innov_check_fail_status.flags.reject_mag_x = (aid_src.test_ratio[0] > 1.f); @@ -134,7 +134,8 @@ void Ekf::controlMagFusion() && checkMagField(mag_sample.mag) && (_mag_counter > 3) // wait until we have more than a few samples through the filter && (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame - && (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset + && (_state_reset_status.reset_count.quat == + _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset && isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL); checkMagHeadingConsistency(mag_sample); @@ -145,22 +146,22 @@ void Ekf::controlMagFusion() { - const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding; - const bool common_conditions_passing = _control_status.flags.mag - && ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding) - || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) - && !_control_status.flags.mag_fault - && !_control_status.flags.mag_field_disturbed - && !_control_status.flags.ev_yaw - && !_control_status.flags.gps_yaw; - - _control_status.flags.mag_3D = common_conditions_passing - && (_params.mag_fusion_type == MagFuseType::AUTO) - && _control_status.flags.mag_aligned_in_flight; - - _control_status.flags.mag_hdg = common_conditions_passing - && ((_params.mag_fusion_type == MagFuseType::HEADING) - || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D)); + const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding; + const bool common_conditions_passing = _control_status.flags.mag + && ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding) + || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) + && !_control_status.flags.mag_fault + && !_control_status.flags.mag_field_disturbed + && !_control_status.flags.ev_yaw + && !_control_status.flags.gps_yaw; + + _control_status.flags.mag_3D = common_conditions_passing + && (_params.mag_fusion_type == MagFuseType::AUTO) + && _control_status.flags.mag_aligned_in_flight; + + _control_status.flags.mag_hdg = common_conditions_passing + && ((_params.mag_fusion_type == MagFuseType::HEADING) + || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D)); } // TODO: allow clearing mag_fault if mag_3d is good? diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp index 474e654ec60d..acd16578c54f 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp @@ -50,7 +50,8 @@ #include -bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states, bool update_tilt) +bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, + bool update_all_states, bool update_tilt) { // if any axis failed, abort the mag fusion if (aid_src.innovation_rejected) { @@ -72,7 +73,8 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H); // recalculate innovation using the updated state - aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index); + aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag( + index); } else if (index == 2) { // we do not fuse synthesized magnetomter measurements when doing 3D fusion @@ -85,7 +87,8 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H); // recalculate innovation using the updated state - aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index); + aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag( + index); } if (aid_src.innovation_variance[index] < R_MAG) { @@ -171,7 +174,8 @@ bool Ekf::fuseDeclination(float decl_sigma) float decl_pred; float innovation_variance; - sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H); + sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, + &H); const float innovation = wrap_pi(decl_pred - decl_measurement); diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp index 91efdace2e79..531a7b56d09e 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp @@ -85,7 +85,8 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain) Kfusion(State::terrain.idx) = 0.f; } - if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], _aid_src_optical_flow.innovation[index])) { + if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], + _aid_src_optical_flow.innovation[index])) { fused[index] = true; } } diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 2bc07e7b22c7..1b7db97e3020 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -101,11 +101,11 @@ void Ekf::controlRangeHaglFusion() const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance); const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast(RngCtrl::ENABLED)) - || (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL))) - && _control_status.flags.tilt_align - && measurement_valid - && _range_sensor.isDataHealthy() - && _rng_consistency_check.isKinematicallyConsistent(); + || (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL))) + && _control_status.flags.tilt_align + && measurement_valid + && _range_sensor.isDataHealthy() + && _rng_consistency_check.isKinematicallyConsistent(); const bool starting_conditions_passing = continuing_conditions_passing && isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL) @@ -260,10 +260,10 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src) float Ekf::getRngVar() const { return fmaxf( - P(State::pos.idx + 2, State::pos.idx + 2) - + sq(_params.range_noise) - + sq(_params.range_noise_scaler * _range_sensor.getRange()), - 0.f); + P(State::pos.idx + 2, State::pos.idx + 2) + + sq(_params.range_noise) + + sq(_params.range_noise_scaler * _range_sensor.getRange()), + 0.f); } void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp index dbfbd96b1006..f3c59be54135 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp @@ -56,7 +56,8 @@ struct rangeSample { int8_t quality{}; ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality. }; -static constexpr uint64_t RNG_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between range finder measurements (uSec) +static constexpr uint64_t RNG_MAX_INTERVAL = + 200e3; ///< Maximum allowable time interval between range finder measurements (uSec) class SensorRangeFinder : public Sensor { diff --git a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp index 997b474793b5..6a9ab3442220 100644 --- a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp @@ -88,12 +88,12 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const sym::ComputeSideslipInnovAndInnovVar(_state.vector(), P, R, FLT_EPSILON, &innov, &innov_var); updateAidSourceStatus(aid_src, - _time_delayed_us, // sample timestamp - observation, // observation - R, // observation variance - innov, // innovation - innov_var, // innovation variance - math::max(_params.beta_innov_gate, 1.f)); // innovation gate + _time_delayed_us, // sample timestamp + observation, // observation + R, // observation variance + innov, // innovation + innov_var, // innovation variance + math::max(_params.beta_innov_gate, 1.f)); // innovation gate } void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) diff --git a/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp index 1bae77c7c8b7..3c0a2c1d6cf7 100644 --- a/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp +++ b/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp @@ -44,7 +44,7 @@ void Ekf::controlZeroInnovationHeadingUpdate() || _control_status.flags.ev_yaw || _control_status.flags.gps_yaw; // fuse zero innovation at a limited rate if the yaw variance is too large - if(!yaw_aiding + if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) { // Use an observation variance larger than usual but small enough @@ -60,7 +60,8 @@ void Ekf::controlZeroInnovationHeadingUpdate() computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW); - if (!_control_status.flags.tilt_align || (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) { + if (!_control_status.flags.tilt_align + || (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) { // The yaw variance is too large, fuse fake measurement fuseYaw(aid_src_status, H_YAW); } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index b8e7664bf94c..7a008783fe1b 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -66,18 +66,26 @@ using math::Utilities::sq; using math::Utilities::updateYawInRotMat; // maximum sensor intervals in usec -static constexpr uint64_t BARO_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between pressure altitude measurements (uSec) -static constexpr uint64_t EV_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between external vision system measurements (uSec) -static constexpr uint64_t GNSS_MAX_INTERVAL = 500e3; ///< Maximum allowable time interval between GNSS measurements (uSec) -static constexpr uint64_t GNSS_YAW_MAX_INTERVAL = 1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec) -static constexpr uint64_t MAG_MAX_INTERVAL = 500e3; ///< Maximum allowable time interval between magnetic field measurements (uSec) +static constexpr uint64_t BARO_MAX_INTERVAL = + 200e3; ///< Maximum allowable time interval between pressure altitude measurements (uSec) +static constexpr uint64_t EV_MAX_INTERVAL = + 200e3; ///< Maximum allowable time interval between external vision system measurements (uSec) +static constexpr uint64_t GNSS_MAX_INTERVAL = + 500e3; ///< Maximum allowable time interval between GNSS measurements (uSec) +static constexpr uint64_t GNSS_YAW_MAX_INTERVAL = + 1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec) +static constexpr uint64_t MAG_MAX_INTERVAL = + 500e3; ///< Maximum allowable time interval between magnetic field measurements (uSec) // bad accelerometer detection and mitigation -static constexpr uint64_t BADACC_PROBATION = 10e6; ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec) -static constexpr float BADACC_BIAS_PNOISE = 4.9f; ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2) +static constexpr uint64_t BADACC_PROBATION = + 10e6; ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec) +static constexpr float BADACC_BIAS_PNOISE = + 4.9f; ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2) // ground effect compensation -static constexpr uint64_t GNDEFFECT_TIMEOUT = 10e6; ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec) +static constexpr uint64_t GNDEFFECT_TIMEOUT = + 10e6; ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec) enum class PositionFrame : uint8_t { LOCAL_FRAME_NED = 0, @@ -93,8 +101,8 @@ enum class VelocityFrame : uint8_t { #if defined(CONFIG_EKF2_MAGNETOMETER) enum GeoDeclinationMask : uint8_t { // Bit locations for mag_declination_source - USE_GEO_DECL = (1<<0), ///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value - SAVE_GEO_DECL = (1<<1) ///< set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library + USE_GEO_DECL = (1 << 0), ///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value + SAVE_GEO_DECL = (1 << 1) ///< set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library }; enum MagFuseType : uint8_t { @@ -128,16 +136,16 @@ enum class PositionSensor : uint8_t { }; enum class ImuCtrl : uint8_t { - GyroBias = (1<<0), - AccelBias = (1<<1), - GravityVector = (1<<2), + GyroBias = (1 << 0), + AccelBias = (1 << 1), + GravityVector = (1 << 2), }; enum class GnssCtrl : uint8_t { - HPOS = (1<<0), - VPOS = (1<<1), - VEL = (1<<2), - YAW = (1<<3) + HPOS = (1 << 0), + VPOS = (1 << 1), + VEL = (1 << 2), + YAW = (1 << 3) }; enum class RngCtrl : uint8_t { @@ -147,10 +155,10 @@ enum class RngCtrl : uint8_t { }; enum class EvCtrl : uint8_t { - HPOS = (1<<0), - VPOS = (1<<1), - VEL = (1<<2), - YAW = (1<<3) + HPOS = (1 << 0), + VPOS = (1 << 1), + VEL = (1 << 2), + YAW = (1 << 3) }; enum class MagCheckMask : uint8_t { @@ -271,7 +279,7 @@ struct parameters { float accel_bias_p_noise{1.0e-2f}; ///< process noise for IMU accelerometer bias prediction (m/sec**3) #if defined(CONFIG_EKF2_WIND) - const float initial_wind_uncertainty{1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec) + const float initial_wind_uncertainty {1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec) float wind_vel_nsd{1.0e-2f}; ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz)) const float wind_vel_nsd_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity #endif // CONFIG_EKF2_WIND @@ -282,7 +290,7 @@ struct parameters { float initial_tilt_err{0.1f}; ///< 1-sigma tilt error after initial alignment using gravity vector (rad) #if defined(CONFIG_EKF2_BAROMETER) - int32_t baro_ctrl{1}; + int32_t baro_ctrl {1}; float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec) float baro_noise{2.0f}; ///< observation noise for barometric height fusion (m) float baro_bias_nsd{0.13f}; ///< process noise for barometric height bias estimation (m/s/sqrt(Hz)) @@ -305,7 +313,7 @@ struct parameters { #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_GNSS) - int32_t gnss_ctrl{static_cast(GnssCtrl::HPOS) | static_cast(GnssCtrl::VEL)}; + int32_t gnss_ctrl {static_cast(GnssCtrl::HPOS) | static_cast(GnssCtrl::VEL)}; float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec) Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m) @@ -346,7 +354,7 @@ struct parameters { float mag_heading_noise{3.0e-1f}; ///< measurement noise used for simple heading fusion (rad) #if defined(CONFIG_EKF2_MAGNETOMETER) - float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec) + float mag_delay_ms {0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec) float mage_p_noise{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec) float magb_p_noise{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec) @@ -383,13 +391,13 @@ struct parameters { #endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_TERRAIN) - float terrain_p_noise{5.0f}; ///< process noise for terrain offset (m/sec) + float terrain_p_noise {5.0f}; ///< process noise for terrain offset (m/sec) float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m) const float terrain_timeout{10.f}; ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s) #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER) - float rng_gnd_clearance{0.1f}; ///< minimum valid value for range when on ground (m) + float rng_gnd_clearance {0.1f}; ///< minimum valid value for range when on ground (m) #endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -433,7 +441,7 @@ struct parameters { #endif // CONFIG_EKF2_GRAVITY_FUSION #if defined(CONFIG_EKF2_OPTICAL_FLOW) - int32_t flow_ctrl{0}; + int32_t flow_ctrl {0}; float flow_delay_ms{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval // optical flow fusion @@ -494,7 +502,8 @@ union fault_status_u { bool bad_hdg : 1; ///< 3 - true if the fusion of the heading angle has encountered a numerical error bool bad_mag_decl : 1; ///< 4 - true if the fusion of the magnetic declination has encountered a numerical error bool bad_airspeed : 1; ///< 5 - true if fusion of the airspeed has encountered a numerical error - bool bad_sideslip : 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error +bool bad_sideslip : + 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error bool bad_optflow_X : 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error bool bad_optflow_Y : 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error bool bad_acc_bias : 1; ///< 9 - true if bad delta velocity bias estimates have been detected @@ -555,7 +564,8 @@ union filter_control_status_u { uint64_t in_air : 1; ///< 7 - true when the vehicle is airborne uint64_t wind : 1; ///< 8 - true when wind velocity is being estimated uint64_t baro_hgt : 1; ///< 9 - true when baro height is being fused as a primary height reference - uint64_t rng_hgt : 1; ///< 10 - true when range finder height is being fused as a primary height reference +uint64_t rng_hgt : + 1; ///< 10 - true when range finder height is being fused as a primary height reference uint64_t gps_hgt : 1; ///< 11 - true when GPS height is being fused as a primary height reference uint64_t ev_pos : 1; ///< 12 - true when local position data fusion from external vision is intended uint64_t ev_yaw : 1; ///< 13 - true when yaw data from external vision measurements fusion is intended @@ -563,26 +573,38 @@ union filter_control_status_u { uint64_t fuse_beta : 1; ///< 15 - true when synthetic sideslip measurements are being fused uint64_t mag_field_disturbed : 1; ///< 16 - true when the mag field does not match the expected strength uint64_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle - uint64_t mag_fault : 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used +uint64_t mag_fault : + 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used uint64_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused - uint64_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active - uint64_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough - uint64_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended +uint64_t gnd_effect : + 1; ///< 20 - true when protection from ground effect induced static pressure rise is active +uint64_t rng_stuck : + 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough +uint64_t gps_yaw : + 1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended uint64_t mag_aligned_in_flight : 1; ///< 23 - true when the in-flight mag field alignment has been completed - uint64_t ev_vel : 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended - uint64_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component +uint64_t ev_vel : + 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended +uint64_t synthetic_mag_z : + 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component uint64_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest - uint64_t gps_yaw_fault : 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used - uint64_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used - uint64_t inertial_dead_reckoning : 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift +uint64_t gps_yaw_fault : + 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used +uint64_t rng_fault : + 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used +uint64_t inertial_dead_reckoning : + 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift uint64_t wind_dead_reckoning : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements uint64_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing uint64_t fake_pos : 1; ///< 32 - true when fake position measurements are being fused uint64_t fake_hgt : 1; ///< 33 - true when fake height measurements are being fused uint64_t gravity_vector : 1; ///< 34 - true when gravity vector measurements are being fused - uint64_t mag : 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended - uint64_t ev_yaw_fault : 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used - uint64_t mag_heading_consistent : 1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter +uint64_t mag : + 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended +uint64_t ev_yaw_fault : + 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used +uint64_t mag_heading_consistent : + 1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter uint64_t aux_gpos : 1; ///< 38 - true if auxiliary global position measurement fusion is intended uint64_t rng_terrain : 1; ///< 39 - true if we are fusing range finder data for terrain uint64_t opt_flow_terrain : 1; ///< 40 - true if we are fusing flow data for terrain @@ -601,9 +623,12 @@ union ekf_solution_status_u { uint16_t pos_horiz_abs : 1; ///< 4 - True if the horizontal position (absolute) estimate is good uint16_t pos_vert_abs : 1; ///< 5 - True if the vertical position (absolute) estimate is good uint16_t pos_vert_agl : 1; ///< 6 - True if the vertical position (above ground) estimate is good - uint16_t const_pos_mode : 1; ///< 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) - uint16_t pred_pos_horiz_rel : 1; ///< 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate - uint16_t pred_pos_horiz_abs : 1; ///< 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate +uint16_t const_pos_mode : + 1; ///< 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) +uint16_t pred_pos_horiz_rel : + 1; ///< 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate +uint16_t pred_pos_horiz_abs : + 1; ///< 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate uint16_t gps_glitch : 1; ///< 10 - True if the EKF has detected a GPS glitch uint16_t accel_error : 1; ///< 11 - True if the EKF has detected bad accelerometer data } flags; @@ -621,16 +646,22 @@ union information_event_status_u { bool reset_pos_to_last_known : 1; ///< 5 - true when the position states are reset to the last known position bool reset_pos_to_gps : 1; ///< 6 - true when the position states are reset to the gps measurement bool reset_pos_to_vision : 1; ///< 7 - true when the position states are reset to the vision system measurement - bool starting_gps_fusion : 1; ///< 8 - true when the filter starts using gps measurements to correct the state estimates - bool starting_vision_pos_fusion : 1; ///< 9 - true when the filter starts using vision system position measurements to correct the state estimates - bool starting_vision_vel_fusion : 1; ///< 10 - true when the filter starts using vision system velocity measurements to correct the state estimates - bool starting_vision_yaw_fusion : 1; ///< 11 - true when the filter starts using vision system yaw measurements to correct the state estimates - bool yaw_aligned_to_imu_gps : 1; ///< 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data +bool starting_gps_fusion : + 1; ///< 8 - true when the filter starts using gps measurements to correct the state estimates +bool starting_vision_pos_fusion : + 1; ///< 9 - true when the filter starts using vision system position measurements to correct the state estimates +bool starting_vision_vel_fusion : + 1; ///< 10 - true when the filter starts using vision system velocity measurements to correct the state estimates +bool starting_vision_yaw_fusion : + 1; ///< 11 - true when the filter starts using vision system yaw measurements to correct the state estimates +bool yaw_aligned_to_imu_gps : + 1; ///< 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data bool reset_hgt_to_baro : 1; ///< 13 - true when the vertical position state is reset to the baro measurement bool reset_hgt_to_gps : 1; ///< 14 - true when the vertical position state is reset to the gps measurement bool reset_hgt_to_rng : 1; ///< 15 - true when the vertical position state is reset to the rng measurement bool reset_hgt_to_ev : 1; ///< 16 - true when the vertical position state is reset to the ev measurement - bool reset_pos_to_ext_obs : 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning +bool reset_pos_to_ext_obs : + 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning } flags; uint32_t value; }; @@ -639,17 +670,27 @@ union information_event_status_u { union warning_event_status_u { struct { bool gps_quality_poor : 1; ///< 0 - true when the gps is failing quality checks - bool gps_fusion_timout : 1; ///< 1 - true when the gps data has not been used to correct the state estimates for a significant time period +bool gps_fusion_timout : + 1; ///< 1 - true when the gps data has not been used to correct the state estimates for a significant time period bool gps_data_stopped : 1; ///< 2 - true when the gps data has stopped for a significant time period - bool gps_data_stopped_using_alternate : 1; ///< 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation - bool height_sensor_timeout : 1; ///< 4 - true when the height sensor has not been used to correct the state estimates for a significant time period - bool stopping_navigation : 1; ///< 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation - bool invalid_accel_bias_cov_reset : 1; ///< 6 - true when the filter has detected bad acceerometer bias state estimates and has reset the corresponding covariance matrix elements - bool bad_yaw_using_gps_course : 1; ///< 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course - bool stopping_mag_use : 1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data - bool vision_data_stopped : 1; ///< 9 - true when the vision system data has stopped for a significant time period - bool emergency_yaw_reset_mag_stopped : 1; ///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data - bool emergency_yaw_reset_gps_yaw_stopped: 1; ///< 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data +bool gps_data_stopped_using_alternate : + 1; ///< 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation +bool height_sensor_timeout : + 1; ///< 4 - true when the height sensor has not been used to correct the state estimates for a significant time period +bool stopping_navigation : + 1; ///< 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation +bool invalid_accel_bias_cov_reset : + 1; ///< 6 - true when the filter has detected bad acceerometer bias state estimates and has reset the corresponding covariance matrix elements +bool bad_yaw_using_gps_course : + 1; ///< 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course +bool stopping_mag_use : + 1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data +bool vision_data_stopped : + 1; ///< 9 - true when the vision system data has stopped for a significant time period +bool emergency_yaw_reset_mag_stopped : + 1; ///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data +bool emergency_yaw_reset_gps_yaw_stopped: + 1; ///< 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data } flags; uint32_t value; }; diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 519cbbcfa28f..8108d26c1381 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -76,14 +76,17 @@ void Ekf::initialiseCovariance() if (_control_status.flags.gps_hgt) { z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f)); } + #else const float xy_pos_var = sq(fmaxf(_params.pos_noaid_noise, 0.01f)); #endif #if defined(CONFIG_EKF2_RANGE_FINDER) + if (_control_status.flags.rng_hgt) { z_pos_var = sq(fmaxf(_params.range_noise, 0.01f)); } + #endif // CONFIG_EKF2_RANGE_FINDER P.uncorrelateCovarianceSetVariance(State::pos.idx, Vector3f(xy_pos_var, xy_pos_var, z_pos_var)); @@ -191,13 +194,16 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) P(i, i) += mag_B_process_noise; } } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) + if (_control_status.flags.wind) { // wind vel: add process noise - float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); + float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, + 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; for (unsigned index = 0; index < State::wind_vel.dof; index++) { @@ -205,18 +211,22 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) P(i, i) += wind_vel_process_noise; } } + #endif // CONFIG_EKF2_WIND #if defined(CONFIG_EKF2_TERRAIN) + if (_height_sensor_ref != HeightSensor::RANGE) { // predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle // process noise due to errors in vehicle height estimate float terrain_process_noise = sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise); // process noise due to terrain gradient - terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(1))); + terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel( + 1))); P(State::terrain.idx, State::terrain.idx) += terrain_process_noise; } + #endif // CONFIG_EKF2_TERRAIN // covariance matrix is symmetrical, so copy upper half to lower half @@ -244,16 +254,20 @@ void Ekf::constrainStateVariances() constrainStateVarLimitRatio(State::accel_bias, kAccelBiasVarianceMin, 1.f); #if defined(CONFIG_EKF2_MAGNETOMETER) + if (_control_status.flags.mag) { constrainStateVarLimitRatio(State::mag_I, kMagVarianceMin, 1.f); constrainStateVarLimitRatio(State::mag_B, kMagVarianceMin, 1.f); } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) + if (_control_status.flags.wind) { constrainStateVarLimitRatio(State::wind_vel, 1e-6f, 1e6f); } + #endif // CONFIG_EKF2_WIND #if defined(CONFIG_EKF2_TERRAIN) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index ac410b4d8d64..4479dc37b8ea 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -168,7 +168,8 @@ 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, _state.gyro_bias, _state.accel_bias); + _output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, + _state.gyro_bias, _state.accel_bias); return true; } @@ -281,7 +282,8 @@ void Ekf::predictState(const imuSample &imu_delayed) _height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf; } -bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation) +bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, + uint64_t timestamp_observation) { if (!_pos_ref.isInitialized()) { ECL_WARN("unable to reset global position, position reference not initialized"); @@ -315,7 +317,7 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl const float sq_gate = sq(5.f); // magic hardcoded gate const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)), - sq(innov(1)) / (sq_gate * innov_var(1))}; + sq(innov(1)) / (sq_gate * innov_var(1))}; const bool innov_rejected = (test_ratio.max() > 1.f); @@ -331,8 +333,8 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl } else { if (fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0) - && fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1) - ) { + && fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1) + ) { ECL_INFO("fused external observation as position measurement"); _time_last_hor_pos_fuse = _time_delayed_us; _last_known_pos.xy() = _state.pos.xy(); @@ -366,40 +368,49 @@ void Ekf::print_status() printf("\nStates: (%.4f seconds ago)\n", (_time_latest_us - _time_delayed_us) * 1e-6); printf("Orientation (%d-%d): [%.3f, %.3f, %.3f, %.3f] (Euler [%.1f, %.1f, %.1f] deg) var: [%.1e, %.1e, %.1e]\n", State::quat_nominal.idx, State::quat_nominal.idx + State::quat_nominal.dof - 1, - (double)_state.quat_nominal(0), (double)_state.quat_nominal(1), (double)_state.quat_nominal(2), (double)_state.quat_nominal(3), - (double)math::degrees(matrix::Eulerf(_state.quat_nominal).phi()), (double)math::degrees(matrix::Eulerf(_state.quat_nominal).theta()), (double)math::degrees(matrix::Eulerf(_state.quat_nominal).psi()), - (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + (double)_state.quat_nominal(0), (double)_state.quat_nominal(1), (double)_state.quat_nominal(2), + (double)_state.quat_nominal(3), + (double)math::degrees(matrix::Eulerf(_state.quat_nominal).phi()), + (double)math::degrees(matrix::Eulerf(_state.quat_nominal).theta()), + (double)math::degrees(matrix::Eulerf(_state.quat_nominal).psi()), + (double)getStateVariance()(0), (double)getStateVariance()(1), + (double)getStateVariance()(2) ); printf("Velocity (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n", State::vel.idx, State::vel.idx + State::vel.dof - 1, (double)_state.vel(0), (double)_state.vel(1), (double)_state.vel(2), - (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + (double)getStateVariance()(0), (double)getStateVariance()(1), + (double)getStateVariance()(2) ); 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)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + (double)getStateVariance()(0), (double)getStateVariance()(1), + (double)getStateVariance()(2) ); printf("Gyro Bias (%d-%d): [%.6f, %.6f, %.6f] var: [%.1e, %.1e, %.1e]\n", State::gyro_bias.idx, State::gyro_bias.idx + State::gyro_bias.dof - 1, (double)_state.gyro_bias(0), (double)_state.gyro_bias(1), (double)_state.gyro_bias(2), - (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + (double)getStateVariance()(0), (double)getStateVariance()(1), + (double)getStateVariance()(2) ); printf("Accel Bias (%d-%d): [%.6f, %.6f, %.6f] var: [%.1e, %.1e, %.1e]\n", State::accel_bias.idx, State::accel_bias.idx + State::accel_bias.dof - 1, (double)_state.accel_bias(0), (double)_state.accel_bias(1), (double)_state.accel_bias(2), - (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + (double)getStateVariance()(0), (double)getStateVariance()(1), + (double)getStateVariance()(2) ); #if defined(CONFIG_EKF2_MAGNETOMETER) printf("Magnetic Field (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n", State::mag_I.idx, State::mag_I.idx + State::mag_I.dof - 1, (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2), - (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + (double)getStateVariance()(0), (double)getStateVariance()(1), + (double)getStateVariance()(2) ); printf("Magnetic Bias (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n", diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 42ea84aa1f8a..1b20c433e2a6 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -138,21 +138,27 @@ class Ekf final : public EstimatorInterface float getHeadingInnov() const { #if defined(CONFIG_EKF2_MAGNETOMETER) + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { return Vector3f(_aid_src_mag.innovation).max(); } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) + if (_control_status.flags.gps_yaw) { return _aid_src_gnss_yaw.innovation; } + #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_yaw) { return _aid_src_ev_yaw.innovation; } + #endif // CONFIG_EKF2_EXTERNAL_VISION return 0.f; @@ -161,21 +167,27 @@ class Ekf final : public EstimatorInterface float getHeadingInnovVar() const { #if defined(CONFIG_EKF2_MAGNETOMETER) + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { return Vector3f(_aid_src_mag.innovation_variance).max(); } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) + if (_control_status.flags.gps_yaw) { return _aid_src_gnss_yaw.innovation_variance; } + #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_yaw) { return _aid_src_ev_yaw.innovation_variance; } + #endif // CONFIG_EKF2_EXTERNAL_VISION return 0.f; @@ -184,21 +196,27 @@ class Ekf final : public EstimatorInterface float getHeadingInnovRatio() const { #if defined(CONFIG_EKF2_MAGNETOMETER) + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { return Vector3f(_aid_src_mag.test_ratio).max(); } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) + if (_control_status.flags.gps_yaw) { return _aid_src_gnss_yaw.test_ratio; } + #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_yaw) { return _aid_src_ev_yaw.test_ratio; } + #endif // CONFIG_EKF2_EXTERNAL_VISION return 0.f; @@ -490,6 +508,7 @@ class Ekf final : public EstimatorInterface P(j, i) = P(i, j); } } + #endif constrainStateVariances(); @@ -499,7 +518,8 @@ class Ekf final : public EstimatorInterface return true; } - bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation); + bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, + uint64_t timestamp_observation); void updateParameters(); @@ -576,7 +596,7 @@ class Ekf final : public EstimatorInterface SquareMatrixState P{}; ///< state covariance matrix #if defined(CONFIG_EKF2_DRAG_FUSION) - estimator_aid_source2d_s _aid_src_drag{}; + estimator_aid_source2d_s _aid_src_drag {}; #endif // CONFIG_EKF2_DRAG_FUSION #if defined(CONFIG_EKF2_TERRAIN) @@ -587,11 +607,11 @@ class Ekf final : public EstimatorInterface #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) - estimator_aid_source1d_s _aid_src_rng_hgt{}; + estimator_aid_source1d_s _aid_src_rng_hgt {}; #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) - estimator_aid_source2d_s _aid_src_optical_flow{}; + estimator_aid_source2d_s _aid_src_optical_flow {}; // optical flow processing Vector3f _flow_gyro_bias{}; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) @@ -603,17 +623,17 @@ class Ekf final : public EstimatorInterface #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_AIRSPEED) - estimator_aid_source1d_s _aid_src_airspeed{}; + estimator_aid_source1d_s _aid_src_airspeed {}; #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) - estimator_aid_source1d_s _aid_src_sideslip{}; + estimator_aid_source1d_s _aid_src_sideslip {}; #endif // CONFIG_EKF2_SIDESLIP estimator_aid_source2d_s _aid_src_fake_pos{}; estimator_aid_source1d_s _aid_src_fake_hgt{}; #if defined(CONFIG_EKF2_EXTERNAL_VISION) - estimator_aid_source1d_s _aid_src_ev_hgt{}; + estimator_aid_source1d_s _aid_src_ev_hgt {}; estimator_aid_source2d_s _aid_src_ev_pos{}; estimator_aid_source3d_s _aid_src_ev_vel{}; estimator_aid_source1d_s _aid_src_ev_yaw{}; @@ -624,7 +644,7 @@ class Ekf final : public EstimatorInterface #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) - bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused + bool _gps_data_ready {false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused // variables used for the GPS quality checks Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec) @@ -647,16 +667,16 @@ class Ekf final : public EstimatorInterface estimator_aid_source3d_s _aid_src_gnss_vel{}; # if defined(CONFIG_EKF2_GNSS_YAW) - estimator_aid_source1d_s _aid_src_gnss_yaw{}; + estimator_aid_source1d_s _aid_src_gnss_yaw {}; # endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_GRAVITY_FUSION) - estimator_aid_source3d_s _aid_src_gravity{}; + estimator_aid_source3d_s _aid_src_gravity {}; #endif // CONFIG_EKF2_GRAVITY_FUSION #if defined(CONFIG_EKF2_AUXVEL) - estimator_aid_source2d_s _aid_src_aux_vel{}; + estimator_aid_source2d_s _aid_src_aux_vel {}; #endif // CONFIG_EKF2_AUXVEL // Variables used by the initial filter alignment @@ -665,7 +685,7 @@ class Ekf final : public EstimatorInterface AlphaFilter _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) #if defined(CONFIG_EKF2_BAROMETER) - estimator_aid_source1d_s _aid_src_baro_hgt{}; + estimator_aid_source1d_s _aid_src_baro_hgt {}; // Variables used to perform in flight resets and switch between height sources AlphaFilter _baro_lpf{0.1f}; ///< filtered barometric height measurement (m) @@ -729,7 +749,8 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_MAGNETOMETER) // ekf sequential fusion of magnetometer measurements - bool fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states = false, bool update_tilt = false); + bool fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, + bool update_all_states = false, bool update_tilt = false); // fuse magnetometer declination measurement // argument passed in is the declination uncertainty in radians @@ -790,7 +811,8 @@ class Ekf final : public EstimatorInterface void resetVerticalVelocityToZero(); // horizontal and vertical position aid source - void updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us, const float observation, const float observation_variance, const float innovation_gate = 1.f) const; + void updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us, + const float observation, const float observation_variance, const float innovation_gate = 1.f) const; // horizontal and vertical position fusion bool fuseHorizontalPosition(estimator_aid_source2d_s &pos_aid_src); @@ -870,6 +892,7 @@ class Ekf final : public EstimatorInterface } #if defined(CONFIG_EKF2_MAGNETOMETER) + if (!_control_status.flags.mag) { for (unsigned i = 0; i < State::mag_I.dof; i++) { K(State::mag_I.idx + i) = 0.f; @@ -881,14 +904,17 @@ class Ekf final : public EstimatorInterface K(State::mag_B.idx + i) = 0.f; } } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) + if (!_control_status.flags.wind) { for (unsigned i = 0; i < State::wind_vel.dof; i++) { K(State::wind_vel.idx + i) = 0.f; } } + #endif // CONFIG_EKF2_WIND } @@ -912,15 +938,20 @@ class Ekf final : public EstimatorInterface // control fusion of external vision observations void controlExternalVisionFusion(); void updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset); - void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); - void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src); - void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src); - void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); + void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, + const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); + void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, + const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src); + void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, + const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src); + void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, + const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); void resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame); Vector3f rotateVarianceToEkf(const Vector3f &measurement_var); 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 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(); @@ -1091,7 +1122,7 @@ class Ekf final : public EstimatorInterface PositionSensor _position_sensor_ref{PositionSensor::GNSS}; #if defined(CONFIG_EKF2_EXTERNAL_VISION) - HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref}; + HeightBiasEstimator _ev_hgt_b_est {HeightSensor::EV, _height_sensor_ref}; PositionBiasEstimator _ev_pos_b_est{PositionSensor::EV, _position_sensor_ref}; AlphaFilter _ev_q_error_filt{0.001f}; bool _ev_q_error_initialized{false}; @@ -1297,7 +1328,7 @@ class Ekf final : public EstimatorInterface ZeroVelocityUpdate _zero_velocity_update{}; #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) - AuxGlobalPosition _aux_global_position{}; + AuxGlobalPosition _aux_global_position {}; #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION }; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 621ac87c6571..e44d6c0bf61b 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -72,13 +72,14 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo return _NED_origin_initialised; } -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 eph, + const float epv) { // sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90) - && PX4_ISFINITE(longitude) && (abs(longitude) <= 180) - && PX4_ISFINITE(altitude) && (altitude > -12'000.f) && (altitude < 100'000.f) - ) { + && PX4_ISFINITE(longitude) && (abs(longitude) <= 180) + && PX4_ISFINITE(altitude) && (altitude > -12'000.f) && (altitude < 100'000.f) + ) { bool current_pos_available = false; double current_lat = static_cast(NAN); double current_lon = static_cast(NAN); @@ -107,6 +108,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons _wmm_gps_time_last_set = _time_delayed_us; } + #endif // CONFIG_EKF2_MAGNETOMETER _gpos_origin_eph = eph; @@ -176,15 +178,19 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const // and using state variances for accuracy reporting is overly optimistic in these situations if (_horizontal_deadreckon_time_exceeded) { #if defined(CONFIG_EKF2_GNSS) + if (_control_status.flags.gps) { hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm()); } + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_pos) { hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm()); } + #endif // CONFIG_EKF2_EXTERNAL_VISION } @@ -203,19 +209,24 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const float vel_err_conservative = 0.0f; #if defined(CONFIG_EKF2_OPTICAL_FLOW) + if (_control_status.flags.opt_flow) { float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f); vel_err_conservative = math::max(getHagl(), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm(); } + #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_GNSS) + if (_control_status.flags.gps) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm()); } + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_pos) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_pos.innovation).norm()); } @@ -223,6 +234,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const if (_control_status.flags.ev_vel) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_vel.innovation).norm()); } + #endif // CONFIG_EKF2_EXTERNAL_VISION hvel_err = math::max(hvel_err, vel_err_conservative); @@ -282,6 +294,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl *hagl_min = flow_hagl_min; *hagl_max = flow_hagl_max; } + # endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_RANGE_FINDER @@ -309,23 +322,29 @@ float Ekf::getHeadingInnovationTestRatio() const float test_ratio = 0.f; #if defined(CONFIG_EKF2_MAGNETOMETER) - if (_control_status.flags.mag_hdg ||_control_status.flags.mag_3D) { + + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { for (auto &test_ratio_filtered : _aid_src_mag.test_ratio_filtered) { test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); } } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) + if (_control_status.flags.gps_yaw) { test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_yaw.test_ratio_filtered)); } + #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_yaw) { test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_yaw.test_ratio_filtered)); } + #endif // CONFIG_EKF2_EXTERNAL_VISION return sqrtf(test_ratio); @@ -337,27 +356,33 @@ float Ekf::getVelocityInnovationTestRatio() const float test_ratio = -1.f; #if defined(CONFIG_EKF2_GNSS) + if (_control_status.flags.gps) { for (int i = 0; i < 3; i++) { test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[i])); } } + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_vel) { for (int i = 0; i < 3; i++) { test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[i])); } } + #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_OPTICAL_FLOW) + if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) { for (auto &test_ratio_filtered : _aid_src_optical_flow.test_ratio_filtered) { test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); } } + #endif // CONFIG_EKF2_OPTICAL_FLOW if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) { @@ -373,25 +398,31 @@ float Ekf::getHorizontalPositionInnovationTestRatio() const float test_ratio = -1.f; #if defined(CONFIG_EKF2_GNSS) + if (_control_status.flags.gps) { for (auto &test_ratio_filtered : _aid_src_gnss_pos.test_ratio_filtered) { test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); } } + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_pos) { for (auto &test_ratio_filtered : _aid_src_ev_pos.test_ratio_filtered) { test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); } } + #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) + if (_control_status.flags.aux_gpos) { test_ratio = math::max(test_ratio, fabsf(_aux_global_position.test_ratio_filtered())); } + #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) { @@ -408,31 +439,39 @@ float Ekf::getVerticalPositionInnovationTestRatio() const int n_hgt_sources = 0; #if defined(CONFIG_EKF2_BAROMETER) + if (_control_status.flags.baro_hgt) { hgt_sum += sqrtf(fabsf(_aid_src_baro_hgt.test_ratio_filtered)); n_hgt_sources++; } + #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_GNSS) + if (_control_status.flags.gps_hgt) { hgt_sum += sqrtf(fabsf(_aid_src_gnss_hgt.test_ratio_filtered)); n_hgt_sources++; } + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) + if (_control_status.flags.rng_hgt) { hgt_sum += sqrtf(fabsf(_aid_src_rng_hgt.test_ratio_filtered)); n_hgt_sources++; } + #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_hgt) { hgt_sum += sqrtf(fabsf(_aid_src_ev_hgt.test_ratio_filtered)); n_hgt_sources++; } + #endif // CONFIG_EKF2_EXTERNAL_VISION if (n_hgt_sources > 0) { @@ -445,10 +484,12 @@ float Ekf::getVerticalPositionInnovationTestRatio() const float Ekf::getAirspeedInnovationTestRatio() const { #if defined(CONFIG_EKF2_AIRSPEED) + if (_control_status.flags.fuse_aspd) { // return the airspeed fusion innovation test ratio return sqrtf(fabsf(_aid_src_airspeed.test_ratio_filtered)); } + #endif // CONFIG_EKF2_AIRSPEED return NAN; @@ -457,10 +498,12 @@ float Ekf::getAirspeedInnovationTestRatio() const float Ekf::getSyntheticSideslipInnovationTestRatio() const { #if defined(CONFIG_EKF2_SIDESLIP) + if (_control_status.flags.fuse_beta) { // return the synthetic sideslip innovation test ratio return sqrtf(fabsf(_aid_src_sideslip.test_ratio_filtered)); } + #endif // CONFIG_EKF2_SIDESLIP return NAN; @@ -472,10 +515,12 @@ float Ekf::getHeightAboveGroundInnovationTestRatio() const #if defined(CONFIG_EKF2_TERRAIN) # if defined(CONFIG_EKF2_RANGE_FINDER) + if (_control_status.flags.rng_terrain) { // return the terrain height innovation test ratio test_ratio = math::max(test_ratio, fabsf(_aid_src_rng_hgt.test_ratio_filtered)); } + # endif // CONFIG_EKF2_RANGE_FINDER #endif // CONFIG_EKF2_TERRAIN @@ -491,10 +536,14 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const ekf_solution_status_u soln_status{}; // TODO: Is this accurate enough? soln_status.flags.attitude = attitude_valid(); - soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); - soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0); - soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); - soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0); + soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta + && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); + soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt + || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0); + soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos + || _control_status.flags.opt_flow) && (_fault_status.value == 0); + soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) + && (_fault_status.value == 0); soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert; #if defined(CONFIG_EKF2_TERRAIN) soln_status.flags.pos_vert_agl = isTerrainEstimateValid(); @@ -506,11 +555,13 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const bool mag_innov_good = true; #if defined(CONFIG_EKF2_MAGNETOMETER) - if (_control_status.flags.mag_hdg ||_control_status.flags.mag_3D) { + + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { if (Vector3f(_aid_src_mag.test_ratio).max() < 1.f) { mag_innov_good = false; } } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS) @@ -529,7 +580,8 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const void Ekf::fuse(const VectorState &K, float innovation) { // quat_nominal - Quatf delta_quat(matrix::AxisAnglef(K.slice(State::quat_nominal.idx, 0) * (-1.f * innovation))); + Quatf delta_quat(matrix::AxisAnglef(K.slice(State::quat_nominal.idx, + 0) * (-1.f * innovation))); _state.quat_nominal = delta_quat * _state.quat_nominal; _state.quat_nominal.normalize(); _R_to_earth = Dcmf(_state.quat_nominal); @@ -541,26 +593,35 @@ void Ekf::fuse(const VectorState &K, float innovation) _state.pos = matrix::constrain(_state.pos - K.slice(State::pos.idx, 0) * innovation, -1.e6f, 1.e6f); // gyro_bias - _state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice(State::gyro_bias.idx, 0) * innovation, - -getGyroBiasLimit(), getGyroBiasLimit()); + _state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice(State::gyro_bias.idx, + 0) * innovation, + -getGyroBiasLimit(), getGyroBiasLimit()); // accel_bias - _state.accel_bias = matrix::constrain(_state.accel_bias - K.slice(State::accel_bias.idx, 0) * innovation, - -getAccelBiasLimit(), getAccelBiasLimit()); + _state.accel_bias = matrix::constrain(_state.accel_bias - K.slice(State::accel_bias.idx, + 0) * innovation, + -getAccelBiasLimit(), getAccelBiasLimit()); #if defined(CONFIG_EKF2_MAGNETOMETER) + // mag_I, mag_B if (_control_status.flags.mag) { - _state.mag_I = matrix::constrain(_state.mag_I - K.slice(State::mag_I.idx, 0) * innovation, -1.f, 1.f); - _state.mag_B = matrix::constrain(_state.mag_B - K.slice(State::mag_B.idx, 0) * innovation, -getMagBiasLimit(), getMagBiasLimit()); + _state.mag_I = matrix::constrain(_state.mag_I - K.slice(State::mag_I.idx, 0) * innovation, -1.f, + 1.f); + _state.mag_B = matrix::constrain(_state.mag_B - K.slice(State::mag_B.idx, 0) * innovation, + -getMagBiasLimit(), getMagBiasLimit()); } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) + // wind_vel if (_control_status.flags.wind) { - _state.wind_vel = matrix::constrain(_state.wind_vel - K.slice(State::wind_vel.idx, 0) * innovation, -1.e2f, 1.e2f); + _state.wind_vel = matrix::constrain(_state.wind_vel - K.slice(State::wind_vel.idx, + 0) * innovation, -1.e2f, 1.e2f); } + #endif // CONFIG_EKF2_WIND #if defined(CONFIG_EKF2_TERRAIN) @@ -581,40 +642,43 @@ void Ekf::updateHorizontalDeadReckoningstatus() // velocity aiding active if ((_control_status.flags.gps || _control_status.flags.ev_vel) - && isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max) - ) { + && isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max) + ) { inertial_dead_reckoning = false; } // position aiding active if ((_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.aux_gpos) - && isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) - ) { + && isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) + ) { inertial_dead_reckoning = false; } #if defined(CONFIG_EKF2_OPTICAL_FLOW) + // optical flow active if (_control_status.flags.opt_flow - && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max) - ) { + && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max) + ) { inertial_dead_reckoning = false; } else { if (!_control_status.flags.in_air && (_params.flow_ctrl == 1) - && isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max) - ) { + && isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max) + ) { // currently landed, but optical flow aiding should be possible once in air aiding_expected_in_air = true; } } + #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_AIRSPEED) + // air data aiding active if ((_control_status.flags.fuse_aspd && isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max)) - && (_control_status.flags.fuse_beta && isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max)) - ) { + && (_control_status.flags.fuse_beta && isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max)) + ) { // wind_dead_reckoning: no other aiding but air data _control_status.flags.wind_dead_reckoning = inertial_dead_reckoning; @@ -625,13 +689,14 @@ void Ekf::updateHorizontalDeadReckoningstatus() _control_status.flags.wind_dead_reckoning = false; if (!_control_status.flags.in_air && _control_status.flags.fixed_wing - && (_params.beta_fusion_enabled == 1) - && (_params.arsp_thr > 0.f) && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max) - ) { + && (_params.beta_fusion_enabled == 1) + && (_params.arsp_thr > 0.f) && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max) + ) { // currently landed, but air data aiding should be possible once in air aiding_expected_in_air = true; } } + #endif // CONFIG_EKF2_AIRSPEED // zero velocity update @@ -712,6 +777,7 @@ void Ekf::updateGroundEffect() { if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) { #if defined(CONFIG_EKF2_TERRAIN) + if (isTerrainEstimateValid()) { // automatically set ground effect if terrain is valid float height = getHagl(); @@ -719,12 +785,12 @@ void Ekf::updateGroundEffect() } else #endif // CONFIG_EKF2_TERRAIN - if (_control_status.flags.gnd_effect) { - // Turn off ground effect compensation if it times out - if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { - _control_status.flags.gnd_effect = false; + if (_control_status.flags.gnd_effect) { + // Turn off ground effect compensation if it times out + if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { + _control_status.flags.gnd_effect = false; + } } - } } else { _control_status.flags.gnd_effect = false; @@ -736,10 +802,12 @@ void Ekf::updateGroundEffect() void Ekf::resetWind() { #if defined(CONFIG_EKF2_AIRSPEED) + if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) { resetWindUsingAirspeed(_airspeed_sample_delayed); return; } + #endif // CONFIG_EKF2_AIRSPEED resetWindToZero(); @@ -764,7 +832,8 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) { const float alpha = math::constrain((imu_delayed.delta_ang_dt / _params.acc_bias_learn_tc), 0.f, 1.f); const float beta = 1.f - alpha; - _ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, beta * _ang_rate_magnitude_filt); + _ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, + beta * _ang_rate_magnitude_filt); } { @@ -789,8 +858,8 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) // accel bias inhibit const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast(ImuCtrl::AccelBias)) - || is_manoeuvre_level_high - || _fault_status.flags.bad_acc_vertical; + || is_manoeuvre_level_high + || _fault_status.flags.bad_acc_vertical; for (unsigned index = 0; index < State::accel_bias.dof; index++) { bool is_bias_observable = true; @@ -861,6 +930,7 @@ bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, c P(j, i) = P(i, j); } } + #endif constrainStateVariances(); diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 8bc021feedc3..bce6ec88637f 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -86,7 +86,8 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) _time_latest_us = imu_sample.time_us; // the output observer always runs - _output_predictor.calculateOutputStates(imu_sample.time_us, imu_sample.delta_ang, imu_sample.delta_ang_dt, imu_sample.delta_vel, imu_sample.delta_vel_dt); + _output_predictor.calculateOutputStates(imu_sample.time_us, imu_sample.delta_ang, imu_sample.delta_ang_dt, + imu_sample.delta_vel, imu_sample.delta_vel_dt); // accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available if (_imu_down_sampler.update(imu_sample)) { @@ -141,7 +142,8 @@ void EstimatorInterface::setMagData(const magSample &mag_sample) _time_last_mag_buffer_push = _time_latest_us; } else { - ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_MAGNETOMETER @@ -179,13 +181,16 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample) _time_last_gps_buffer_push = _time_latest_us; #if defined(CONFIG_EKF2_GNSS_YAW) + if (PX4_ISFINITE(gnss_sample.yaw)) { _time_last_gps_yaw_buffer_push = _time_latest_us; } + #endif // CONFIG_EKF2_GNSS_YAW } else { - ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_GNSS @@ -223,7 +228,8 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample) _time_last_baro_buffer_push = _time_latest_us; } else { - ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_BAROMETER @@ -260,7 +266,8 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) _airspeed_buffer->push(airspeed_sample_new); } else { - ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_AIRSPEED @@ -298,7 +305,8 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample) _time_last_range_buffer_push = _time_latest_us; } else { - ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_RANGE_FINDER @@ -335,7 +343,8 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow) _flow_buffer->push(optflow_sample_new); } else { - ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_OPTICAL_FLOW @@ -374,7 +383,8 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata) _time_last_ext_vision_buffer_push = _time_latest_us; } else { - ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_EXTERNAL_VISION @@ -411,7 +421,8 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample) _auxvel_buffer->push(auxvel_sample_new); } else { - ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_AUXVEL @@ -446,7 +457,8 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags) _system_flag_buffer->push(system_flags_new); } else { - ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, + _system_flag_buffer->get_newest().time_us, _min_obs_interval_us); } } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index e390f41ea1b1..e7bef6bb0931 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -348,15 +348,15 @@ class EstimatorInterface OutputPredictor _output_predictor{}; #if defined(CONFIG_EKF2_AIRSPEED) - airspeedSample _airspeed_sample_delayed{}; + airspeedSample _airspeed_sample_delayed {}; #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_EXTERNAL_VISION) - extVisionSample _ev_sample_prev{}; + extVisionSample _ev_sample_prev {}; #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_RANGE_FINDER) - RingBuffer *_range_buffer{nullptr}; + RingBuffer *_range_buffer {nullptr}; uint64_t _time_last_range_buffer_push{0}; sensor::SensorRangeFinder _range_sensor{}; @@ -364,7 +364,7 @@ class EstimatorInterface #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) - RingBuffer *_flow_buffer{nullptr}; + RingBuffer *_flow_buffer {nullptr}; flowSample _flow_sample_delayed{}; @@ -387,7 +387,7 @@ class EstimatorInterface float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin #if defined(CONFIG_EKF2_GNSS) - RingBuffer *_gps_buffer{nullptr}; + RingBuffer *_gps_buffer {nullptr}; uint64_t _time_last_gps_buffer_push{0}; gnssSample _gps_sample_delayed{}; @@ -406,7 +406,7 @@ class EstimatorInterface #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_DRAG_FUSION) - RingBuffer *_drag_buffer{nullptr}; + RingBuffer *_drag_buffer {nullptr}; dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) #endif // CONFIG_EKF2_DRAG_FUSION @@ -424,25 +424,25 @@ class EstimatorInterface RingBuffer _imu_buffer{kBufferLengthDefault}; #if defined(CONFIG_EKF2_MAGNETOMETER) - RingBuffer *_mag_buffer{nullptr}; + RingBuffer *_mag_buffer {nullptr}; uint64_t _time_last_mag_buffer_push{0}; #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_AIRSPEED) - RingBuffer *_airspeed_buffer{nullptr}; + RingBuffer *_airspeed_buffer {nullptr}; #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_EXTERNAL_VISION) - RingBuffer *_ext_vision_buffer{nullptr}; + RingBuffer *_ext_vision_buffer {nullptr}; uint64_t _time_last_ext_vision_buffer_push{0}; #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_AUXVEL) - RingBuffer *_auxvel_buffer{nullptr}; + RingBuffer *_auxvel_buffer {nullptr}; #endif // CONFIG_EKF2_AUXVEL - RingBuffer *_system_flag_buffer{nullptr}; + RingBuffer *_system_flag_buffer {nullptr}; #if defined(CONFIG_EKF2_BAROMETER) - RingBuffer *_baro_buffer{nullptr}; + RingBuffer *_baro_buffer {nullptr}; uint64_t _time_last_baro_buffer_push{0}; #endif // CONFIG_EKF2_BAROMETER @@ -457,7 +457,7 @@ class EstimatorInterface uint64_t _wmm_gps_time_last_set{0}; // time WMM last set #if defined(CONFIG_EKF2_MAGNETOMETER) - float _mag_declination_gps{NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad) + float _mag_declination_gps {NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad) float _mag_inclination_gps{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad) float _mag_strength_gps{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (T) diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 51da34e343a5..a7e0b00f541d 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -294,12 +294,15 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const } checks[6] {}; #if defined(CONFIG_EKF2_BAROMETER) + if (_control_status.flags.baro_hgt) { checks[0] = {ReferenceType::PRESSURE, _aid_src_baro_hgt.innovation, _aid_src_baro_hgt.innovation_variance}; } + #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_GNSS) + if (_control_status.flags.gps_hgt) { checks[1] = {ReferenceType::GNSS, _aid_src_gnss_hgt.innovation, _aid_src_gnss_hgt.innovation_variance}; } @@ -307,16 +310,20 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const if (_control_status.flags.gps) { checks[2] = {ReferenceType::GNSS, _aid_src_gnss_vel.innovation[2], _aid_src_gnss_vel.innovation_variance[2]}; } + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) + if (_control_status.flags.rng_hgt) { // Range is a distance to ground measurement, not a direct height observation and has an opposite sign checks[3] = {ReferenceType::GROUND, -_aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance}; } + #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_hgt) { checks[4] = {ReferenceType::GROUND, _aid_src_ev_hgt.innovation, _aid_src_ev_hgt.innovation_variance}; } @@ -324,6 +331,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const if (_control_status.flags.ev_vel) { checks[5] = {ReferenceType::GROUND, _aid_src_ev_vel.innovation[2], _aid_src_ev_vel.innovation_variance[2]}; } + #endif // CONFIG_EKF2_EXTERNAL_VISION // Compute the check based on innovation ratio for all the sources diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp index 826717d7b82e..d16e20a79095 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp @@ -261,7 +261,8 @@ 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 matrix::Vector3f &accel_bias) + const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, const matrix::Vector3f &gyro_bias, + const matrix::Vector3f &accel_bias) { // calculate an average filter update time if (_time_last_correct_states_us != 0) { @@ -365,7 +366,8 @@ void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_corre next_state.vert_vel += vert_vel_correction; // 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.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f * + next_state.dt; // advance the index index = (index + 1) % size; diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.h b/src/modules/ekf2/EKF/output_predictor/output_predictor.h index fee4fdff644c..07248f3dce72 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.h +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.h @@ -67,7 +67,8 @@ class OutputPredictor 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::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias); + const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state, + const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias); void resetQuaternion(const matrix::Quatf &quat_change); diff --git a/src/modules/ekf2/EKF/position_fusion.cpp b/src/modules/ekf2/EKF/position_fusion.cpp index 3ae92a6b6dd5..6cc503de7323 100644 --- a/src/modules/ekf2/EKF/position_fusion.cpp +++ b/src/modules/ekf2/EKF/position_fusion.cpp @@ -40,9 +40,9 @@ void Ekf::updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, con float innovation_variance = getStateVariance()(2) + observation_variance; updateAidSourceStatus(aid_src, time_us, - observation, observation_variance, - innovation, innovation_variance, - innovation_gate); + observation, observation_variance, + innovation, innovation_variance, + innovation_gate); // z special case if there is bad vertical acceleration data, then don't reject measurement, // but limit innovation to prevent spikes that could destabilise the filter @@ -57,8 +57,10 @@ bool Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) { // x & y if (!aid_src.innovation_rejected - && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::pos.idx + 0) - && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::pos.idx + 1) + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], + State::pos.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], + State::pos.idx + 1) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -76,7 +78,8 @@ bool Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) { // z if (!aid_src.innovation_rejected - && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance, State::pos.idx + 2) + && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance, + State::pos.idx + 2) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/velocity_fusion.cpp b/src/modules/ekf2/EKF/velocity_fusion.cpp index 07e590c2111a..349a40092fd4 100644 --- a/src/modules/ekf2/EKF/velocity_fusion.cpp +++ b/src/modules/ekf2/EKF/velocity_fusion.cpp @@ -37,8 +37,10 @@ bool Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src) { // vx, vy if (!aid_src.innovation_rejected - && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0) - && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1) + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], + State::vel.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], + State::vel.idx + 1) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -56,9 +58,12 @@ bool Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) { // vx, vy, vz if (!aid_src.innovation_rejected - && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0) - && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1) - && fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], aid_src.observation_variance[2], State::vel.idx + 2) + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], + State::vel.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], + State::vel.idx + 1) + && fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], aid_src.observation_variance[2], + State::vel.idx + 2) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp index b9e0afd9c38e..7aad6620cee5 100644 --- a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp @@ -301,7 +301,8 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang // Use fixed values for delta angle process noise variances const float d_ang_var = sq(_gyro_noise * delta_ang_dt); - _ekf_gsf[model_index].P = sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P, Vector2f(dvx, dvy), d_vel_var, daz, d_ang_var); + _ekf_gsf[model_index].P = sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P, Vector2f(dvx, + dvy), d_vel_var, daz, d_ang_var); // covariance matrix is symmetrical, so copy upper half to lower half _ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1); diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp index 7714170a62e0..f87dcb8e41c2 100644 --- a/src/modules/ekf2/EKF/yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -145,11 +145,13 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) _output_predictor.resetQuaternion(q_error); #if defined(CONFIG_EKF2_EXTERNAL_VISION) + // update EV attitude error filter if (_ev_q_error_initialized) { const Quatf ev_q_error_updated = (q_error * _ev_q_error_filt.getState()).normalized(); _ev_q_error_filt.reset(ev_q_error_updated); } + #endif // CONFIG_EKF2_EXTERNAL_VISION // record the state change