dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: add dedicated EKF2_CONFIG_TERRAIN in kconfig 

- new estimator_aid_src_terrain_range_finder for HAGL RNG
This commit is contained in:
Daniel Agar 2023-08-31 13:21:41 -04:00
parent 845b01a00d
commit 10b54d08fc
14 changed files with 367 additions and 253 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/EstimatorAidSource1d.msg
View File

@ -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_airspeed estimator_aid_src_sideslip
# TOPICS estimator_aid_src_fake_hgt # 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_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
# TOPICS estimator_aid_src_terrain_range_finder

5
src/modules/ekf2/CMakeLists.txt
View File

@ -139,7 +139,6 @@ if(CONFIG_EKF2_RANGE_FINDER)
EKF/range_finder_consistency_check.cpp EKF/range_finder_consistency_check.cpp
EKF/range_height_control.cpp EKF/range_height_control.cpp
EKF/sensor_range_finder.cpp EKF/sensor_range_finder.cpp
EKF/terrain_estimator.cpp
) )
endif() endif()
@ -147,6 +146,10 @@ if(CONFIG_EKF2_SIDESLIP)
list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp) list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp)
endif() endif()
if(CONFIG_EKF2_TERRAIN)
list(APPEND EKF_SRCS EKF/terrain_estimator.cpp)
endif()
px4_add_module( px4_add_module(
MODULE modules__ekf2 MODULE modules__ekf2
MAIN ekf2 MAIN ekf2

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

@ -104,7 +104,6 @@ if(CONFIG_EKF2_RANGE_FINDER)
range_finder_consistency_check.cpp range_finder_consistency_check.cpp
range_height_control.cpp range_height_control.cpp
sensor_range_finder.cpp sensor_range_finder.cpp
terrain_estimator.cpp
) )
endif() endif()
@ -112,6 +111,10 @@ if(CONFIG_EKF2_SIDESLIP)
list(APPEND EKF_SRCS sideslip_fusion.cpp) list(APPEND EKF_SRCS sideslip_fusion.cpp)
endif() endif()
if(CONFIG_EKF2_TERRAIN)
list(APPEND EKF_SRCS terrain_estimator.cpp)
endif()
add_library(ecl_EKF add_library(ecl_EKF
${EKF_SRCS} ${EKF_SRCS}
) )

30
src/modules/ekf2/EKF/common.h
View File

@ -105,12 +105,12 @@ enum MagFuseType : uint8_t {
NONE = 5 ///< Do not use magnetometer under any circumstance.. NONE = 5 ///< Do not use magnetometer under any circumstance..
}; };
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_TERRAIN)
enum TerrainFusionMask : uint8_t { enum TerrainFusionMask : uint8_t {
TerrainFuseRangeFinder = (1 << 0), TerrainFuseRangeFinder = (1 << 0),
TerrainFuseOpticalFlow = (1 << 1) TerrainFuseOpticalFlow = (1 << 1)
}; };
#endif // CONFIG_EKF2_RANGE_FINDER #endif // CONFIG_EKF2_TERRAIN
enum HeightSensor : uint8_t { enum HeightSensor : uint8_t {
BARO = 0, BARO = 0,
@ -362,18 +362,27 @@ struct parameters {
const float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec) const float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec)
#endif // CONFIG_EKF2_SIDESLIP #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) #if defined(CONFIG_EKF2_RANGE_FINDER)
// range finder fusion // range finder fusion
int32_t rng_ctrl{RngCtrl::CONDITIONAL}; 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_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_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 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_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 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) 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 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 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) 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 #endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_EXTERNAL_VISION) #if defined(CONFIG_EKF2_EXTERNAL_VISION)
@ -625,7 +629,7 @@ union ekf_solution_status_u {
uint16_t value; uint16_t value;
}; };
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_TERRAIN)
union terrain_fusion_status_u { union terrain_fusion_status_u {
struct { struct {
bool range_finder : 1; ///< 0 - true if we are fusing range finder data bool range_finder : 1; ///< 0 - true if we are fusing range finder data
@ -633,7 +637,7 @@ union terrain_fusion_status_u {
} flags; } flags;
uint8_t value; uint8_t value;
}; };
#endif // CONFIG_EKF2_RANGE_FINDER #endif // CONFIG_EKF2_TERRAIN
// define structure used to communicate information events // define structure used to communicate information events
union information_event_status_u { union information_event_status_u {

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

@ -189,10 +189,10 @@ bool Ekf::update()
// control fusion of observation data // control fusion of observation data
controlFusionModes(imu_sample_delayed); controlFusionModes(imu_sample_delayed);
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_TERRAIN)
// run a separate filter for terrain estimation // run a separate filter for terrain estimation
runTerrainEstimator(imu_sample_delayed); 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); _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 // initialise the state covariance matrix now we have starting values for all the states
initialiseCovariance(); initialiseCovariance();
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_TERRAIN)
// Initialise the terrain estimator // Initialise the terrain estimator
initHagl(); initHagl();
#endif // CONFIG_EKF2_RANGE_FINDER #endif // CONFIG_EKF2_TERRAIN
// reset the output predictor state history to match the EKF initial values // reset the output predictor state history to match the EKF initial values
_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos); _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);

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

