ekf2: add kconfig option to enable/disable range finder fusion (and t…

…errain estimator)

src/modules/ekf2/CMakeLists.txt

@@ -87,10 +87,6 @@ list(APPEND EKF_SRCS
  EKF/mag_control.cpp
  EKF/mag_fusion.cpp
  EKF/output_predictor.cpp
- EKF/range_finder_consistency_check.cpp
- EKF/range_height_control.cpp
- EKF/sensor_range_finder.cpp
- EKF/terrain_estimator.cpp
  EKF/vel_pos_fusion.cpp
  EKF/zero_innovation_heading_update.cpp
  EKF/zero_velocity_update.cpp
@@ -129,6 +125,15 @@ if(CONFIG_EKF2_OPTICAL_FLOW)
  )
 endif()
 
+if(CONFIG_EKF2_RANGE_FINDER)
+ list(APPEND EKF_SRCS
+ EKF/range_finder_consistency_check.cpp
+ EKF/range_height_control.cpp
+ EKF/sensor_range_finder.cpp
+ EKF/terrain_estimator.cpp
+ )
+endif()
+
 if(CONFIG_EKF2_SIDESLIP)
  list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp)
 endif()

src/modules/ekf2/EKF/CMakeLists.txt

@@ -52,10 +52,6 @@ list(APPEND EKF_SRCS
  mag_control.cpp
  mag_fusion.cpp
  output_predictor.cpp
- range_finder_consistency_check.cpp
- range_height_control.cpp
- sensor_range_finder.cpp
- terrain_estimator.cpp
  vel_pos_fusion.cpp
  zero_innovation_heading_update.cpp
  zero_velocity_update.cpp
@@ -94,6 +90,15 @@ if(CONFIG_EKF2_OPTICAL_FLOW)
  )
 endif()
 
+if(CONFIG_EKF2_RANGE_FINDER)
+ list(APPEND EKF_SRCS
+ range_finder_consistency_check.cpp
+ range_height_control.cpp
+ sensor_range_finder.cpp
+ terrain_estimator.cpp
+ )
+endif()
+
 if(CONFIG_EKF2_SIDESLIP)
  list(APPEND EKF_SRCS sideslip_fusion.cpp)
 endif()

src/modules/ekf2/EKF/common.h

@@ -108,10 +108,12 @@ enum MagFuseType : uint8_t {
  NONE = 5 ///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
 };
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 enum TerrainFusionMask : uint8_t {
  TerrainFuseRangeFinder = (1 << 0),
  TerrainFuseOpticalFlow = (1 << 1)
 };
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 enum HeightSensor : uint8_t {
  BARO = 0,
@@ -300,17 +302,13 @@ struct parameters {
  int32_t position_sensor_ref{static_cast<int32_t>(PositionSensor::GNSS)};
  int32_t baro_ctrl{1};
  int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
- int32_t rng_ctrl{RngCtrl::CONDITIONAL};
- int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
- TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
 
  int32_t sensor_interval_max_ms{10}; ///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
 
  // measurement time delays
  float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec)
  float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec)
  float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec)
- float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec)
 
  // input noise
  float gyro_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec)
@@ -324,10 +322,6 @@ struct parameters {
  float wind_vel_nsd{1.0e-2f}; ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
  const float wind_vel_nsd_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity
 
- 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)
-
  // initialization errors
  float switch_on_gyro_bias{0.1f}; ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
  float switch_on_accel_bias{0.2f}; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
@@ -379,7 +373,14 @@ 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_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))
@@ -394,6 +395,14 @@ struct parameters {
  float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
  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)
  // vision position fusion
  int32_t ev_ctrl{0};
@@ -406,6 +415,8 @@ struct parameters {
  float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD)
  float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD)
  float ev_hgt_bias_nsd{0.13f}; ///< process noise for vision height bias estimation (m/s/sqrt(Hz))
+
+ Vector3f ev_pos_body{}; ///< xyz position of VI-sensor focal point in body frame (m)
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
  // gravity fusion
@@ -419,6 +430,8 @@ struct parameters {
  float flow_noise_qual_min{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
  int32_t flow_qual_min{1}; ///< minimum acceptable quality integer from the flow sensor
  float flow_innov_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD)
+
+ Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m)
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
  // these parameters control the strictness of GPS quality checks used to determine if the GPS is
@@ -435,9 +448,6 @@ struct parameters {
  // XYZ offset of sensors in body axes (m)
  Vector3f imu_pos_body{}; ///< xyz position of IMU in body frame (m)
  Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m)
- Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m)
- Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m)
- Vector3f ev_pos_body{}; ///< xyz position of VI-sensor focal point in body frame (m)
 
  // accel bias learning control
  float acc_bias_lim{0.4f}; ///< maximum accel bias magnitude (m/sec**2)
@@ -617,13 +627,15 @@ union ekf_solution_status_u {
  uint16_t value;
 };
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 union terrain_fusion_status_u {
  struct {
  bool range_finder : 1; ///< 0 - true if we are fusing range finder data
  bool flow : 1; ///< 1 - true if we are fusing flow data
  } flags;
  uint8_t value;
 };
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 // define structure used to communicate information events
 union information_event_status_u {

src/modules/ekf2/EKF/covariance.cpp

@@ -69,16 +69,17 @@ void Ekf::initialiseCovariance()
  // position
  P(7,7) = sq(fmaxf(_params.gps_pos_noise, 0.01f));
  P(8,8) = P(7,7);
+ P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f));
 
- if (_control_status.flags.rng_hgt) {
- P(9,9) = sq(fmaxf(_params.range_noise, 0.01f));
-
- } else if (_control_status.flags.gps_hgt) {
+ if (_control_status.flags.gps_hgt) {
  P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
+ }
 
- } else {
- P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f));
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+ if (_control_status.flags.rng_hgt) {
+ P(9,9) = sq(fmaxf(_params.range_noise, 0.01f));
  }
+#endif // CONFIG_EKF2_RANGE_FINDER
 
  // gyro bias
  _prev_delta_ang_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias * dt);
@@ -453,7 +454,12 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
  if (bad_vz_gps || bad_vz_ev) {
  bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.0f);
  bool bad_z_gps = _control_status.flags.gps_hgt && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f);
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
  bool bad_z_rng = _control_status.flags.rng_hgt && (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.0f);
+#else
+ bool bad_z_rng = false;
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
  bool bad_z_ev = _control_status.flags.ev_hgt && (down_dvel_bias * _aid_src_ev_hgt.innovation < 0.0f);

src/modules/ekf2/EKF/ekf.cpp

@@ -63,9 +63,11 @@ void Ekf::reset()
  _state.wind_vel.setZero();
  _state.quat_nominal.setIdentity();
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
  _range_sensor.setPitchOffset(_params.rng_sens_pitch);
  _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
  _range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
+#endif // CONFIG_EKF2_RANGE_FINDER
 
  _control_status.value = 0;
  _control_status_prev.value = 0;
@@ -94,17 +96,13 @@ void Ekf::reset()
  _time_last_hor_vel_fuse = 0;
  _time_last_ver_vel_fuse = 0;
  _time_last_heading_fuse = 0;
-
- _time_last_flow_terrain_fuse = 0;
  _time_last_zero_velocity_fuse = 0;
- _time_last_healthy_rng_data = 0;
 
  _last_known_pos.setZero();
 
  _time_acc_bias_check = 0;
 
  _gps_checks_passed = false;
-
  _gps_alt_ref = NAN;
 
  _baro_counter = 0;
@@ -115,7 +113,6 @@ void Ekf::reset()
  _clip_counter = 0;
 
  resetEstimatorAidStatus(_aid_src_baro_hgt);
- resetEstimatorAidStatus(_aid_src_rng_hgt);
 #if defined(CONFIG_EKF2_AIRSPEED)
  resetEstimatorAidStatus(_aid_src_airspeed);
 #endif // CONFIG_EKF2_AIRSPEED
@@ -152,6 +149,10 @@ void Ekf::reset()
  resetEstimatorAidStatus(_aid_src_optical_flow);
  resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+ resetEstimatorAidStatus(_aid_src_rng_hgt);
+#endif // CONFIG_EKF2_RANGE_FINDER
 }
 
 bool Ekf::update()
@@ -179,8 +180,10 @@ bool Ekf::update()
  // control fusion of observation data
  controlFusionModes(imu_sample_delayed);
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
  // run a separate filter for terrain estimation
  runTerrainEstimator(imu_sample_delayed);
+#endif // CONFIG_EKF2_RANGE_FINDER
 
  _output_predictor.correctOutputStates(imu_sample_delayed.time_us, getGyroBias(), getAccelBias(),
  _state.quat_nominal, _state.vel, _state.pos);
@@ -218,8 +221,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)
  // Initialise the terrain estimator
  initHagl();
+#endif // CONFIG_EKF2_RANGE_FINDER
 
  // reset the output predictor state history to match the EKF initial values
  _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);