From 10b54d08fca6589e5d3dbaa01c73c128060fc9b7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 31 Aug 2023 13:21:41 -0400 Subject: [PATCH] ekf2: add dedicated EKF2_CONFIG_TERRAIN in kconfig - new estimator_aid_src_terrain_range_finder for HAGL RNG --- msg/EstimatorAidSource1d.msg | 1 + src/modules/ekf2/CMakeLists.txt | 5 +- src/modules/ekf2/EKF/CMakeLists.txt | 5 +- src/modules/ekf2/EKF/common.h | 30 +-- src/modules/ekf2/EKF/ekf.cpp | 8 +- src/modules/ekf2/EKF/ekf.h | 152 +++++++++------ src/modules/ekf2/EKF/ekf_helper.cpp | 58 +++--- src/modules/ekf2/EKF/mag_control.cpp | 4 +- src/modules/ekf2/EKF/optical_flow_control.cpp | 2 +- src/modules/ekf2/EKF/range_height_control.cpp | 7 +- src/modules/ekf2/EKF/terrain_estimator.cpp | 174 ++++++++++-------- src/modules/ekf2/EKF2.cpp | 85 ++++++--- src/modules/ekf2/EKF2.hpp | 80 ++++---- src/modules/ekf2/Kconfig | 9 + 14 files changed, 367 insertions(+), 253 deletions(-) diff --git a/msg/EstimatorAidSource1d.msg b/msg/EstimatorAidSource1d.msg index 46faddafac..42be25881d 100644 --- a/msg/EstimatorAidSource1d.msg +++ b/msg/EstimatorAidSource1d.msg @@ -22,3 +22,4 @@ bool fused # true if the sample was successfully fused # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip # TOPICS estimator_aid_src_fake_hgt # TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw +# TOPICS estimator_aid_src_terrain_range_finder diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 64cd3c3bf6..cb4ee6dfb6 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -139,7 +139,6 @@ if(CONFIG_EKF2_RANGE_FINDER) EKF/range_finder_consistency_check.cpp EKF/range_height_control.cpp EKF/sensor_range_finder.cpp - EKF/terrain_estimator.cpp ) endif() @@ -147,6 +146,10 @@ if(CONFIG_EKF2_SIDESLIP) list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp) endif() +if(CONFIG_EKF2_TERRAIN) + list(APPEND EKF_SRCS EKF/terrain_estimator.cpp) +endif() + px4_add_module( MODULE modules__ekf2 MAIN ekf2 diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 99da3e6df4..475c8ef63c 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -104,7 +104,6 @@ if(CONFIG_EKF2_RANGE_FINDER) range_finder_consistency_check.cpp range_height_control.cpp sensor_range_finder.cpp - terrain_estimator.cpp ) endif() @@ -112,6 +111,10 @@ if(CONFIG_EKF2_SIDESLIP) list(APPEND EKF_SRCS sideslip_fusion.cpp) endif() +if(CONFIG_EKF2_TERRAIN) + list(APPEND EKF_SRCS terrain_estimator.cpp) +endif() + add_library(ecl_EKF ${EKF_SRCS} ) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index c7f75b35e2..bf12ed0ee4 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -105,12 +105,12 @@ enum MagFuseType : uint8_t { NONE = 5 ///< Do not use magnetometer under any circumstance.. }; -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) enum TerrainFusionMask : uint8_t { TerrainFuseRangeFinder = (1 << 0), TerrainFuseOpticalFlow = (1 << 1) }; -#endif // CONFIG_EKF2_RANGE_FINDER +#endif // CONFIG_EKF2_TERRAIN enum HeightSensor : uint8_t { BARO = 0, @@ -362,18 +362,27 @@ struct parameters { const float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec) #endif // CONFIG_EKF2_SIDESLIP +#if defined(CONFIG_EKF2_TERRAIN) + int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder | + TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator + + 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) +#endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER + #if defined(CONFIG_EKF2_RANGE_FINDER) // range finder fusion int32_t rng_ctrl{RngCtrl::CONDITIONAL}; - int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder | - TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator - float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) float range_noise{0.1f}; ///< observation noise for range finder measurements (m) float range_innov_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD) float rng_hgt_bias_nsd{0.13f}; ///< process noise for range height bias estimation (m/s/sqrt(Hz)) - float rng_gnd_clearance{0.1f}; ///< minimum valid value for range when on ground (m) float rng_sens_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis. float range_noise_scaler{0.0f}; ///< scaling from range measurement to noise (m/m) const float vehicle_variance_scaler{0.0f}; ///< gain applied to vehicle height variance used in calculation of height above ground observation variance @@ -385,11 +394,6 @@ struct parameters { float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m) - - 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_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) @@ -625,7 +629,7 @@ union ekf_solution_status_u { uint16_t value; }; -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) union terrain_fusion_status_u { struct { bool range_finder : 1; ///< 0 - true if we are fusing range finder data @@ -633,7 +637,7 @@ union terrain_fusion_status_u { } flags; uint8_t value; }; -#endif // CONFIG_EKF2_RANGE_FINDER +#endif // CONFIG_EKF2_TERRAIN // define structure used to communicate information events union information_event_status_u { diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 0b6d5485ce..0885ad9168 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -189,10 +189,10 @@ bool Ekf::update() // control fusion of observation data controlFusionModes(imu_sample_delayed); -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) // run a separate filter for terrain estimation runTerrainEstimator(imu_sample_delayed); -#endif // CONFIG_EKF2_RANGE_FINDER +#endif // CONFIG_EKF2_TERRAIN _output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, _state.gyro_bias, _state.accel_bias); @@ -229,10 +229,10 @@ bool Ekf::initialiseFilter() // initialise the state covariance matrix now we have starting values for all the states initialiseCovariance(); -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) // Initialise the terrain estimator initHagl(); -#endif // CONFIG_EKF2_RANGE_FINDER +#endif // CONFIG_EKF2_TERRAIN // reset the output predictor state history to match the EKF initial values _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 666abf9256..bac99afb7c 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -95,23 +95,7 @@ public: void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; } void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; } -#if defined(CONFIG_EKF2_RANGE_FINDER) - // range height - const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); } - const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; } - - void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; } - void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; } - void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.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; } - - void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); } - void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); } - void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); } - +#if defined(CONFIG_EKF2_TERRAIN) // terrain estimate bool isTerrainEstimateValid() const; @@ -125,13 +109,64 @@ public: // get the terrain variance float get_terrain_var() const { return _terrain_var; } + +# if defined(CONFIG_EKF2_RANGE_FINDER) + const auto &aid_src_terrain_range_finder() const { return _aid_src_terrain_range_finder; } + + void getHaglInnov(float &hagl_innov) const { hagl_innov = _aid_src_terrain_range_finder.innovation; } + void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance; } + void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _aid_src_terrain_range_finder.test_ratio; } +# endif // CONFIG_EKF2_RANGE_FINDER + +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; } + + void getTerrainFlowInnov(float flow_innov[2]) const + { + flow_innov[0] = _aid_src_terrain_optical_flow.innovation[0]; + flow_innov[1] = _aid_src_terrain_optical_flow.innovation[1]; + } + + void getTerrainFlowInnovVar(float flow_innov_var[2]) const + { + flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0]; + flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1]; + } + + void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); } +# endif // CONFIG_EKF2_OPTICAL_FLOW + +#endif // CONFIG_EKF2_TERRAIN + +#if defined(CONFIG_EKF2_RANGE_FINDER) + // range height + const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); } + const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; } + + void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; } + void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; } + void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; } + + void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); } + void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); } + void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); } #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; } - void getFlowInnov(float flow_innov[2]) const; - void getFlowInnovVar(float flow_innov_var[2]) const; + void getFlowInnov(float flow_innov[2]) const + { + flow_innov[0] = _aid_src_optical_flow.innovation[0]; + flow_innov[1] = _aid_src_optical_flow.innovation[1]; + } + + void getFlowInnovVar(float flow_innov_var[2]) const + { + flow_innov_var[0] = _aid_src_optical_flow.innovation_variance[0]; + flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1]; + } + void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_optical_flow.test_ratio[0], _aid_src_optical_flow.test_ratio[1]); } const Vector2f &getFlowVelBody() const { return _flow_vel_body; } @@ -142,12 +177,6 @@ public: const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); } const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; } - - void getTerrainFlowInnov(float flow_innov[2]) const; - void getTerrainFlowInnovVar(float flow_innov_var[2]) const; - void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); } - - const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; } #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_AUXVEL) @@ -598,30 +627,33 @@ private: Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2) #endif // CONFIG_EKF2_DRAG_FUSION -#if defined(CONFIG_EKF2_RANGE_FINDER) - estimator_aid_source1d_s _aid_src_rng_hgt{}; - - HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref}; - - float _hagl_innov{0.0f}; ///< innovation of the last height above terrain measurement (m) - float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2) - float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio - - uint64_t _time_last_healthy_rng_data{0}; - +#if defined(CONFIG_EKF2_TERRAIN) // Terrain height state estimation float _terrain_vpos{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m) float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2) uint8_t _terrain_vpos_reset_counter{0}; ///< number of times _terrain_vpos has been reset - uint64_t _time_last_hagl_fuse{0}; ///< last system time that a range sample was fused by the terrain estimator - terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground + terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) + +# if defined(CONFIG_EKF2_RANGE_FINDER) + estimator_aid_source1d_s _aid_src_terrain_range_finder{}; + uint64_t _time_last_healthy_rng_data{0}; +# endif // CONFIG_EKF2_RANGE_FINDER + +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + estimator_aid_source2d_s _aid_src_terrain_optical_flow{}; +# endif // CONFIG_EKF2_OPTICAL_FLOW + +#endif // CONFIG_EKF2_TERRAIN + +#if defined(CONFIG_EKF2_RANGE_FINDER) + estimator_aid_source1d_s _aid_src_rng_hgt{}; + HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref}; #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_terrain_optical_flow{}; // optical flow processing Vector3f _flow_gyro_bias{}; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) @@ -635,7 +667,6 @@ private: Vector2f _flow_compensated_XY_rad{}; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon - uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec) #endif // CONFIG_EKF2_OPTICAL_FLOW estimator_aid_source1d_s _aid_src_baro_hgt{}; @@ -867,12 +898,7 @@ private: void fuseVelocity(estimator_aid_source2d_s &vel_aid_src); void fuseVelocity(estimator_aid_source3d_s &vel_aid_src); -#if defined(CONFIG_EKF2_RANGE_FINDER) - // range height - void controlRangeHeightFusion(); - bool isConditionalRangeAidSuitable(); - void stopRngHgtFusion(); - +#if defined(CONFIG_EKF2_TERRAIN) // terrain vertical position estimator void initHagl(); void runTerrainEstimator(const imuSample &imu_delayed); @@ -880,16 +906,33 @@ private: float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; } + void controlHaglFakeFusion(); + +# if defined(CONFIG_EKF2_RANGE_FINDER) // update the terrain vertical position estimate using a height above ground measurement from the range finder void controlHaglRngFusion(); - void fuseHaglRng(); - void startHaglRngFusion(); - void resetHaglRngIfNeeded(); + void updateHaglRng(estimator_aid_source1d_s &aid_src) const; + void fuseHaglRng(estimator_aid_source1d_s &aid_src); void resetHaglRng(); void stopHaglRngFusion(); - float getRngVar(); + float getRngVar() const; +# endif // CONFIG_EKF2_RANGE_FINDER - void controlHaglFakeFusion(); +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + // update the terrain vertical position estimate using an optical flow measurement + void controlHaglFlowFusion(); + void resetHaglFlow(); + void stopHaglFlowFusion(); + void fuseFlowForTerrain(estimator_aid_source2d_s &flow); +# endif // CONFIG_EKF2_OPTICAL_FLOW + +#endif // CONFIG_EKF2_TERRAIN + +#if defined(CONFIG_EKF2_RANGE_FINDER) + // range height + void controlRangeHeightFusion(); + bool isConditionalRangeAidSuitable(); + void stopRngHgtFusion(); #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -912,13 +955,6 @@ private: void fuseOptFlow(); float predictFlowRange(); Vector2f predictFlowVelBody(); - - // update the terrain vertical position estimate using an optical flow measurement - void controlHaglFlowFusion(); - void startHaglFlowFusion(); - void resetHaglFlow(); - void stopHaglFlowFusion(); - void fuseFlowForTerrain(estimator_aid_source2d_s &flow); #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_MAGNETOMETER) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index d8465622d2..1b4cb37e44 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -363,32 +363,6 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const } #endif // CONFIG_EKF2_AUXVEL -#if defined(CONFIG_EKF2_OPTICAL_FLOW) -void Ekf::getFlowInnov(float flow_innov[2]) const -{ - flow_innov[0] = _aid_src_optical_flow.innovation[0]; - flow_innov[1] = _aid_src_optical_flow.innovation[1]; -} - -void Ekf::getFlowInnovVar(float flow_innov_var[2]) const -{ - flow_innov_var[0] = _aid_src_optical_flow.innovation_variance[0]; - flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1]; -} - -void Ekf::getTerrainFlowInnov(float flow_innov[2]) const -{ - flow_innov[0] = _aid_src_terrain_optical_flow.innovation[0]; - flow_innov[1] = _aid_src_terrain_optical_flow.innovation[1]; -} - -void Ekf::getTerrainFlowInnovVar(float flow_innov_var[2]) const -{ - flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0]; - flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1]; -} -#endif // CONFIG_EKF2_OPTICAL_FLOW - // get the state vector at the delayed time horizon matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const { @@ -605,9 +579,8 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl *hagl_min = rangefinder_hagl_min; *hagl_max = rangefinder_hagl_max; } -#endif // CONFIG_EKF2_RANGE_FINDER -#if defined(CONFIG_EKF2_OPTICAL_FLOW) +# if defined(CONFIG_EKF2_OPTICAL_FLOW) // Keep within flow AND range sensor limits when exclusively using optical flow const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow); @@ -625,7 +598,9 @@ 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_OPTICAL_FLOW + +#endif // CONFIG_EKF2_RANGE_FINDER } void Ekf::resetImuBias() @@ -767,11 +742,23 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f tas = sqrtf(_aid_src_airspeed.test_ratio); #endif // CONFIG_EKF2_AIRSPEED -#if defined(CONFIG_EKF2_RANGE_FINDER) - // return the terrain height innovation test ratio - hagl = sqrtf(_hagl_test_ratio); + hagl = NAN; +#if defined(CONFIG_EKF2_TERRAIN) +# if defined(CONFIG_EKF2_RANGE_FINDER) + if (_hagl_sensor_status.flags.range_finder) { + // return the terrain height innovation test ratio + hagl = sqrtf(_aid_src_terrain_range_finder.test_ratio); + } #endif // CONFIG_EKF2_RANGE_FINDER +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + if (_hagl_sensor_status.flags.flow) { + // return the terrain height innovation test ratio + hagl = sqrtf(math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1])); + } +# endif // CONFIG_EKF2_OPTICAL_FLOW +#endif // CONFIG_EKF2_TERRAIN + #if defined(CONFIG_EKF2_SIDESLIP) // return the synthetic sideslip innovation test ratio beta = sqrtf(_aid_src_sideslip.test_ratio); @@ -789,7 +776,9 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const 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(); +#endif // CONFIG_EKF2_TERRAIN soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz; soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel; soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs; @@ -927,12 +916,15 @@ float Ekf::getYawVar() const 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 = _terrain_vpos - _state.pos(2); _control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt); - } else if (_control_status.flags.gnd_effect) { + } 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; diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 7822a41980..69adf7e9ba 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -123,17 +123,17 @@ void Ekf::controlMagFusion() bool Ekf::checkHaglYawResetReq() const { +#if defined(CONFIG_EKF2_TERRAIN) // We need to reset the yaw angle after climbing away from the ground to enable // recovery from ground level magnetic interference. if (_control_status.flags.in_air && _control_status.flags.yaw_align && !_control_status.flags.mag_aligned_in_flight) { // Check if height has increased sufficiently to be away from ground magnetic anomalies // and request a yaw reset if not already requested. -#if defined(CONFIG_EKF2_RANGE_FINDER) static constexpr float mag_anomalies_max_hagl = 1.5f; const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl; return above_mag_anomalies; -#endif // CONFIG_EKF2_RANGE_FINDER } +#endif // CONFIG_EKF2_TERRAIN return false; } diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index 8b99d2e5c7..a8207db178 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -209,7 +209,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) if (_time_delayed_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) { // Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently // but use a relaxed time criteria to enable it to coast through bad range finder data - if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) { + if (isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)10e6)) { fuseOptFlow(); _last_known_pos.xy() = _state.pos.xy(); } diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index 88a9cb7197..aafcfce3f6 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -204,6 +204,7 @@ void Ekf::controlRangeHeightFusion() bool Ekf::isConditionalRangeAidSuitable() { +#if defined(CONFIG_EKF2_TERRAIN) if (_control_status.flags.in_air && _range_sensor.isHealthy() && isTerrainEstimateValid()) { @@ -212,7 +213,10 @@ bool Ekf::isConditionalRangeAidSuitable() float range_hagl_max = _params.max_hagl_for_range_aid; float max_vel_xy = _params.max_vel_for_range_aid; - const float hagl_test_ratio = (_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)); + const float hagl_innov = _aid_src_terrain_range_finder.innovation; + const float hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance; + + const float hagl_test_ratio = (hagl_innov * hagl_innov / (sq(_params.range_aid_innov_gate) * hagl_innov_var)); bool is_hagl_stable = (hagl_test_ratio < 1.f); @@ -234,6 +238,7 @@ bool Ekf::isConditionalRangeAidSuitable() return is_in_range && is_hagl_stable && is_below_max_speed; } +#endif // CONFIG_EKF2_TERRAIN return false; } diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 657b989f48..a33f1cae6e 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -49,7 +49,9 @@ void Ekf::initHagl() stopHaglFlowFusion(); #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_RANGE_FINDER) stopHaglRngFusion(); +#endif // CONFIG_EKF2_RANGE_FINDER // assume a ground clearance _terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance; @@ -68,10 +70,14 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed) predictHagl(imu_delayed); +#if defined(CONFIG_EKF2_RANGE_FINDER) controlHaglRngFusion(); +#endif // CONFIG_EKF2_RANGE_FINDER + #if defined(CONFIG_EKF2_OPTICAL_FLOW) controlHaglFlowFusion(); #endif // CONFIG_EKF2_OPTICAL_FLOW + controlHaglFakeFusion(); // constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2) @@ -95,6 +101,7 @@ void Ekf::predictHagl(const imuSample &imu_delayed) _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f); } +#if defined(CONFIG_EKF2_RANGE_FINDER) void Ekf::controlHaglRngFusion() { if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder) @@ -104,20 +111,29 @@ void Ekf::controlHaglRngFusion() return; } + if (_range_sensor.isDataReady()) { + updateHaglRng(_aid_src_terrain_range_finder); + } + if (_range_sensor.isDataHealthy()) { - const bool continuing_conditions_passing = _control_status.flags.in_air && _rng_consistency_check.isKinematicallyConsistent(); - //const bool continuing_conditions_passing = _control_status.flags.in_air && !_control_status.flags.rng_hgt; // TODO: should not be fused when using range height - const bool starting_conditions_passing = continuing_conditions_passing && _range_sensor.isRegularlySendingData() && (_rng_consistency_check.getTestRatio() < 1.f); + + const bool continuing_conditions_passing = _control_status.flags.in_air + //&& !_control_status.flags.rng_hgt // TODO: should not be fused when using range height + && _rng_consistency_check.isKinematicallyConsistent(); + + const bool starting_conditions_passing = continuing_conditions_passing + && _range_sensor.isRegularlySendingData() + && (_rng_consistency_check.getTestRatio() < 1.f); _time_last_healthy_rng_data = _time_delayed_us; if (_hagl_sensor_status.flags.range_finder) { if (continuing_conditions_passing) { - fuseHaglRng(); + fuseHaglRng(_aid_src_terrain_range_finder); // We have been rejecting range data for too long const uint64_t timeout = static_cast(_params.terrain_timeout * 1e6f); - const bool is_fusion_failing = isTimedOut(_time_last_hagl_fuse, timeout); + const bool is_fusion_failing = isTimedOut(_aid_src_terrain_range_finder.time_last_fuse, timeout); if (is_fusion_failing) { if (_range_sensor.getDistBottom() > 2.f * _params.rng_gnd_clearance) { @@ -143,7 +159,16 @@ void Ekf::controlHaglRngFusion() } else { if (starting_conditions_passing) { - startHaglRngFusion(); + _hagl_sensor_status.flags.range_finder = true; + + // Reset the state to the measurement only if the test ratio is large, + // otherwise let it converge through the fusion + if (_hagl_sensor_status.flags.flow && (_aid_src_terrain_range_finder.test_ratio < 0.2f)) { + fuseHaglRng(_aid_src_terrain_range_finder); + + } else { + resetHaglRng(); + } } } @@ -151,42 +176,11 @@ void Ekf::controlHaglRngFusion() // No data anymore. Stop until it comes back. stopHaglRngFusion(); } + + _aid_src_terrain_range_finder.fusion_enabled = _hagl_sensor_status.flags.range_finder; } -void Ekf::startHaglRngFusion() -{ - _hagl_sensor_status.flags.range_finder = true; - resetHaglRngIfNeeded(); -} - -void Ekf::resetHaglRngIfNeeded() -{ - if (_hagl_sensor_status.flags.flow) { - const float meas_hagl = _range_sensor.getDistBottom(); - const float pred_hagl = _terrain_vpos - _state.pos(2); - const float hagl_innov = pred_hagl - meas_hagl; - const float obs_variance = getRngVar(); - - const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); - - const float gate_size = fmaxf(_params.range_innov_gate, 1.0f); - const float hagl_test_ratio = sq(hagl_innov) / (sq(gate_size) * hagl_innov_var); - - // Reset the state to the measurement only if the test ratio is large, - // otherwise let it converge through the fusion - if (hagl_test_ratio > 0.2f) { - resetHaglRng(); - - } else { - fuseHaglRng(); - } - - } else { - resetHaglRng(); - } -} - -float Ekf::getRngVar() +float Ekf::getRngVar() const { return fmaxf(P(9, 9) * _params.vehicle_variance_scaler, 0.0f) + sq(_params.range_noise) @@ -198,26 +192,25 @@ void Ekf::resetHaglRng() _terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom(); _terrain_var = getRngVar(); _terrain_vpos_reset_counter++; - _time_last_hagl_fuse = _time_delayed_us; - _time_last_healthy_rng_data = 0; + + _aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us; } void Ekf::stopHaglRngFusion() { if (_hagl_sensor_status.flags.range_finder) { + ECL_INFO("stopping HAGL range fusion"); + + // reset flags + resetEstimatorAidStatus(_aid_src_terrain_range_finder); - _hagl_innov = 0.f; - _hagl_innov_var = 0.f; - _hagl_test_ratio = 0.f; _innov_check_fail_status.flags.reject_hagl = false; _hagl_sensor_status.flags.range_finder = false; } - - _time_last_healthy_rng_data = 0; } -void Ekf::fuseHaglRng() +void Ekf::updateHaglRng(estimator_aid_source1d_s &aid_src) const { // get a height above ground measurement from the range finder assuming a flat earth const float meas_hagl = _range_sensor.getDistBottom(); @@ -226,33 +219,56 @@ void Ekf::fuseHaglRng() const float pred_hagl = _terrain_vpos - _state.pos(2); // calculate the innovation - _hagl_innov = pred_hagl - meas_hagl; + const float hagl_innov = pred_hagl - meas_hagl; // calculate the observation variance adding the variance of the vehicles own height uncertainty const float obs_variance = getRngVar(); // calculate the innovation variance - limiting it to prevent a badly conditioned fusion - _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); + const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); // perform an innovation consistency check and only fuse data if it passes - const float gate_size = fmaxf(_params.range_innov_gate, 1.0f); - _hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var); + const float innov_gate = fmaxf(_params.range_innov_gate, 1.0f); - if (_hagl_test_ratio <= 1.0f) { + + aid_src.timestamp_sample = _time_delayed_us; // TODO + + aid_src.observation = pred_hagl; + aid_src.observation_variance = obs_variance; + + aid_src.innovation = hagl_innov; + aid_src.innovation_variance = hagl_innov_var; + + setEstimatorAidStatusTestRatio(aid_src, innov_gate); + + aid_src.fusion_enabled = false; + aid_src.fused = false; +} + +void Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src) +{ + if (!aid_src.innovation_rejected) { // calculate the Kalman gain - const float gain = _terrain_var / _hagl_innov_var; + const float gain = _terrain_var / aid_src.innovation_variance; + // correct the state - _terrain_vpos -= gain * _hagl_innov; + _terrain_vpos -= gain * aid_src.innovation; + // correct the variance _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); + // record last successful fusion event - _time_last_hagl_fuse = _time_delayed_us; _innov_check_fail_status.flags.reject_hagl = false; + aid_src.time_last_fuse = _time_delayed_us; + aid_src.fused = true; + } else { _innov_check_fail_status.flags.reject_hagl = true; + aid_src.fused = false; } } +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) void Ekf::controlHaglFlowFusion() @@ -266,10 +282,10 @@ void Ekf::controlHaglFlowFusion() updateOptFlow(_aid_src_terrain_optical_flow); const bool continuing_conditions_passing = _control_status.flags.in_air - && !_control_status.flags.opt_flow - && _control_status.flags.gps - && isTimedOut(_time_last_hagl_fuse, 5e6f); // TODO: check for range_finder hagl aiding instead? - /* && !_hagl_sensor_status.flags.range_finder; */ + && !_control_status.flags.opt_flow + && _control_status.flags.gps + && !_hagl_sensor_status.flags.range_finder; + const bool starting_conditions_passing = continuing_conditions_passing; if (_hagl_sensor_status.flags.flow) { @@ -277,10 +293,9 @@ void Ekf::controlHaglFlowFusion() // TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon fuseFlowForTerrain(_aid_src_terrain_optical_flow); - _flow_data_ready = false; // TODO: do something when failing continuously the innovation check - /* const bool is_fusion_failing = isTimedOut(_time_last_flow_terrain_fuse, _params.reset_timeout_max); */ + /* const bool is_fusion_failing = isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max); */ /* if (is_fusion_failing) { */ /* resetHaglFlow(); */ @@ -292,28 +307,27 @@ void Ekf::controlHaglFlowFusion() } else { if (starting_conditions_passing) { - startHaglFlowFusion(); + _hagl_sensor_status.flags.flow = true; + // TODO: do a reset instead of trying to fuse the data? + fuseFlowForTerrain(_aid_src_terrain_optical_flow); } } - } else if (_hagl_sensor_status.flags.flow - && (_time_delayed_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) { + _flow_data_ready = false; + + } else if (_hagl_sensor_status.flags.flow && (_time_delayed_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) { // No data anymore. Stop until it comes back. stopHaglFlowFusion(); } -} -void Ekf::startHaglFlowFusion() -{ - _hagl_sensor_status.flags.flow = true; - // TODO: do a reset instead of trying to fuse the data? - fuseFlowForTerrain(_aid_src_terrain_optical_flow); - _flow_data_ready = false; + _aid_src_terrain_optical_flow.fusion_enabled = _hagl_sensor_status.flags.flow; } void Ekf::stopHaglFlowFusion() { if (_hagl_sensor_status.flags.flow) { + ECL_INFO("stopping HAGL flow fusion"); + _hagl_sensor_status.flags.flow = false; resetEstimatorAidStatus(_aid_src_terrain_optical_flow); } @@ -325,11 +339,14 @@ void Ekf::resetHaglFlow() _terrain_vpos = fmaxf(0.0f, _state.pos(2)); _terrain_var = 100.0f; _terrain_vpos_reset_counter++; + + _aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us; } void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow) { flow.fusion_enabled = true; + flow.fused = true; const float R_LOS = flow.observation_variance[0]; @@ -397,9 +414,8 @@ void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow) _fault_status.flags.bad_optflow_X = false; _fault_status.flags.bad_optflow_Y = false; - _time_last_flow_terrain_fuse = _time_delayed_us; - //_aid_src_optical_flow.time_last_fuse = _time_delayed_us; // TODO: separate aid source status for OF terrain? - _aid_src_optical_flow.fused = true; + flow.time_last_fuse = _time_delayed_us; + flow.fused = true; } #endif // CONFIG_EKF2_OPTICAL_FLOW @@ -416,18 +432,22 @@ void Ekf::controlHaglFakeFusion() bool Ekf::isTerrainEstimateValid() const { #if defined(CONFIG_EKF2_RANGE_FINDER) + // we have been fusing range finder measurements in the last 5 seconds - if (_hagl_sensor_status.flags.range_finder && isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) { + if (_hagl_sensor_status.flags.range_finder && isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)5e6)) { return true; } + #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) + // we have been fusing optical flow measurements for terrain estimation within the last 5 seconds // this can only be the case if the main filter does not fuse optical flow - if (_hagl_sensor_status.flags.flow && isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6)) { + if (_hagl_sensor_status.flags.flow && isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)5e6)) { return true; } + #endif // CONFIG_EKF2_OPTICAL_FLOW return false; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 9eb94dc9ec..81fe6974b1 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -120,13 +120,20 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_baro_ctrl(_params->baro_ctrl), _param_ekf2_gps_ctrl(_params->gnss_ctrl), _param_ekf2_noaid_tout(_params->valid_timeout_max), +#if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER) + _param_ekf2_min_rng(_params->rng_gnd_clearance), +#endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_TERRAIN) + _param_ekf2_terr_mask(_params->terrain_fusion_mode), + _param_ekf2_terr_noise(_params->terrain_p_noise), + _param_ekf2_terr_grad(_params->terrain_gradient), +#endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) _param_ekf2_rng_ctrl(_params->rng_ctrl), _param_ekf2_rng_delay(_params->range_delay_ms), _param_ekf2_rng_noise(_params->range_noise), _param_ekf2_rng_sfe(_params->range_noise_scaler), _param_ekf2_rng_gate(_params->range_innov_gate), - _param_ekf2_min_rng(_params->rng_gnd_clearance), _param_ekf2_rng_pitch(_params->rng_sens_pitch), _param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid), _param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid), @@ -136,9 +143,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_pos_x(_params->rng_pos_body(0)), _param_ekf2_rng_pos_y(_params->rng_pos_body(1)), _param_ekf2_rng_pos_z(_params->rng_pos_body(2)), - _param_ekf2_terr_mask(_params->terrain_fusion_mode), - _param_ekf2_terr_noise(_params->terrain_p_noise), - _param_ekf2_terr_grad(_params->terrain_gradient), #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) _param_ekf2_ev_delay(_params->ev_delay_ms), @@ -1022,9 +1026,22 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_OPTICAL_FLOW) // optical flow PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub); +#endif // CONFIG_EKF2_OPTICAL_FLOW + +#if defined(CONFIG_EKF2_TERRAIN) +# if defined(CONFIG_EKF2_RANGE_FINDER) + // range finder + PublishAidSourceStatus(_ekf.aid_src_terrain_range_finder(), _status_terrain_range_finder_pub_last, + _estimator_aid_src_terrain_range_finder_pub); +#endif // CONFIG_EKF2_RANGE_FINDER + +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + // optical flow PublishAidSourceStatus(_ekf.aid_src_terrain_optical_flow(), _status_terrain_optical_flow_pub_last, _estimator_aid_src_terrain_optical_flow_pub); -#endif // CONFIG_EKF2_OPTICAL_FLOW +# endif // CONFIG_EKF2_OPTICAL_FLOW + +#endif // CONFIG_EKF2_TERRAIN } void EKF2::PublishAttitude(const hrt_abstime ×tamp) @@ -1247,7 +1264,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) global_pos.terrain_alt = NAN; global_pos.terrain_alt_valid = false; -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) if (_ekf.isTerrainEstimateValid()) { // Terrain altitude in m, WGS84 @@ -1255,7 +1272,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) global_pos.terrain_alt_valid = true; } -#endif // CONFIG_EKF2_RANGE_FINDER +#endif // CONFIG_EKF2_TERRAIN global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning || _ekf.control_status_flags().wind_dead_reckoning; @@ -1318,7 +1335,6 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) #endif // CONFIG_EKF2_AUXVEL #if defined(CONFIG_EKF2_OPTICAL_FLOW) _ekf.getFlowInnov(innovations.flow); - _ekf.getTerrainFlowInnov(innovations.terr_flow); #endif // CONFIG_EKF2_OPTICAL_FLOW _ekf.getHeadingInnov(innovations.heading); #if defined(CONFIG_EKF2_MAGNETOMETER) @@ -1333,11 +1349,18 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_SIDESLIP) _ekf.getBetaInnov(innovations.beta); #endif // CONFIG_EKF2_SIDESLIP -#if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getHaglInnov(innovations.hagl); - _ekf.getHaglRateInnov(innovations.hagl_rate); -#endif // CONFIG_EKF2_RANGE_FINDER + _ekf.getGravityInnov(innovations.gravity); + +#if defined(CONFIG_EKF2_TERRAIN) +# if defined(CONFIG_EKF2_RANGE_FINDER) + _ekf.getHaglInnov(innovations.hagl); +# endif //CONFIG_EKF2_RANGE_FINDER +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + _ekf.getTerrainFlowInnov(innovations.terr_flow); +# endif // CONFIG_EKF2_OPTICAL_FLOW +#endif // CONFIG_EKF2_TERRAIN + // Not yet supported innovations.aux_vvel = NAN; @@ -1355,11 +1378,13 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps); #if defined(CONFIG_EKF2_OPTICAL_FLOW) _preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow); +#endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_OPTICAL_FLOW) // set dist bottom to scale flow innovation const float dist_bottom = _ekf.getTerrainVertPos() - _ekf.getPosition()(2); _preflt_checker.setDistBottom(dist_bottom); -#endif // CONFIG_EKF2_OPTICAL_FLOW +#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_EXTERNAL_VISION) _preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos); @@ -1392,13 +1417,13 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) _ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos); #if defined(CONFIG_EKF2_RANGE_FINDER) _ekf.getRngHgtInnovRatio(test_ratios.rng_vpos); + _ekf.getHaglRateInnovRatio(test_ratios.hagl_rate); #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_AUXVEL) _ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]); #endif // CONFIG_EKF2_AUXVEL #if defined(CONFIG_EKF2_OPTICAL_FLOW) _ekf.getFlowInnovRatio(test_ratios.flow[0]); - _ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]); #endif // CONFIG_EKF2_OPTICAL_FLOW _ekf.getHeadingInnovRatio(test_ratios.heading); #if defined(CONFIG_EKF2_MAGNETOMETER) @@ -1413,11 +1438,18 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_SIDESLIP) _ekf.getBetaInnovRatio(test_ratios.beta); #endif // CONFIG_EKF2_SIDESLIP -#if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getHaglInnovRatio(test_ratios.hagl); - _ekf.getHaglRateInnovRatio(test_ratios.hagl_rate); -#endif // CONFIG_EKF2_RANGE_FINDER + _ekf.getGravityInnovRatio(test_ratios.gravity[0]); + +#if defined(CONFIG_EKF2_TERRAIN) +# if defined(CONFIG_EKF2_RANGE_FINDER) + _ekf.getHaglInnovRatio(test_ratios.hagl); +# endif +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + _ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]); +# endif // CONFIG_EKF2_OPTICAL_FLOW +#endif // CONFIG_EKF2_TERRAIN + // Not yet supported test_ratios.aux_vvel = NAN; @@ -1437,13 +1469,13 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) _ekf.getBaroHgtInnovVar(variances.baro_vpos); #if defined(CONFIG_EKF2_RANGE_FINDER) _ekf.getRngHgtInnovVar(variances.rng_vpos); + _ekf.getHaglRateInnovVar(variances.hagl_rate); #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_AUXVEL) _ekf.getAuxVelInnovVar(variances.aux_hvel); #endif // CONFIG_EKF2_AUXVEL #if defined(CONFIG_EKF2_OPTICAL_FLOW) _ekf.getFlowInnovVar(variances.flow); - _ekf.getTerrainFlowInnovVar(variances.terr_flow); #endif // CONFIG_EKF2_OPTICAL_FLOW _ekf.getHeadingInnovVar(variances.heading); #if defined(CONFIG_EKF2_MAGNETOMETER) @@ -1458,11 +1490,18 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_SIDESLIP) _ekf.getBetaInnovVar(variances.beta); #endif // CONFIG_EKF2_SIDESLIP -#if defined(CONFIG_EKF2_RANGE_FINDER) + +#if defined(CONFIG_EKF2_TERRAIN) +# if defined(CONFIG_EKF2_RANGE_FINDER) _ekf.getHaglInnovVar(variances.hagl); - _ekf.getHaglRateInnovVar(variances.hagl_rate); -#endif // CONFIG_EKF2_RANGE_FINDER +# endif // CONFIG_EKF2_RANGE_FINDER +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + _ekf.getTerrainFlowInnovVar(variances.terr_flow); +# endif // CONFIG_EKF2_OPTICAL_FLOW +#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER + _ekf.getGravityInnovVar(variances.gravity); + // Not yet supported variances.aux_vvel = NAN; @@ -1530,10 +1569,12 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.delta_heading = Eulerf(delta_q_reset).psi(); lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete(); +#if defined(CONFIG_EKF2_TERRAIN) // Distance to bottom surface (ground) in meters, must be positive lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, 0.f); lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid(); lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield(); +#endif // CONFIG_EKF2_TERRAIN _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv); _ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index becc594471..45ed80262d 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -303,7 +303,6 @@ private: hrt_abstime _last_gps_status_published{0}; hrt_abstime _status_baro_hgt_pub_last{0}; - hrt_abstime _status_rng_hgt_pub_last{0}; hrt_abstime _status_fake_hgt_pub_last{0}; hrt_abstime _status_fake_pos_pub_last{0}; @@ -353,7 +352,6 @@ private: hrt_abstime _status_gnss_yaw_pub_last {0}; #endif // CONFIG_EKF2_GNSS_YAW - hrt_abstime _status_gravity_pub_last{0}; #if defined(CONFIG_EKF2_AUXVEL) @@ -365,20 +363,31 @@ private: perf_counter_t _msg_missed_landing_target_pose_perf{nullptr}; #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_TERRAIN) + +# if defined(CONFIG_EKF2_RANGE_FINDER) + uORB::PublicationMulti _estimator_aid_src_terrain_range_finder_pub {ORB_ID(estimator_aid_src_terrain_range_finder)}; + hrt_abstime _status_terrain_range_finder_pub_last{0}; +# endif // CONFIG_EKF2_RANGE_FINDER + +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + uORB::PublicationMulti _estimator_aid_src_terrain_optical_flow_pub {ORB_ID(estimator_aid_src_terrain_optical_flow)}; + hrt_abstime _status_terrain_optical_flow_pub_last{0}; +# endif // CONFIG_EKF2_OPTICAL_FLOW +#endif // CONFIG_EKF2_TERRAIN + #if defined(CONFIG_EKF2_OPTICAL_FLOW) uORB::Subscription _vehicle_optical_flow_sub {ORB_ID(vehicle_optical_flow)}; uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; - uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; - uORB::PublicationMulti _estimator_aid_src_terrain_optical_flow_pub{ORB_ID(estimator_aid_src_terrain_optical_flow)}; + uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; hrt_abstime _status_optical_flow_pub_last{0}; - hrt_abstime _status_terrain_optical_flow_pub_last{0}; + perf_counter_t _msg_missed_optical_flow_perf{nullptr}; #endif // CONFIG_EKF2_OPTICAL_FLOW float _last_baro_bias_published{}; float _last_gnss_hgt_bias_published{}; - float _last_rng_hgt_bias_published{}; #if defined(CONFIG_EKF2_AIRSPEED) uORB::Subscription _airspeed_sub {ORB_ID(airspeed)}; @@ -414,6 +423,9 @@ private: uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; #if defined(CONFIG_EKF2_RANGE_FINDER) + hrt_abstime _status_rng_hgt_pub_last {0}; + float _last_rng_hgt_bias_published{}; + uORB::PublicationMulti _estimator_rng_hgt_bias_pub {ORB_ID(estimator_rng_hgt_bias)}; uORB::PublicationMulti _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)}; @@ -590,42 +602,30 @@ private: (ParamExtInt) _param_ekf2_noaid_tout, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec) +#if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER) + (ParamExtFloat) _param_ekf2_min_rng, +#endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_TERRAIN) + (ParamExtInt) _param_ekf2_terr_mask, + (ParamExtFloat) _param_ekf2_terr_noise, + (ParamExtFloat) _param_ekf2_terr_grad, +#endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) // range finder fusion - (ParamExtInt) _param_ekf2_rng_ctrl, ///< range finder control selection - (ParamExtFloat) - _param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _param_ekf2_rng_noise, ///< observation noise for range finder measurements (m) - (ParamExtFloat) - _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m) - (ParamExtFloat) - _param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m) - (ParamExtFloat) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad) - (ParamExtFloat) - _param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s) - (ParamExtFloat) - _param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m) - (ParamExtFloat) - _param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD) - (ParamExtFloat) - _param_ekf2_rng_qlty_t, ///< Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) - (ParamExtFloat) - _param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m) - (ParamExtFloat) - _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m) - (ParamExtFloat) - _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m) - - (ParamExtInt) - _param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation - (ParamExtFloat) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec) - (ParamExtFloat) - _param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m) + (ParamExtInt) _param_ekf2_rng_ctrl, + (ParamExtFloat) _param_ekf2_rng_delay, + (ParamExtFloat) _param_ekf2_rng_noise, + (ParamExtFloat) _param_ekf2_rng_sfe, + (ParamExtFloat) _param_ekf2_rng_gate, + (ParamExtFloat) _param_ekf2_rng_pitch, + (ParamExtFloat) _param_ekf2_rng_a_vmax, + (ParamExtFloat) _param_ekf2_rng_a_hmax, + (ParamExtFloat) _param_ekf2_rng_a_igate, + (ParamExtFloat) _param_ekf2_rng_qlty_t, + (ParamExtFloat) _param_ekf2_rng_k_gate, + (ParamExtFloat) _param_ekf2_rng_pos_x, + (ParamExtFloat) _param_ekf2_rng_pos_y, + (ParamExtFloat) _param_ekf2_rng_pos_z, #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index 38584adda6..515c958367 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -67,6 +67,7 @@ menuconfig EKF2_OPTICAL_FLOW depends on MODULES_EKF2 bool "optical flow fusion support" default y + select EKF2_TERRAIN depends on EKF2_RANGE_FINDER ---help--- EKF2 optical flow fusion support. @@ -85,6 +86,14 @@ depends on MODULES_EKF2 ---help--- EKF2 sideslip fusion support. +menuconfig EKF2_TERRAIN +depends on MODULES_EKF2 + bool "terrain estimator support" + default y + depends on EKF2_OPTICAL_FLOW || EKF2_RANGE_FINDER + ---help--- + EKF2 terrain estimator support. + menuconfig USER_EKF2 bool "ekf2 running as userspace module" default n