@ -95,23 +95,7 @@ public:
void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; } 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; } void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; }
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_TERRAIN)
// 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(); }
// terrain estimate // terrain estimate
bool isTerrainEstimateValid() const; bool isTerrainEstimateValid() const;
@ -125,13 +109,64 @@ public:
// get the terrain variance // get the terrain variance
float get_terrain_var() const { return _terrain_var; } 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 #endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW) #if defined(CONFIG_EKF2_OPTICAL_FLOW)
const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; } const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
void getFlowInnov(float flow_innov[2]) const; void getFlowInnov(float flow_innov[2]) const
void getFlowInnovVar(float flow_innov_var[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]); } 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; } 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 getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); }
const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; } 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 #endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_AUXVEL) #if defined(CONFIG_EKF2_AUXVEL)
@ -598,30 +627,33 @@ private:
Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2) Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2)
#endif // CONFIG_EKF2_DRAG_FUSION #endif // CONFIG_EKF2_DRAG_FUSION
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_TERRAIN)
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};
// Terrain height state estimation // 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_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) 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 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) 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 #endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW) #if defined(CONFIG_EKF2_OPTICAL_FLOW)
estimator_aid_source2d_s _aid_src_optical_flow{}; estimator_aid_source2d_s _aid_src_optical_flow{};
estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
// optical flow processing // optical flow processing
Vector3f _flow_gyro_bias{}; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) 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 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 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 #endif // CONFIG_EKF2_OPTICAL_FLOW
estimator_aid_source1d_s _aid_src_baro_hgt{}; 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_source2d_s &vel_aid_src);
void fuseVelocity(estimator_aid_source3d_s &vel_aid_src); void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_TERRAIN)
// range height
void controlRangeHeightFusion();
bool isConditionalRangeAidSuitable();
void stopRngHgtFusion();
// terrain vertical position estimator // terrain vertical position estimator
void initHagl(); void initHagl();
void runTerrainEstimator(const imuSample &imu_delayed); void runTerrainEstimator(const imuSample &imu_delayed);
@ -880,16 +906,33 @@ private:
float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; } 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 // update the terrain vertical position estimate using a height above ground measurement from the range finder
void controlHaglRngFusion(); void controlHaglRngFusion();
void fuseHaglRng(); void updateHaglRng(estimator_aid_source1d_s &aid_src) const;
void startHaglRngFusion(); void fuseHaglRng(estimator_aid_source1d_s &aid_src);
void resetHaglRngIfNeeded();
void resetHaglRng(); void resetHaglRng();
void stopHaglRngFusion(); 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 #endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW) #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@ -912,13 +955,6 @@ private:
void fuseOptFlow(); void fuseOptFlow();
float predictFlowRange(); float predictFlowRange();
Vector2f predictFlowVelBody(); 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 #endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_MAGNETOMETER) #if defined(CONFIG_EKF2_MAGNETOMETER)

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

