From a21092804a784d012939f96cc633e7da5f5e8cde Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 1 Nov 2020 11:39:34 -0500 Subject: [PATCH] EKF: remove virtual getters from estimator_interface --- EKF/airspeed_fusion.cpp | 7 +- EKF/control.cpp | 2 +- EKF/covariance.cpp | 2 - EKF/ekf.h | 201 +++++++++++++------------ EKF/ekf_helper.cpp | 35 ++--- EKF/estimator_interface.cpp | 8 - EKF/estimator_interface.h | 287 +++++++++--------------------------- EKF/mag_control.cpp | 30 ---- EKF/terrain_estimator.cpp | 23 +-- 9 files changed, 194 insertions(+), 401 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 0188c3397e..be5385a3e2 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -161,12 +161,7 @@ void Ekf::fuseAirspeed() } } -Vector2f Ekf::getWindVelocityVariance() const -{ - return P.slice<2, 2>(22,22).diag(); -} - -void Ekf::get_true_airspeed(float *tas) +void Ekf::get_true_airspeed(float *tas) const { const float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2))); memcpy(tas, &tempvar, sizeof(float)); diff --git a/EKF/control.cpp b/EKF/control.cpp index a1e9cceac0..265cccf556 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -77,7 +77,7 @@ void Ekf::controlFusionModes() height_source = "unknown"; } - if(height_source){ + if (height_source){ ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length); } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 7f2491113b..d874283c0c 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -1034,7 +1034,6 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f& KHP) { return healthy; } - void Ekf::resetMagRelatedCovariances() { resetQuatCov(); @@ -1119,6 +1118,5 @@ void Ekf::resetWindCovariance() } else { // without airspeed, start with a small initial uncertainty to improve the initial estimate P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty); - } } diff --git a/EKF/ekf.h b/EKF/ekf.h index c1272c0596..d84dfa66f6 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -44,6 +44,8 @@ #include "estimator_interface.h" +#include "EKFGSF_yaw.h" + class Ekf final : public EstimatorInterface { public: @@ -61,78 +63,73 @@ class Ekf final : public EstimatorInterface // initialise variables to sane values (also interface class) bool init(uint64_t timestamp) override; - // set the internal states and status to their default value - void reset() override; - - bool initialiseTilt(); - // should be called every time new data is pushed into the filter - bool update() override; + bool update(); - void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const override; - void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const override; - void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const override; + void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const; + void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const; + void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const; - void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const override; - void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const override; - void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const override; + void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const; + void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const; + void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const; - void getBaroHgtInnov(float &baro_hgt_innov) const override { baro_hgt_innov = _baro_hgt_innov(2); } - void getBaroHgtInnovVar(float &baro_hgt_innov_var) const override { baro_hgt_innov_var = _baro_hgt_innov_var(2); } - void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const override { baro_hgt_innov_ratio = _baro_hgt_test_ratio(1); } + void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov(2); } + void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _baro_hgt_innov_var(2); } + void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _baro_hgt_test_ratio(1); } - void getRngHgtInnov(float &rng_hgt_innov) const override { rng_hgt_innov = _rng_hgt_innov(2); } - void getRngHgtInnovVar(float &rng_hgt_innov_var) const override { rng_hgt_innov_var = _rng_hgt_innov_var(2); } - void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const override { rng_hgt_innov_ratio = _rng_hgt_test_ratio(1); } + void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov(2); } + void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _rng_hgt_innov_var(2); } + void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _rng_hgt_test_ratio(1); } - void getAuxVelInnov(float aux_vel_innov[2]) const override; - void getAuxVelInnovVar(float aux_vel_innov[2]) const override; - void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const override { aux_vel_innov_ratio = _aux_vel_test_ratio(0); } + void getAuxVelInnov(float aux_vel_innov[2]) const; + void getAuxVelInnovVar(float aux_vel_innov[2]) const; + void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = _aux_vel_test_ratio(0); } - void getFlowInnov(float flow_innov[2]) const override { _flow_innov.copyTo(flow_innov); } - void getFlowInnovVar(float flow_innov_var[2]) const override { _flow_innov_var.copyTo(flow_innov_var); } - void getFlowInnovRatio(float &flow_innov_ratio) const override { flow_innov_ratio = _optflow_test_ratio; } - const Vector2f &getFlowVelBody() const override { return _flow_vel_body; } - const Vector2f &getFlowVelNE() const override { return _flow_vel_ne; } - const Vector2f &getFlowCompensated() const override { return _flow_compensated_XY_rad; } - const Vector2f &getFlowUncompensated() const override { return _flow_sample_delayed.flow_xy_rad; } - const Vector3f &getFlowGyro() const override { return _flow_sample_delayed.gyro_xyz; } + void getFlowInnov(float flow_innov[2]) const { _flow_innov.copyTo(flow_innov); } + void getFlowInnovVar(float flow_innov_var[2]) const { _flow_innov_var.copyTo(flow_innov_var); } + void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = _optflow_test_ratio; } + const Vector2f &getFlowVelBody() const { return _flow_vel_body; } + const Vector2f &getFlowVelNE() const { return _flow_vel_ne; } + const Vector2f &getFlowCompensated() const { return _flow_compensated_XY_rad; } + const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_xy_rad; } + const Vector3f &getFlowGyro() const { return _flow_sample_delayed.gyro_xyz; } - void getHeadingInnov(float &heading_innov) const override { heading_innov = _heading_innov; } - void getHeadingInnovVar(float &heading_innov_var) const override { heading_innov_var = _heading_innov_var; } + void getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; } + void getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; } - void getHeadingInnovRatio(float &heading_innov_ratio) const override { heading_innov_ratio = _yaw_test_ratio; } - void getMagInnov(float mag_innov[3]) const override { _mag_innov.copyTo(mag_innov); } - void getMagInnovVar(float mag_innov_var[3]) const override { _mag_innov_var.copyTo(mag_innov_var); } - void getMagInnovRatio(float &mag_innov_ratio) const override { mag_innov_ratio = _mag_test_ratio.max(); } + void getHeadingInnovRatio(float &heading_innov_ratio) const { heading_innov_ratio = _yaw_test_ratio; } + void getMagInnov(float mag_innov[3]) const { _mag_innov.copyTo(mag_innov); } + void getMagInnovVar(float mag_innov_var[3]) const { _mag_innov_var.copyTo(mag_innov_var); } + void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = _mag_test_ratio.max(); } - void getDragInnov(float drag_innov[2]) const override { _drag_innov.copyTo(drag_innov); } - void getDragInnovVar(float drag_innov_var[2]) const override { _drag_innov_var.copyTo(drag_innov_var); } - void getDragInnovRatio(float drag_innov_ratio[2]) const override { _drag_test_ratio.copyTo(drag_innov_ratio); } + void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); } + void getDragInnovVar(float drag_innov_var[2]) const { _drag_innov_var.copyTo(drag_innov_var); } + void getDragInnovRatio(float drag_innov_ratio[2]) const { _drag_test_ratio.copyTo(drag_innov_ratio); } - void getAirspeedInnov(float &airspeed_innov) const override { airspeed_innov = _airspeed_innov; } - void getAirspeedInnovVar(float &airspeed_innov_var) const override { airspeed_innov_var = _airspeed_innov_var; } - void getAirspeedInnovRatio(float &airspeed_innov_ratio) const override { airspeed_innov_ratio = _tas_test_ratio; } + void getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _airspeed_innov; } + void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _airspeed_innov_var; } + void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _tas_test_ratio; } - void getBetaInnov(float &beta_innov) const override { beta_innov = _beta_innov; } - void getBetaInnovVar(float &beta_innov_var) const override { beta_innov_var = _beta_innov_var; } - void getBetaInnovRatio(float &beta_innov_ratio) const override { beta_innov_ratio = _beta_test_ratio; } + void getBetaInnov(float &beta_innov) const { beta_innov = _beta_innov; } + void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _beta_innov_var; } + void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _beta_test_ratio; } - void getHaglInnov(float &hagl_innov) const override { hagl_innov = _hagl_innov; } - void getHaglInnovVar(float &hagl_innov_var) const override { hagl_innov_var = _hagl_innov_var; } - void getHaglInnovRatio(float &hagl_innov_ratio) const override { hagl_innov_ratio = _hagl_test_ratio; } + void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; } + void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; } + void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; } // get the state vector at the delayed time horizon - matrix::Vector getStateAtFusionHorizonAsVector() const override; + matrix::Vector getStateAtFusionHorizonAsVector() const; // get the wind velocity in m/s const Vector2f &getWindVelocity() const { return _state.wind_vel; }; // get the wind velocity var - Vector2f getWindVelocityVariance() const override; + Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22,22).diag(); } // get the true airspeed in m/s - void get_true_airspeed(float *tas) override; + void get_true_airspeed(float *tas) const; // get the full covariance matrix const matrix::SquareMatrix &covariances() const { return P; } @@ -154,26 +151,26 @@ class Ekf final : public EstimatorInterface // get the ekf WGS-84 origin position and height and the system time it was last set // return true if the origin is valid - bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) override; + bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) const; // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position - void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) override; + void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const; // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position - void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) override; + void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const; // get the 1-sigma horizontal and vertical velocity uncertainty - void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) override; + void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const; // get the vehicle control limits required by the estimator to keep within sensor limitations - void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) override; + void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const; /* Reset all IMU bias states and covariances to initial alignment values. Use when the IMU sensor has changed. Returns true if reset performed, false if rejected due to less than 10 seconds lapsed since last reset. */ - bool reset_imu_bias() override; + bool reset_imu_bias(); Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); }; @@ -183,14 +180,6 @@ class Ekf final : public EstimatorInterface // error magnitudes (rad), (m/sec), (m) const Vector3f &getOutputTrackingError() const { return _output_tracking_error; } - /* - Returns following IMU vibration metrics in the following array locations - 0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle) - 1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) - 2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) - */ - const Vector3f &getImuVibrationMetrics() const { return _vibe_metrics; } - /* First argument returns GPS drift metrics in the following array locations 0 : Horizontal position drift rate (m/s) @@ -199,22 +188,22 @@ class Ekf final : public EstimatorInterface Second argument returns true when IMU movement is blocking the drift calculation Function returns true if the metrics have been updated and not returned previously by this function */ - bool get_gps_drift_metrics(float drift[3], bool *blocked) override; + bool get_gps_drift_metrics(float drift[3], bool *blocked); // return true if the global position estimate is valid - bool global_position_is_valid() override; - - // check if the EKF is dead reckoning horizontal velocity using inertial data only - void update_deadreckoning_status(); - - bool isTerrainEstimateValid() const override; + // return true if the origin is set we are not doing unconstrained free inertial navigation + // and have not started using synthetic position observations to constrain drift + bool global_position_is_valid() const + { + return (_NED_origin_initialised && !_deadreckon_time_exceeded && !_using_synthetic_position); + } - uint8_t getTerrainEstimateSensorBitfield() const override {return _hagl_sensor_status.value;} + bool isTerrainEstimateValid() const { return _hagl_valid; }; - void updateTerrainValidity(); + uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; } // get the estimated terrain vertical position relative to the NED origin - float getTerrainVertPos() const override; + float getTerrainVertPos() const { return _terrain_vpos; }; // get the terrain variance float get_terrain_var() const { return _terrain_var; } @@ -226,30 +215,38 @@ class Ekf final : public EstimatorInterface Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get GPS check status - void get_gps_check_status(uint16_t *val) override { *val = _gps_check_fail_status.value; } + void get_gps_check_status(uint16_t *val) const { *val = _gps_check_fail_status.value; } // return the amount the local vertical position changed in the last reset and the number of reset events - void get_posD_reset(float *delta, uint8_t *counter) override {*delta = _state_reset_status.posD_change; *counter = _state_reset_status.posD_counter;} + void get_posD_reset(float *delta, uint8_t *counter) const + { + *delta = _state_reset_status.posD_change; + *counter = _state_reset_status.posD_counter; + } // return the amount the local vertical velocity changed in the last reset and the number of reset events - void get_velD_reset(float *delta, uint8_t *counter) override {*delta = _state_reset_status.velD_change; *counter = _state_reset_status.velD_counter;} + void get_velD_reset(float *delta, uint8_t *counter) const + { + *delta = _state_reset_status.velD_change; + *counter = _state_reset_status.velD_counter; + } // return the amount the local horizontal position changed in the last reset and the number of reset events - void get_posNE_reset(float delta[2], uint8_t *counter) override + void get_posNE_reset(float delta[2], uint8_t *counter) const { _state_reset_status.posNE_change.copyTo(delta); *counter = _state_reset_status.posNE_counter; } // return the amount the local horizontal velocity changed in the last reset and the number of reset events - void get_velNE_reset(float delta[2], uint8_t *counter) override + void get_velNE_reset(float delta[2], uint8_t *counter) const { _state_reset_status.velNE_change.copyTo(delta); *counter = _state_reset_status.velNE_counter; } // return the amount the quaternion has changed in the last reset and the number of reset events - void get_quat_reset(float delta_quat[4], uint8_t *counter) override + void get_quat_reset(float delta_quat[4], uint8_t *counter) const { _state_reset_status.quat_change.copyTo(delta_quat); *counter = _state_reset_status.quat_counter; @@ -261,10 +258,10 @@ class Ekf final : public EstimatorInterface // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned. void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, - float &hagl, float &beta) override; + float &hagl, float &beta) const; // return a bitmask integer that describes which state estimates can be used for flight control - void get_ekf_soln_status(uint16_t *status) override; + void get_ekf_soln_status(uint16_t *status) const; // return the quaternion defining the rotation from the External Vision to the EKF reference frame matrix::Quatf getVisionAlignmentQuaternion() const { return Quatf(_R_ev_to_ekf); }; @@ -278,15 +275,26 @@ class Ekf final : public EstimatorInterface // get solution data from the EKF-GSF emergency yaw estimator // returns false when data is not available bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], - float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) override; + float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]); + +private: + + // set the internal states and status to their default value + void reset(); + + bool initialiseTilt(); // Request the EKF reset the yaw to the estimate from the internal EKF-GSF filter // and reset the velocity and position states to the GPS. This will cause the EKF // to ignore the magnetometer for the remainder of flight. // This should only be used as a last resort before activating a loss of navigation failsafe - void requestEmergencyNavReset() override { _do_ekfgsf_yaw_reset = true; } + void requestEmergencyNavReset() { _do_ekfgsf_yaw_reset = true; } + + // check if the EKF is dead reckoning horizontal velocity using inertial data only + void update_deadreckoning_status(); + + void updateTerrainValidity(); -private: struct { uint8_t velNE_counter; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) uint8_t velD_counter; ///< number of vertical velocity reset events (allow to wrap if count exceeds 255) @@ -620,8 +628,8 @@ class Ekf final : public EstimatorInterface // return true if the initialisation is successful bool initHagl(); - bool shouldUseRangeFinderForHagl() const; - bool shouldUseOpticalFlowForHagl() const; + bool shouldUseRangeFinderForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder); } + bool shouldUseOpticalFlowForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow); } // run the terrain estimator void runTerrainEstimator(); @@ -722,7 +730,7 @@ class Ekf final : public EstimatorInterface // and a scalar innovation value void fuse(const Vector24f &K, float innovation); - float compensateBaroForDynamicPressure(float baro_alt_uncompensated) override; + float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const override; // calculate the earth rotation vector from a given latitude Vector3f calcEarthRateNED(float lat_rad) const; @@ -750,28 +758,28 @@ class Ekf final : public EstimatorInterface bool noOtherYawAidingThanMag() const; void checkHaglYawResetReq(); - float getTerrainVPos() const; + float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; } void runOnGroundYawReset(); - bool isYawResetAuthorized() const; + bool isYawResetAuthorized() const { return !_is_yaw_fusion_inhibited; } bool canResetMagHeading() const; void runInAirYawReset(); - bool canRealignYawUsingGps() const; + bool canRealignYawUsingGps() const { return _control_status.flags.fixed_wing; } void runVelPosReset(); void selectMagAuto(); void check3DMagFusionSuitability(); void checkYawAngleObservability(); void checkMagBiasObservability(); - bool isYawAngleObservable() const; - bool isMagBiasObservable() const; + bool isYawAngleObservable() const { return _yaw_angle_observable; } + bool isMagBiasObservable() const { return _mag_bias_observable; } bool canUse3DMagFusion() const; void checkMagDeclRequired(); void checkMagInhibition(); bool shouldInhibitMag() const; void checkMagFieldStrength(); - bool isStrongMagneticDisturbance() const; + bool isStrongMagneticDisturbance() const { return _control_status.flags.mag_field_disturbed; } bool isMeasuredMatchingGpsMagStrength() const; bool isMeasuredMatchingAverageMagStrength() const; static bool isMeasuredMatchingExpected(float measured, float expected, float gate); @@ -809,7 +817,7 @@ class Ekf final : public EstimatorInterface // determine if flight condition is suitable to use range finder instead of the primary height sensor void checkRangeAidSuitability(); - bool isRangeAidSuitable() { return _is_range_aid_suitable; } + bool isRangeAidSuitable() const { return _is_range_aid_suitable; } // set control flags to use baro height void setControlBaroHeight(); @@ -918,6 +926,9 @@ class Ekf final : public EstimatorInterface // Declarations used to control use of the EKF-GSF yaw estimator + // yaw estimator instance + EKFGSF_yaw _yawEstimator; + int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec) uint64_t _time_last_on_ground_us{0}; ///< last tine we were on the ground (uSec) bool _do_ekfgsf_yaw_reset{false}; // true when an emergency yaw reset has been requested diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 697c821b71..26510cf1f0 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -153,7 +153,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel) _state_reset_status.velD_counter++; } - void Ekf::resetHorizontalPosition() { // let the next odometry update know that the previous value of states cannot be used to calculate the change in position @@ -300,7 +299,6 @@ void Ekf::resetHeight() } else { _state.pos(2) = _ev_sample_delayed.pos(2); } - } // reset the vertical velocity state @@ -574,7 +572,7 @@ void Ekf::constrainStates() _state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f); } -float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) +float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const { // calculate static pressure error = Pmeas - Ptruth // model position error sensitivity as a body fixed ellipse with a different scale in the positive and @@ -696,7 +694,7 @@ matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const // get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set // return true if the origin is valid -bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) +bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) const { memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t)); memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s)); @@ -726,7 +724,7 @@ bool Ekf::get_gps_drift_metrics(float drift[3], bool *blocked) } // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position -void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) +void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const { // report absolute accuracy taking into account the uncertainty in location of the origin // If not aiding, return 0 for horizontal position estimate as no estimate is available @@ -748,7 +746,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) } // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position -void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) +void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const { // TODO - allow for baro drift in vertical position error float hpos_err = sqrtf(P(7, 7) + P(8, 8)); @@ -765,7 +763,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) } // get the 1-sigma horizontal and vertical velocity uncertainty -void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) +void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const { float hvel_err = sqrtf(P(4, 4) + P(5, 5)); @@ -805,7 +803,7 @@ vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting hagl_min : Minimum height above ground (meters). NaN when limiting is not needed. hagl_max : Maximum height above ground (meters). NaN when limiting is not needed. */ -void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) +void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const { // Calculate range finder limits const float rangefinder_hagl_min = _range_sensor.getValidMinVal(); @@ -845,14 +843,12 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl *hagl_min = fmaxf(rangefinder_hagl_min, flow_hagl_min); *hagl_max = fminf(rangefinder_hagl_max, flow_hagl_max); } - } bool Ekf::reset_imu_bias() { if (_imu_sample_delayed.time_us - _last_imu_bias_cov_reset_us < (uint64_t)10e6) { return false; - } // Zero the delta angle and delta velocity bias states @@ -877,7 +873,7 @@ bool Ekf::reset_imu_bias() // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned. void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, - float &hagl, float &beta) + float &hagl, float &beta) const { // return the integer bitmask containing the consistency check pass/fail status status = _innov_check_fail_status.value; @@ -900,7 +896,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f } // return a bitmask integer that describes which state estimates are valid -void Ekf::get_ekf_soln_status(uint16_t *status) +void Ekf::get_ekf_soln_status(uint16_t *status) const { ekf_solution_status soln_status; // TODO: Is this accurate enough? @@ -941,13 +937,6 @@ void Ekf::uncorrelateQuatFromOtherStates() P.slice<4, _k_num_states - 4>(0, 4) = 0.f; } -bool Ekf::global_position_is_valid() -{ - // return true if the origin is set we are not doing unconstrained free inertial navigation - // and have not started using synthetic position observations to constrain drift - return (_NED_origin_initialised && !_deadreckon_time_exceeded && !_using_synthetic_position); -} - // return true if we are totally reliant on inertial dead-reckoning for position void Ekf::update_deadreckoning_status() { @@ -1533,7 +1522,7 @@ bool Ekf::resetYawToEKFGSF() // data and the yaw estimate has converged float new_yaw, new_yaw_variance; - if (!yawEstimator.getYawData(&new_yaw, &new_yaw_variance)) { + if (!_yawEstimator.getYawData(&new_yaw, &new_yaw_variance)) { return false; } @@ -1569,7 +1558,7 @@ bool Ekf::resetYawToEKFGSF() bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) { - return yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight); + return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight); } void Ekf::runYawEKFGSF() @@ -1584,12 +1573,12 @@ void Ekf::runYawEKFGSF() } const Vector3f imu_gyro_bias = getGyroBias(); - yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias); + _yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias); // basic sanity check on GPS velocity data if (_gps_data_ready && _gps_sample_delayed.vacc > FLT_EPSILON && ISFINITE(_gps_sample_delayed.vel(0)) && ISFINITE(_gps_sample_delayed.vel(1))) { - yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc); + _yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc); } } diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index ad54bdcb53..ea945c98e3 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -567,13 +567,6 @@ void EstimatorInterface::unallocate_buffers() _output_vert_buffer.unallocate(); _drag_buffer.unallocate(); _auxvel_buffer.unallocate(); - -} - -bool EstimatorInterface::local_position_is_valid() -{ - // return true if we are not doing unconstrained free inertial navigation - return !_deadreckon_time_exceeded; } bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const @@ -610,7 +603,6 @@ void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name) void EstimatorInterface::print_status() { ECL_INFO("local position valid: %s", local_position_is_valid() ? "yes" : "no"); - ECL_INFO("global position valid: %s", global_position_is_valid() ? "yes" : "no"); ECL_INFO("imu buffer: %d (%d Bytes)", _imu_buffer.get_length(), _imu_buffer.get_total_size()); ECL_INFO("gps buffer: %d (%d Bytes)", _gps_buffer.get_length(), _gps_buffer.get_total_size()); diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index a190ea2b5d..9aee41ff8a 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -46,7 +46,6 @@ #include "RingBuffer.h" #include #include "imu_down_sampler.hpp" -#include "EKFGSF_yaw.h" #include "sensor_range_finder.hpp" #include "utils.hpp" @@ -58,106 +57,19 @@ using namespace estimator; class EstimatorInterface { - public: - EstimatorInterface(): _imu_down_sampler(FILTER_UPDATE_PERIOD_S) {}; - virtual ~EstimatorInterface() = default; - - virtual bool init(uint64_t timestamp) = 0; - virtual void reset() = 0; - - virtual bool update() = 0; - - virtual void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const = 0; - virtual void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const = 0; - virtual void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const = 0; - - virtual void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const = 0; - virtual void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const = 0; - virtual void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const = 0; - - virtual void getBaroHgtInnov(float &baro_hgt_innov) const = 0; - virtual void getBaroHgtInnovVar(float &baro_hgt_innov_var) const = 0; - virtual void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const = 0; - - virtual void getRngHgtInnov(float &rng_hgt_innov) const = 0; - virtual void getRngHgtInnovVar(float &rng_hgt_innov_var) const = 0; - virtual void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const = 0; - - virtual void getAuxVelInnov(float aux_vel_innov[2]) const = 0; - virtual void getAuxVelInnovVar(float aux_vel_innov[2]) const = 0; - virtual void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const = 0; - - virtual void getFlowInnov(float flow_innov[2]) const = 0; - virtual void getFlowInnovVar(float flow_innov_var[2]) const = 0; - virtual void getFlowInnovRatio(float &flow_innov_ratio) const = 0; - virtual const Vector2f &getFlowVelBody() const = 0; - virtual const Vector2f &getFlowVelNE() const = 0; - virtual const Vector2f &getFlowCompensated() const = 0; - virtual const Vector2f &getFlowUncompensated() const = 0; - virtual const Vector3f &getFlowGyro() const = 0; - - virtual void getHeadingInnov(float &heading_innov) const = 0; - virtual void getHeadingInnovVar(float &heading_innov_var) const = 0; - virtual void getHeadingInnovRatio(float &heading_innov_ratio) const = 0; - - virtual void getMagInnov(float mag_innov[3]) const = 0; - virtual void getMagInnovVar(float mag_innov_var[3]) const = 0; - virtual void getMagInnovRatio(float &mag_innov_ratio) const = 0; - - virtual void getDragInnov(float drag_innov[2]) const = 0; - virtual void getDragInnovVar(float drag_innov_var[2]) const = 0; - virtual void getDragInnovRatio(float drag_innov_ratio[2]) const = 0; - - virtual void getAirspeedInnov(float &airspeed_innov) const = 0; - virtual void getAirspeedInnovVar(float &get_airspeed_innov_var) const = 0; - virtual void getAirspeedInnovRatio(float &airspeed_innov_ratio) const = 0; - - virtual void getBetaInnov(float &beta_innov) const = 0; - virtual void getBetaInnovVar(float &get_beta_innov_var) const = 0; - virtual void getBetaInnovRatio(float &beta_innov_ratio) const = 0; - - virtual void getHaglInnov(float &hagl_innov) const = 0; - virtual void getHaglInnovVar(float &hagl_innov_var) const = 0; - virtual void getHaglInnovRatio(float &hagl_innov_ratio) const = 0; - - virtual matrix::Vector getStateAtFusionHorizonAsVector() const = 0; - - virtual Vector2f getWindVelocityVariance() const = 0; - - virtual void get_true_airspeed(float *tas) = 0; - - /* - First argument returns GPS drift metrics in the following array locations - 0 : Horizontal position drift rate (m/s) - 1 : Vertical position drift rate (m/s) - 2 : Filtered horizontal velocity (m/s) - Second argument returns true when IMU movement is blocking the drift calculation - Function returns true if the metrics have been updated and not returned previously by this function - */ - virtual bool get_gps_drift_metrics(float drift[3], bool *blocked) = 0; - - // get the ekf WGS-84 origin position and height and the system time it was last set - // return true if the origin is valid - virtual bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0; - - // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position - virtual void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) = 0; - - // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position - virtual void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) = 0; - - // get the 1-sigma horizontal and vertical velocity uncertainty - virtual void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) = 0; - - // get the vehicle control limits required by the estimator to keep within sensor limitations - virtual void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) = 0; - // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined virtual bool collect_gps(const gps_message &gps) = 0; void setIMUData(const imuSample &imu_sample); + /* + Returns following IMU vibration metrics in the following array locations + 0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle) + 1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) + 2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) + */ + const Vector3f &getImuVibrationMetrics() const { return _vibe_metrics; } void setMagData(const magSample &mag_sample); @@ -182,29 +94,22 @@ class EstimatorInterface parameters *getParamHandle() { return &_params; } // set vehicle landed status data - void set_in_air_status(bool in_air) {_control_status.flags.in_air = in_air;} - - /* - Reset all IMU bias states and covariances to initial alignment values. - Use when the IMU sensor has changed. - Returns true if reset performed, false if rejected due to less than 10 seconds lapsed since last reset. - */ - virtual bool reset_imu_bias() = 0; + void set_in_air_status(bool in_air) { _control_status.flags.in_air = in_air; } // return true if the attitude is usable - bool attitude_valid() { return ISFINITE(_output_new.quat_nominal(0)) && _control_status.flags.tilt_align; } + bool attitude_valid() const { return ISFINITE(_output_new.quat_nominal(0)) && _control_status.flags.tilt_align; } // get vehicle landed status data - bool get_in_air_status() {return _control_status.flags.in_air;} + bool get_in_air_status() const { return _control_status.flags.in_air; } // get wind estimation status - bool get_wind_status() { return _control_status.flags.wind; } + bool get_wind_status() const { return _control_status.flags.wind; } // set vehicle is fixed wing status - void set_is_fixed_wing(bool is_fixed_wing) {_control_status.flags.fixed_wing = is_fixed_wing;} + void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; } // set flag if synthetic sideslip measurement should be fused - void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air);} + void set_fuse_beta_flag(bool fuse_beta) { _control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air); } // set flag if static pressure rise due to ground effect is expected // use _params.gnd_effect_deadzone to adjust for expected rise in static pressure @@ -216,7 +121,7 @@ class EstimatorInterface } // set air density used by the multi-rotor specific drag force fusion - void set_air_density(float air_density) {_air_density = air_density;} + void set_air_density(float air_density) { _air_density = air_density; } // set sensor limitations reported by the rangefinder void set_rangefinder_limits(float min_distance, float max_distance) @@ -232,9 +137,6 @@ class EstimatorInterface _flow_max_distance = max_distance; } - // return true if the global position estimate is valid - virtual bool global_position_is_valid() = 0; - // the flags considered are opt_flow, gps, ev_vel and ev_pos bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const; @@ -256,29 +158,16 @@ class EstimatorInterface int getNumberOfActiveHorizontalAidingSources() const; - // return true if the EKF is dead reckoning the position using inertial data only - bool inertial_dead_reckoning() {return _is_dead_reckoning;} - - virtual bool isTerrainEstimateValid() const = 0; - //[[deprecated("Replaced by isTerrainEstimateValid")]] - bool get_terrain_valid() { return isTerrainEstimateValid(); } - - virtual uint8_t getTerrainEstimateSensorBitfield() const = 0; - - // get the estimated terrain vertical position relative to the NED origin - virtual float getTerrainVertPos() const = 0; - // return true if the local position estimate is valid - bool local_position_is_valid(); + bool local_position_is_valid() const { return !_deadreckon_time_exceeded; } + + // return true if the EKF is dead reckoning the position using inertial data only + bool inertial_dead_reckoning() const { return _is_dead_reckoning; } const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; } // get the velocity of the body frame origin in local NED earth frame - Vector3f getVelocity() const - { - const Vector3f vel_earth = _output_new.vel - _vel_imu_rel_body_ned; - return vel_earth; - } + Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; } // get the velocity derivative in earth frame const Vector3f &getVelocityDerivative() const { return _vel_deriv; } @@ -298,7 +187,7 @@ class EstimatorInterface // Get the value of magnetic declination in degrees to be saved for use at the next startup // Returns true when the declination can be saved // At the next startup, set param.mag_declination_deg to the value saved - bool get_mag_decl_deg(float *val) + bool get_mag_decl_deg(float *val) const { if (_NED_origin_initialised && (_params.mag_declination_source & MASK_SAVE_GEO_DECL)) { *val = math::degrees(_mag_declination_gps); @@ -310,70 +199,36 @@ class EstimatorInterface } // get EKF mode status - void get_control_mode(uint32_t *val) { *val = _control_status.value; } + void get_control_mode(uint32_t *val) const { *val = _control_status.value; } // get EKF internal fault status - void get_filter_fault_status(uint16_t *val) { *val = _fault_status.value; } + void get_filter_fault_status(uint16_t *val) const { *val = _fault_status.value; } bool isVehicleAtRest() const { return _control_status.flags.vehicle_at_rest; } - // get GPS check status - virtual void get_gps_check_status(uint16_t *val) = 0; - - // return the amount the local vertical position changed in the last reset and the number of reset events - virtual void get_posD_reset(float *delta, uint8_t *counter) = 0; - - // return the amount the local vertical velocity changed in the last reset and the number of reset events - virtual void get_velD_reset(float *delta, uint8_t *counter) = 0; - - // return the amount the local horizontal position changed in the last reset and the number of reset events - virtual void get_posNE_reset(float delta[2], uint8_t *counter) = 0; - - // return the amount the local horizontal velocity changed in the last reset and the number of reset events - virtual void get_velNE_reset(float delta[2], uint8_t *counter) = 0; - - // return the amount the quaternion has changed in the last reset and the number of reset events - virtual void get_quat_reset(float delta_quat[4], uint8_t *counter) = 0; - - // get EKF innovation consistency check status information comprising of: - // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check - // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. - // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF - // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned. - virtual void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, - float &hagl, float &beta) = 0; - - // return a bitmask integer that describes which state estimates can be used for flight control - virtual void get_ekf_soln_status(uint16_t *status) = 0; - // Getter for the average imu update period in s float get_dt_imu_avg() const { return _dt_imu_avg; } // Getter for the imu sample on the delayed time horizon - const imuSample &get_imu_sample_delayed() { return _imu_sample_delayed; } + const imuSample &get_imu_sample_delayed() const { return _imu_sample_delayed; } // Getter for the baro sample on the delayed time horizon - const baroSample &get_baro_sample_delayed() { return _baro_sample_delayed; } + const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; } void print_status(); static constexpr unsigned FILTER_UPDATE_PERIOD_MS{10}; // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta static constexpr float FILTER_UPDATE_PERIOD_S{FILTER_UPDATE_PERIOD_MS * 0.001f}; - // request the EKF reset the yaw to the estimate from the internal EKF-GSF filter - // argment should be incremented only when a new reset is required - virtual void requestEmergencyNavReset() = 0; +protected: - // get ekf-gsf debug data - virtual bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], - float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) = 0; + EstimatorInterface() = default; + virtual ~EstimatorInterface() = default; -protected: + virtual bool init(uint64_t timestamp) = 0; parameters _params; // filter parameters - ImuDownSampler _imu_down_sampler; - /* OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer which sets the maximum frequency at which we can process non-IMU measurements. Measurements that @@ -391,8 +246,6 @@ class EstimatorInterface */ uint8_t _imu_buffer_length{0}; - unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec) - float _dt_imu_avg{0.0f}; // average imu update period in s imuSample _imu_sample_delayed{}; // captures the imu sample on the delayed time horizon @@ -409,9 +262,6 @@ class EstimatorInterface dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) auxVelSample _auxvel_sample_delayed{}; - // Used by the multi-rotor specific drag force fusion - uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate - float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec) float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) // Sensor limitations @@ -460,13 +310,6 @@ class EstimatorInterface bool _deadreckon_time_exceeded{true}; // true if the horizontal nav solution has been deadreckoning for too long and is invalid bool _is_wind_dead_reckoning{false}; // true if we are navigationg reliant on wind relative measurements - // IMU vibration and movement monitoring - Vector3f _delta_ang_prev; // delta angle from the previous IMU measurement - Vector3f _delta_vel_prev; // delta velocity from the previous IMU measurement - Vector3f _vibe_metrics; // IMU vibration metrics - // [0] Level of coning vibration in the IMU delta angles (rad^2) - // [1] high frequency vibration level in the IMU delta angle data (rad) - // [2] high frequency vibration level in the IMU delta velocity data (m/s) float _gps_drift_metrics[3] {}; // Array containing GPS drift metrics // [0] Horizontal position drift rate (m/s) // [1] Vertical position drift rate (m/s) @@ -488,20 +331,6 @@ class EstimatorInterface RingBuffer _drag_buffer; RingBuffer _auxvel_buffer; - // yaw estimator instance - EKFGSF_yaw yawEstimator; - - // observation buffer final allocation failed - bool _gps_buffer_fail{false}; - bool _mag_buffer_fail{false}; - bool _baro_buffer_fail{false}; - bool _range_buffer_fail{false}; - bool _airspeed_buffer_fail{false}; - bool _flow_buffer_fail{false}; - bool _ev_buffer_fail{false}; - bool _drag_buffer_fail{false}; - bool _auxvel_buffer_fail{false}; - // timestamps of latest in buffer saved measurement in microseconds uint64_t _time_last_imu{0}; uint64_t _time_last_gps{0}; @@ -515,24 +344,11 @@ class EstimatorInterface //last time the baro ground effect compensation was turned on externally (uSec) uint64_t _time_last_gnd_effect_on{0}; - // Used to downsample magnetometer data - Vector3f _mag_data_sum; - uint8_t _mag_sample_count {0}; - uint64_t _mag_timestamp_sum {0}; - - // Used to down sample barometer data - float _baro_alt_sum {0.0f}; // summed pressure altitude readings (m) - uint8_t _baro_sample_count {0}; // number of barometric altitude measurements summed - uint64_t _baro_timestamp_sum {0}; // summed timestamp to provide the timestamp of the averaged sample - fault_status_u _fault_status{}; // allocate data buffers and initialize interface variables bool initialise_interface(uint64_t timestamp); - // free buffer memory - void unallocate_buffers(); - 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) @@ -543,12 +359,55 @@ class EstimatorInterface // this is the previous status of the filter control modes - used to detect mode transitions filter_control_status_u _control_status_prev{}; + virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const = 0; + +private: + inline void setDragData(const imuSample &imu); inline void computeVibrationMetric(const imuSample &imu); inline bool checkIfVehicleAtRest(float dt, const imuSample &imu); - virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) = 0; - void printBufferAllocationFailed(const char *buffer_name); + + // free buffer memory + void unallocate_buffers(); + + ImuDownSampler _imu_down_sampler{FILTER_UPDATE_PERIOD_S}; + + unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec) + + // IMU vibration and movement monitoring + Vector3f _delta_ang_prev; // delta angle from the previous IMU measurement + Vector3f _delta_vel_prev; // delta velocity from the previous IMU measurement + Vector3f _vibe_metrics; // IMU vibration metrics + // [0] Level of coning vibration in the IMU delta angles (rad^2) + // [1] high frequency vibration level in the IMU delta angle data (rad) + // [2] high frequency vibration level in the IMU delta velocity data (m/s) + + // Used to down sample barometer data + uint64_t _baro_timestamp_sum{0}; // summed timestamp to provide the timestamp of the averaged sample + float _baro_alt_sum{0.0f}; // summed pressure altitude readings (m) + uint8_t _baro_sample_count{0}; // number of barometric altitude measurements summed + + // Used by the multi-rotor specific drag force fusion + uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate + float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec) + + // Used to downsample magnetometer data + uint64_t _mag_timestamp_sum{0}; + Vector3f _mag_data_sum; + uint8_t _mag_sample_count{0}; + + // observation buffer final allocation failed + bool _gps_buffer_fail{false}; + bool _mag_buffer_fail{false}; + bool _baro_buffer_fail{false}; + bool _range_buffer_fail{false}; + bool _airspeed_buffer_fail{false}; + bool _flow_buffer_fail{false}; + bool _ev_buffer_fail{false}; + bool _drag_buffer_fail{false}; + bool _auxvel_buffer_fail{false}; + }; diff --git a/EKF/mag_control.cpp b/EKF/mag_control.cpp index 43027d318e..49d477c1fb 100644 --- a/EKF/mag_control.cpp +++ b/EKF/mag_control.cpp @@ -138,11 +138,6 @@ void Ekf::checkHaglYawResetReq() } } -float Ekf::getTerrainVPos() const -{ - return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; -} - void Ekf::runOnGroundYawReset() { if (_mag_yaw_reset_req && isYawResetAuthorized()) { @@ -154,11 +149,6 @@ void Ekf::runOnGroundYawReset() } } -bool Ekf::isYawResetAuthorized() const -{ - return !_is_yaw_fusion_inhibited; -} - bool Ekf::canResetMagHeading() const { return !isStrongMagneticDisturbance() && (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE); @@ -177,11 +167,6 @@ void Ekf::runInAirYawReset() } } -bool Ekf::canRealignYawUsingGps() const -{ - return _control_status.flags.fixed_wing; -} - void Ekf::runVelPosReset() { if (_velpos_reset_request) { @@ -237,16 +222,6 @@ void Ekf::checkMagBiasObservability() _time_yaw_started = _imu_sample_delayed.time_us; } -bool Ekf::isYawAngleObservable() const -{ - return _yaw_angle_observable; -} - -bool Ekf::isMagBiasObservable() const -{ - return _mag_bias_observable; -} - bool Ekf::canUse3DMagFusion() const { // Use of 3D fusion requires an in-air heading alignment but it should not @@ -308,11 +283,6 @@ void Ekf::checkMagFieldStrength() } } -bool Ekf::isStrongMagneticDisturbance() const -{ - return _control_status.flags.mag_field_disturbed; -} - bool Ekf::isMeasuredMatchingGpsMagStrength() const { constexpr float wmm_gate_size = 0.2f; // +/- Gauss diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 41ef99cd2b..77cc681810 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -83,16 +83,6 @@ bool Ekf::initHagl() return initialized; } -bool Ekf::shouldUseRangeFinderForHagl() const -{ - return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder); -} - -bool Ekf::shouldUseOpticalFlowForHagl() const -{ - return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow); -} - void Ekf::runTerrainEstimator() { // If we are on ground, store the local position and time to use as a reference @@ -286,11 +276,6 @@ void Ekf::fuseFlowForTerrain() } } -bool Ekf::isTerrainEstimateValid() const -{ - return _hagl_valid; -} - void Ekf::updateTerrainValidity() { // we have been fusing range finder measurements in the last 5 seconds @@ -306,12 +291,6 @@ void Ekf::updateTerrainValidity() _hagl_sensor_status.flags.range_finder = shouldUseRangeFinderForHagl() && recent_range_fusion && (_time_last_fake_hagl_fuse != _time_last_hagl_fuse); - _hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl() - && recent_flow_for_terrain_fusion; -} -// get the estimated vertical position of the terrain relative to the NED origin -float Ekf::getTerrainVertPos() const -{ - return _terrain_vpos; + _hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl() && recent_flow_for_terrain_fusion; }