@ -363,32 +363,6 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
} }
#endif // CONFIG_EKF2_AUXVEL #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 // get the state vector at the delayed time horizon
matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const matrix::Vector<float, 24> 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_min = rangefinder_hagl_min;
*hagl_max = rangefinder_hagl_max; *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 // Keep within flow AND range sensor limits when exclusively using optical flow
const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_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_min = flow_hagl_min;
*hagl_max = flow_hagl_max; *hagl_max = flow_hagl_max;
} }
#endif // CONFIG_EKF2_OPTICAL_FLOW # endif // CONFIG_EKF2_OPTICAL_FLOW
#endif // CONFIG_EKF2_RANGE_FINDER
} }
void Ekf::resetImuBias() 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); tas = sqrtf(_aid_src_airspeed.test_ratio);
#endif // CONFIG_EKF2_AIRSPEED #endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_RANGE_FINDER) hagl = NAN;
// return the terrain height innovation test ratio #if defined(CONFIG_EKF2_TERRAIN)
hagl = sqrtf(_hagl_test_ratio); # 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 #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) #if defined(CONFIG_EKF2_SIDESLIP)
// return the synthetic sideslip innovation test ratio // return the synthetic sideslip innovation test ratio
beta = sqrtf(_aid_src_sideslip.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_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_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; soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
#if defined(CONFIG_EKF2_TERRAIN)
soln_status.flags.pos_vert_agl = isTerrainEstimateValid(); 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.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_rel = soln_status.flags.pos_horiz_rel;
soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs; soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
@ -927,12 +916,15 @@ float Ekf::getYawVar() const
void Ekf::updateGroundEffect() void Ekf::updateGroundEffect()
{ {
if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) { if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {
#if defined(CONFIG_EKF2_TERRAIN)
if (isTerrainEstimateValid()) { if (isTerrainEstimateValid()) {
// automatically set ground effect if terrain is valid // automatically set ground effect if terrain is valid
float height = _terrain_vpos - _state.pos(2); float height = _terrain_vpos - _state.pos(2);
_control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt); _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 // Turn off ground effect compensation if it times out
if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
_control_status.flags.gnd_effect = false; _control_status.flags.gnd_effect = false;

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

@ -123,17 +123,17 @@ void Ekf::controlMagFusion()
bool Ekf::checkHaglYawResetReq() const bool Ekf::checkHaglYawResetReq() const
{ {
#if defined(CONFIG_EKF2_TERRAIN)
// We need to reset the yaw angle after climbing away from the ground to enable // We need to reset the yaw angle after climbing away from the ground to enable
// recovery from ground level magnetic interference. // recovery from ground level magnetic interference.
if (_control_status.flags.in_air && _control_status.flags.yaw_align && !_control_status.flags.mag_aligned_in_flight) { 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 // Check if height has increased sufficiently to be away from ground magnetic anomalies
// and request a yaw reset if not already requested. // and request a yaw reset if not already requested.
#if defined(CONFIG_EKF2_RANGE_FINDER)
static constexpr float mag_anomalies_max_hagl = 1.5f; static constexpr float mag_anomalies_max_hagl = 1.5f;
const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl; const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
return above_mag_anomalies; return above_mag_anomalies;
#endif // CONFIG_EKF2_RANGE_FINDER
} }
#endif // CONFIG_EKF2_TERRAIN
return false; return false;
} }

2
src/modules/ekf2/EKF/optical_flow_control.cpp
View File

@ -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)) { 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 // 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 // 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(); fuseOptFlow();
_last_known_pos.xy() = _state.pos.xy(); _last_known_pos.xy() = _state.pos.xy();
} }

7
src/modules/ekf2/EKF/range_height_control.cpp
View File

@ -204,6 +204,7 @@ void Ekf::controlRangeHeightFusion()
bool Ekf::isConditionalRangeAidSuitable() bool Ekf::isConditionalRangeAidSuitable()
{ {
#if defined(CONFIG_EKF2_TERRAIN)
if (_control_status.flags.in_air if (_control_status.flags.in_air
&& _range_sensor.isHealthy() && _range_sensor.isHealthy()
&& isTerrainEstimateValid()) { && isTerrainEstimateValid()) {
@ -212,7 +213,10 @@ bool Ekf::isConditionalRangeAidSuitable()
float range_hagl_max = _params.max_hagl_for_range_aid; float range_hagl_max = _params.max_hagl_for_range_aid;
float max_vel_xy = _params.max_vel_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); 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; return is_in_range && is_hagl_stable && is_below_max_speed;
} }
#endif // CONFIG_EKF2_TERRAIN
return false; return false;
} }

174
src/modules/ekf2/EKF/terrain_estimator.cpp
View File

@ -49,7 +49,9 @@ void Ekf::initHagl()
stopHaglFlowFusion(); stopHaglFlowFusion();
#endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_RANGE_FINDER)
stopHaglRngFusion(); stopHaglRngFusion();
#endif // CONFIG_EKF2_RANGE_FINDER
// assume a ground clearance // assume a ground clearance
_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance; _terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
@ -68,10 +70,14 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
predictHagl(imu_delayed); predictHagl(imu_delayed);
#if defined(CONFIG_EKF2_RANGE_FINDER)
controlHaglRngFusion(); controlHaglRngFusion();
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW) #if defined(CONFIG_EKF2_OPTICAL_FLOW)
controlHaglFlowFusion(); controlHaglFlowFusion();
#endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_OPTICAL_FLOW
controlHaglFakeFusion(); controlHaglFakeFusion();
// constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2) // 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); _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
} }
#if defined(CONFIG_EKF2_RANGE_FINDER)
void Ekf::controlHaglRngFusion() void Ekf::controlHaglRngFusion()
{ {
if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder) if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder)
@ -104,20 +111,29 @@ void Ekf::controlHaglRngFusion()
return; return;
} }
if (_range_sensor.isDataReady()) {
updateHaglRng(_aid_src_terrain_range_finder);
}
if (_range_sensor.isDataHealthy()) { 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 continuing_conditions_passing = _control_status.flags.in_air
const bool starting_conditions_passing = continuing_conditions_passing && _range_sensor.isRegularlySendingData() && (_rng_consistency_check.getTestRatio() < 1.f); //&& !_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; _time_last_healthy_rng_data = _time_delayed_us;
if (_hagl_sensor_status.flags.range_finder) { if (_hagl_sensor_status.flags.range_finder) {
if (continuing_conditions_passing) { if (continuing_conditions_passing) {
fuseHaglRng(); fuseHaglRng(_aid_src_terrain_range_finder);
// We have been rejecting range data for too long // We have been rejecting range data for too long
const uint64_t timeout = static_cast<uint64_t>(_params.terrain_timeout * 1e6f); const uint64_t timeout = static_cast<uint64_t>(_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 (is_fusion_failing) {
if (_range_sensor.getDistBottom() > 2.f * _params.rng_gnd_clearance) { if (_range_sensor.getDistBottom() > 2.f * _params.rng_gnd_clearance) {
@ -143,7 +159,16 @@ void Ekf::controlHaglRngFusion()
} else { } else {
if (starting_conditions_passing) { 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. // No data anymore. Stop until it comes back.
stopHaglRngFusion(); stopHaglRngFusion();
} }
_aid_src_terrain_range_finder.fusion_enabled = _hagl_sensor_status.flags.range_finder;
} }
void Ekf::startHaglRngFusion() float Ekf::getRngVar() const
{
_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()
{ {
return fmaxf(P(9, 9) * _params.vehicle_variance_scaler, 0.0f) return fmaxf(P(9, 9) * _params.vehicle_variance_scaler, 0.0f)
+ sq(_params.range_noise) + sq(_params.range_noise)
@ -198,26 +192,25 @@ void Ekf::resetHaglRng()
_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom(); _terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
_terrain_var = getRngVar(); _terrain_var = getRngVar();
_terrain_vpos_reset_counter++; _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() void Ekf::stopHaglRngFusion()
{ {
if (_hagl_sensor_status.flags.range_finder) { 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; _innov_check_fail_status.flags.reject_hagl = false;
_hagl_sensor_status.flags.range_finder = 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 // get a height above ground measurement from the range finder assuming a flat earth
const float meas_hagl = _range_sensor.getDistBottom(); const float meas_hagl = _range_sensor.getDistBottom();
@ -226,33 +219,56 @@ void Ekf::fuseHaglRng()
const float pred_hagl = _terrain_vpos - _state.pos(2); const float pred_hagl = _terrain_vpos - _state.pos(2);
// calculate the innovation // 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 // calculate the observation variance adding the variance of the vehicles own height uncertainty
const float obs_variance = getRngVar(); const float obs_variance = getRngVar();
// calculate the innovation variance - limiting it to prevent a badly conditioned fusion // 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 // perform an innovation consistency check and only fuse data if it passes
const float gate_size = fmaxf(_params.range_innov_gate, 1.0f); const float innov_gate = fmaxf(_params.range_innov_gate, 1.0f);
_hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
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 // calculate the Kalman gain
const float gain = _terrain_var / _hagl_innov_var; const float gain = _terrain_var / aid_src.innovation_variance;
// correct the state // correct the state
_terrain_vpos -= gain * _hagl_innov; _terrain_vpos -= gain * aid_src.innovation;
// correct the variance // correct the variance
_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
// record last successful fusion event // record last successful fusion event
_time_last_hagl_fuse = _time_delayed_us;
_innov_check_fail_status.flags.reject_hagl = false; _innov_check_fail_status.flags.reject_hagl = false;
aid_src.time_last_fuse = _time_delayed_us;
aid_src.fused = true;
} else { } else {
_innov_check_fail_status.flags.reject_hagl = true; _innov_check_fail_status.flags.reject_hagl = true;
aid_src.fused = false;
} }
} }
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW) #if defined(CONFIG_EKF2_OPTICAL_FLOW)
void Ekf::controlHaglFlowFusion() void Ekf::controlHaglFlowFusion()
@ -266,10 +282,10 @@ void Ekf::controlHaglFlowFusion()
updateOptFlow(_aid_src_terrain_optical_flow); updateOptFlow(_aid_src_terrain_optical_flow);
const bool continuing_conditions_passing = _control_status.flags.in_air const bool continuing_conditions_passing = _control_status.flags.in_air
&& !_control_status.flags.opt_flow && !_control_status.flags.opt_flow
&& _control_status.flags.gps && _control_status.flags.gps
&& isTimedOut(_time_last_hagl_fuse, 5e6f); // TODO: check for range_finder hagl aiding instead? && !_hagl_sensor_status.flags.range_finder;
/* && !_hagl_sensor_status.flags.range_finder; */
const bool starting_conditions_passing = continuing_conditions_passing; const bool starting_conditions_passing = continuing_conditions_passing;
if (_hagl_sensor_status.flags.flow) { 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 // TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon
fuseFlowForTerrain(_aid_src_terrain_optical_flow); fuseFlowForTerrain(_aid_src_terrain_optical_flow);
_flow_data_ready = false;
// TODO: do something when failing continuously the innovation check // 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) { */ /* if (is_fusion_failing) { */
/* resetHaglFlow(); */ /* resetHaglFlow(); */
@ -292,28 +307,27 @@ void Ekf::controlHaglFlowFusion()
} else { } else {
if (starting_conditions_passing) { 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 _flow_data_ready = false;
&& (_time_delayed_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) {
} 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. // No data anymore. Stop until it comes back.
stopHaglFlowFusion(); stopHaglFlowFusion();
} }
}
void Ekf::startHaglFlowFusion() _aid_src_terrain_optical_flow.fusion_enabled = _hagl_sensor_status.flags.flow;
{
_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;
} }
void Ekf::stopHaglFlowFusion() void Ekf::stopHaglFlowFusion()
{ {
if (_hagl_sensor_status.flags.flow) { if (_hagl_sensor_status.flags.flow) {
ECL_INFO("stopping HAGL flow fusion");
_hagl_sensor_status.flags.flow = false; _hagl_sensor_status.flags.flow = false;
resetEstimatorAidStatus(_aid_src_terrain_optical_flow); resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
} }
@ -325,11 +339,14 @@ void Ekf::resetHaglFlow()
_terrain_vpos = fmaxf(0.0f, _state.pos(2)); _terrain_vpos = fmaxf(0.0f, _state.pos(2));
_terrain_var = 100.0f; _terrain_var = 100.0f;
_terrain_vpos_reset_counter++; _terrain_vpos_reset_counter++;
_aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us;
} }
void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow) void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow)
{ {
flow.fusion_enabled = true; flow.fusion_enabled = true;
flow.fused = true;
const float R_LOS = flow.observation_variance[0]; 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_X = false;
_fault_status.flags.bad_optflow_Y = false; _fault_status.flags.bad_optflow_Y = false;
_time_last_flow_terrain_fuse = _time_delayed_us; flow.time_last_fuse = _time_delayed_us;
//_aid_src_optical_flow.time_last_fuse = _time_delayed_us; // TODO: separate aid source status for OF terrain? flow.fused = true;
_aid_src_optical_flow.fused = true;
} }
#endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_OPTICAL_FLOW
@ -416,18 +432,22 @@ void Ekf::controlHaglFakeFusion()
bool Ekf::isTerrainEstimateValid() const bool Ekf::isTerrainEstimateValid() const
{ {
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_RANGE_FINDER)
// we have been fusing range finder measurements in the last 5 seconds // 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; return true;
} }
#endif // CONFIG_EKF2_RANGE_FINDER #endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW) #if defined(CONFIG_EKF2_OPTICAL_FLOW)
// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds // 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 // 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; return true;
} }
#endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_OPTICAL_FLOW
return false; return false;

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

@ -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_baro_ctrl(_params->baro_ctrl),
_param_ekf2_gps_ctrl(_params->gnss_ctrl), _param_ekf2_gps_ctrl(_params->gnss_ctrl),
_param_ekf2_noaid_tout(_params->valid_timeout_max), _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) #if defined(CONFIG_EKF2_RANGE_FINDER)
_param_ekf2_rng_ctrl(_params->rng_ctrl), _param_ekf2_rng_ctrl(_params->rng_ctrl),
_param_ekf2_rng_delay(_params->range_delay_ms), _param_ekf2_rng_delay(_params->range_delay_ms),
_param_ekf2_rng_noise(_params->range_noise), _param_ekf2_rng_noise(_params->range_noise),
_param_ekf2_rng_sfe(_params->range_noise_scaler), _param_ekf2_rng_sfe(_params->range_noise_scaler),
_param_ekf2_rng_gate(_params->range_innov_gate), _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_pitch(_params->rng_sens_pitch),
_param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid), _param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid),
_param_ekf2_rng_a_hmax(_params->max_hagl_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_x(_params->rng_pos_body(0)),
_param_ekf2_rng_pos_y(_params->rng_pos_body(1)), _param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
_param_ekf2_rng_pos_z(_params->rng_pos_body(2)), _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 #endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_EXTERNAL_VISION) #if defined(CONFIG_EKF2_EXTERNAL_VISION)
_param_ekf2_ev_delay(_params->ev_delay_ms), _param_ekf2_ev_delay(_params->ev_delay_ms),
@ -1022,9 +1026,22 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
#if defined(CONFIG_EKF2_OPTICAL_FLOW) #if defined(CONFIG_EKF2_OPTICAL_FLOW)
// optical flow // optical flow
PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub); 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, PublishAidSourceStatus(_ekf.aid_src_terrain_optical_flow(), _status_terrain_optical_flow_pub_last,
_estimator_aid_src_terrain_optical_flow_pub); _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 &timestamp) void EKF2::PublishAttitude(const hrt_abstime &timestamp)
@ -1247,7 +1264,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
global_pos.terrain_alt = NAN; global_pos.terrain_alt = NAN;
global_pos.terrain_alt_valid = false; global_pos.terrain_alt_valid = false;
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_TERRAIN)
if (_ekf.isTerrainEstimateValid()) { if (_ekf.isTerrainEstimateValid()) {
// Terrain altitude in m, WGS84 // Terrain altitude in m, WGS84
@ -1255,7 +1272,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
global_pos.terrain_alt_valid = true; 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 global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning
|| _ekf.control_status_flags().wind_dead_reckoning; || _ekf.control_status_flags().wind_dead_reckoning;
@ -1318,7 +1335,6 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
#endif // CONFIG_EKF2_AUXVEL #endif // CONFIG_EKF2_AUXVEL
#if defined(CONFIG_EKF2_OPTICAL_FLOW) #if defined(CONFIG_EKF2_OPTICAL_FLOW)
_ekf.getFlowInnov(innovations.flow); _ekf.getFlowInnov(innovations.flow);
_ekf.getTerrainFlowInnov(innovations.terr_flow);
#endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_OPTICAL_FLOW
_ekf.getHeadingInnov(innovations.heading); _ekf.getHeadingInnov(innovations.heading);
#if defined(CONFIG_EKF2_MAGNETOMETER) #if defined(CONFIG_EKF2_MAGNETOMETER)
@ -1333,11 +1349,18 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
#if defined(CONFIG_EKF2_SIDESLIP) #if defined(CONFIG_EKF2_SIDESLIP)
_ekf.getBetaInnov(innovations.beta); _ekf.getBetaInnov(innovations.beta);
#endif // CONFIG_EKF2_SIDESLIP #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); _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 // Not yet supported
innovations.aux_vvel = NAN; innovations.aux_vvel = NAN;
@ -1355,11 +1378,13 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps); _preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
#if defined(CONFIG_EKF2_OPTICAL_FLOW) #if defined(CONFIG_EKF2_OPTICAL_FLOW)
_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_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 // set dist bottom to scale flow innovation
const float dist_bottom = _ekf.getTerrainVertPos() - _ekf.getPosition()(2); const float dist_bottom = _ekf.getTerrainVertPos() - _ekf.getPosition()(2);
_preflt_checker.setDistBottom(dist_bottom); _preflt_checker.setDistBottom(dist_bottom);
#endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_EXTERNAL_VISION) #if defined(CONFIG_EKF2_EXTERNAL_VISION)
_preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos); _preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos);
@ -1392,13 +1417,13 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
_ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos); _ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos);
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_RANGE_FINDER)
_ekf.getRngHgtInnovRatio(test_ratios.rng_vpos); _ekf.getRngHgtInnovRatio(test_ratios.rng_vpos);
_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
#endif // CONFIG_EKF2_RANGE_FINDER #endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_AUXVEL) #if defined(CONFIG_EKF2_AUXVEL)
_ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]); _ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]);
#endif // CONFIG_EKF2_AUXVEL #endif // CONFIG_EKF2_AUXVEL
#if defined(CONFIG_EKF2_OPTICAL_FLOW) #if defined(CONFIG_EKF2_OPTICAL_FLOW)
_ekf.getFlowInnovRatio(test_ratios.flow[0]); _ekf.getFlowInnovRatio(test_ratios.flow[0]);
_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
#endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_OPTICAL_FLOW
_ekf.getHeadingInnovRatio(test_ratios.heading); _ekf.getHeadingInnovRatio(test_ratios.heading);
#if defined(CONFIG_EKF2_MAGNETOMETER) #if defined(CONFIG_EKF2_MAGNETOMETER)
@ -1413,11 +1438,18 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
#if defined(CONFIG_EKF2_SIDESLIP) #if defined(CONFIG_EKF2_SIDESLIP)
_ekf.getBetaInnovRatio(test_ratios.beta); _ekf.getBetaInnovRatio(test_ratios.beta);
#endif // CONFIG_EKF2_SIDESLIP #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]); _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 // Not yet supported
test_ratios.aux_vvel = NAN; test_ratios.aux_vvel = NAN;
@ -1437,13 +1469,13 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
_ekf.getBaroHgtInnovVar(variances.baro_vpos); _ekf.getBaroHgtInnovVar(variances.baro_vpos);
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_RANGE_FINDER)
_ekf.getRngHgtInnovVar(variances.rng_vpos); _ekf.getRngHgtInnovVar(variances.rng_vpos);
_ekf.getHaglRateInnovVar(variances.hagl_rate);
#endif // CONFIG_EKF2_RANGE_FINDER #endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_AUXVEL) #if defined(CONFIG_EKF2_AUXVEL)
_ekf.getAuxVelInnovVar(variances.aux_hvel); _ekf.getAuxVelInnovVar(variances.aux_hvel);
#endif // CONFIG_EKF2_AUXVEL #endif // CONFIG_EKF2_AUXVEL
#if defined(CONFIG_EKF2_OPTICAL_FLOW) #if defined(CONFIG_EKF2_OPTICAL_FLOW)
_ekf.getFlowInnovVar(variances.flow); _ekf.getFlowInnovVar(variances.flow);
_ekf.getTerrainFlowInnovVar(variances.terr_flow);
#endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_OPTICAL_FLOW
_ekf.getHeadingInnovVar(variances.heading); _ekf.getHeadingInnovVar(variances.heading);
#if defined(CONFIG_EKF2_MAGNETOMETER) #if defined(CONFIG_EKF2_MAGNETOMETER)
@ -1458,11 +1490,18 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
#if defined(CONFIG_EKF2_SIDESLIP) #if defined(CONFIG_EKF2_SIDESLIP)
_ekf.getBetaInnovVar(variances.beta); _ekf.getBetaInnovVar(variances.beta);
#endif // CONFIG_EKF2_SIDESLIP #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.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); _ekf.getGravityInnovVar(variances.gravity);
// Not yet supported // Not yet supported
variances.aux_vvel = NAN; variances.aux_vvel = NAN;
@ -1530,10 +1569,12 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
lpos.delta_heading = Eulerf(delta_q_reset).psi(); lpos.delta_heading = Eulerf(delta_q_reset).psi();
lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete(); lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();
#if defined(CONFIG_EKF2_TERRAIN)
// Distance to bottom surface (ground) in meters, must be positive // Distance to bottom surface (ground) in meters, must be positive
lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, 0.f); lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, 0.f);
lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid(); lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield(); lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield();
#endif // CONFIG_EKF2_TERRAIN
_ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv); _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv);
_ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv); _ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv);

80
src/modules/ekf2/EKF2.hpp
View File

@ -303,7 +303,6 @@ private:
hrt_abstime _last_gps_status_published{0}; hrt_abstime _last_gps_status_published{0};
hrt_abstime _status_baro_hgt_pub_last{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_hgt_pub_last{0};
hrt_abstime _status_fake_pos_pub_last{0}; hrt_abstime _status_fake_pos_pub_last{0};
@ -353,7 +352,6 @@ private:
hrt_abstime _status_gnss_yaw_pub_last {0}; hrt_abstime _status_gnss_yaw_pub_last {0};
#endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS_YAW
hrt_abstime _status_gravity_pub_last{0}; hrt_abstime _status_gravity_pub_last{0};
#if defined(CONFIG_EKF2_AUXVEL) #if defined(CONFIG_EKF2_AUXVEL)
@ -365,20 +363,31 @@ private:
perf_counter_t _msg_missed_landing_target_pose_perf{nullptr}; perf_counter_t _msg_missed_landing_target_pose_perf{nullptr};
#endif // CONFIG_EKF2_AUXVEL #endif // CONFIG_EKF2_AUXVEL
#if defined(CONFIG_EKF2_TERRAIN)
# if defined(CONFIG_EKF2_RANGE_FINDER)
uORB::PublicationMulti<estimator_aid_source1d_s> _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_source2d_s> _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) #if defined(CONFIG_EKF2_OPTICAL_FLOW)
uORB::Subscription _vehicle_optical_flow_sub {ORB_ID(vehicle_optical_flow)}; uORB::Subscription _vehicle_optical_flow_sub {ORB_ID(vehicle_optical_flow)};
uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_terrain_optical_flow_pub{ORB_ID(estimator_aid_src_terrain_optical_flow)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
hrt_abstime _status_optical_flow_pub_last{0}; 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}; perf_counter_t _msg_missed_optical_flow_perf{nullptr};
#endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_OPTICAL_FLOW
float _last_baro_bias_published{}; float _last_baro_bias_published{};
float _last_gnss_hgt_bias_published{}; float _last_gnss_hgt_bias_published{};
float _last_rng_hgt_bias_published{};
#if defined(CONFIG_EKF2_AIRSPEED) #if defined(CONFIG_EKF2_AIRSPEED)
uORB::Subscription _airspeed_sub {ORB_ID(airspeed)}; uORB::Subscription _airspeed_sub {ORB_ID(airspeed)};
@ -414,6 +423,9 @@ private:
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_RANGE_FINDER)
hrt_abstime _status_rng_hgt_pub_last {0};
float _last_rng_hgt_bias_published{};
uORB::PublicationMulti<estimator_bias_s> _estimator_rng_hgt_bias_pub {ORB_ID(estimator_rng_hgt_bias)}; uORB::PublicationMulti<estimator_bias_s> _estimator_rng_hgt_bias_pub {ORB_ID(estimator_rng_hgt_bias)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)}; uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)};
@ -590,42 +602,30 @@ private:
(ParamExtInt<px4::params::EKF2_NOAID_TOUT>) (ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
_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) _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<px4::params::EKF2_MIN_RNG>) _param_ekf2_min_rng,
#endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_TERRAIN)
(ParamExtInt<px4::params::EKF2_TERR_MASK>) _param_ekf2_terr_mask,
(ParamExtFloat<px4::params::EKF2_TERR_NOISE>) _param_ekf2_terr_noise,
(ParamExtFloat<px4::params::EKF2_TERR_GRAD>) _param_ekf2_terr_grad,
#endif // CONFIG_EKF2_TERRAIN
#if defined(CONFIG_EKF2_RANGE_FINDER) #if defined(CONFIG_EKF2_RANGE_FINDER)
// range finder fusion // range finder fusion
(ParamExtInt<px4::params::EKF2_RNG_CTRL>) _param_ekf2_rng_ctrl, ///< range finder control selection (ParamExtInt<px4::params::EKF2_RNG_CTRL>) _param_ekf2_rng_ctrl,
(ParamExtFloat<px4::params::EKF2_RNG_DELAY>) (ParamExtFloat<px4::params::EKF2_RNG_DELAY>) _param_ekf2_rng_delay,
_param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec) (ParamExtFloat<px4::params::EKF2_RNG_NOISE>) _param_ekf2_rng_noise,
(ParamExtFloat<px4::params::EKF2_RNG_NOISE>) (ParamExtFloat<px4::params::EKF2_RNG_SFE>) _param_ekf2_rng_sfe,
_param_ekf2_rng_noise, ///< observation noise for range finder measurements (m) (ParamExtFloat<px4::params::EKF2_RNG_GATE>) _param_ekf2_rng_gate,
(ParamExtFloat<px4::params::EKF2_RNG_SFE>) (ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _param_ekf2_rng_pitch,
_param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m) (ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>) _param_ekf2_rng_a_vmax,
(ParamExtFloat<px4::params::EKF2_RNG_GATE>) (ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>) _param_ekf2_rng_a_hmax,
_param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD) (ParamExtFloat<px4::params::EKF2_RNG_A_IGATE>) _param_ekf2_rng_a_igate,
(ParamExtFloat<px4::params::EKF2_MIN_RNG>) (ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>) _param_ekf2_rng_qlty_t,
_param_ekf2_min_rng, ///< minimum valid value for range when on ground (m) (ParamExtFloat<px4::params::EKF2_RNG_K_GATE>) _param_ekf2_rng_k_gate,
(ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad) (ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x,
(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>) (ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y,
_param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s) (ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z,
(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>)
_param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m)
(ParamExtFloat<px4::params::EKF2_RNG_A_IGATE>)
_param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD)
(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>)
_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<px4::params::EKF2_RNG_K_GATE>)
_param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_RNG_POS_X>)
_param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>)
_param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>)
_param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m)
(ParamExtInt<px4::params::EKF2_TERR_MASK>)
_param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation
(ParamExtFloat<px4::params::EKF2_TERR_NOISE>) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec)
(ParamExtFloat<px4::params::EKF2_TERR_GRAD>)
_param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m)
#endif // CONFIG_EKF2_RANGE_FINDER #endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_EXTERNAL_VISION) #if defined(CONFIG_EKF2_EXTERNAL_VISION)

9
src/modules/ekf2/Kconfig
View File

@ -67,6 +67,7 @@ menuconfig EKF2_OPTICAL_FLOW
depends on MODULES_EKF2 depends on MODULES_EKF2
bool "optical flow fusion support" bool "optical flow fusion support"
default y default y
select EKF2_TERRAIN
depends on EKF2_RANGE_FINDER depends on EKF2_RANGE_FINDER
---help--- ---help---
EKF2 optical flow fusion support. EKF2 optical flow fusion support.
@ -85,6 +86,14 @@ depends on MODULES_EKF2
---help--- ---help---
EKF2 sideslip fusion support. 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 menuconfig USER_EKF2
bool "ekf2 running as userspace module" bool "ekf2 running as userspace module"
default n default n