fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 06:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

* naming conventions

Browse Source 
* add landed-state for range-finder-cons-check
* ajdust unit-tests
This commit is contained in:
Marco Hauswirth
2025-08-26 15:08:55 +02:00
parent 85b116f14e
commit 58f63909ca
9 changed files with 64 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
+30 -11
View File
@@ -59,7 +59,7 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
{
const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;
if (dt > 1.f) {
if (dt > _kTestRatioLpfTimeConstant || _landed) {
_time_last_update_us = time_us;
_initialized = false;
return;
@@ -108,8 +108,7 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
} else if (measurement_idx == 1) {
_innov = y;
const float test_ratio = fminf(sq(y) / (sq(_gate) * S), 4.f); // limit to 4 to limit sensitivity to outliers
_test_ratio_lpf.setParameters(dt, _t_to_init);
_test_ratio_lpf.update(sign(_innov) * test_ratio);
_test_ratio_lpf.update(test_ratio, dt);
}
// update step
@@ -129,21 +128,21 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, const float vz_var)
{
// start the consistency check after 1s
if (_t_since_first_sample > _t_to_init) {
// let the filter settle before starting the consistency check
if (_t_since_first_sample > _kTestRatioLpfTimeConstant) {
if (fabsf(_test_ratio_lpf.getState()) < 1.f) {
const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f);
if (!horizontal_motion && vertical_motion) {
_state = KinematicState::CONSISTENT;
if (!_horizontal_motion && vertical_motion) {
_state = KinematicState::kConsistent;
} else if (_state == KinematicState::CONSISTENT || vertical_motion) {
_state = KinematicState::UNKNOWN;
} else if (_state == KinematicState::kConsistent || vertical_motion) {
_state = KinematicState::kUnknown;
}
} else {
_t_since_first_sample = 0.f;
_state = KinematicState::INCONSISTENT;
_state = KinematicState::kInconsistent;
}
} else {
@@ -152,7 +151,9 @@ void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz,
}
void RangeFinderConsistencyCheck::run(const float z, const float z_var, const float vz, const float vz_var,
const float dist_bottom, const float dist_bottom_var, const uint64_t time_us)
const float dist_bottom, const float dist_bottom_var, const uint64_t time_us,
const uint8_t current_posD_reset_count
)
{
if (!_initialized || current_posD_reset_count != _last_posD_reset_count) {
_last_posD_reset_count = current_posD_reset_count;
@@ -161,3 +162,21 @@ void RangeFinderConsistencyCheck::run(const float z, const float z_var, const fl
update(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us);
}
void RangeFinderConsistencyCheck::setIsLanded(const bool landed)
{
if (landed) {
_state = KinematicState::kUnknown;
}
_landed = landed;
}
void RangeFinderConsistencyCheck::reset()
{
if (_initialized && _state == KinematicState::kConsistent) {
_state = KinematicState::kUnknown;
}
_initialized = false;
}
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
+16 -21
View File
@@ -49,31 +49,25 @@ public:
~RangeFinderConsistencyCheck() = default;
enum class KinematicState : int {
INCONSISTENT = 0,
CONSISTENT = 1,
UNKNOWN = 2
kInconsistent = 0,
kConsistent = 1,
kUnknown = 2
};
float getTestRatioLpf() const { return _initialized ? _test_ratio_lpf.getState() : 0.f; }
float getInnov() const { return _initialized ? _innov : 0.f; }
float getInnovVar() const { return _initialized ? _innov_var : 0.f; }
bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT && _t_since_first_sample > _t_to_init; }
bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT && _t_since_first_sample > _t_to_init; }
bool isKinematicallyConsistent() const { return _state == KinematicState::kConsistent && _t_since_first_sample > _kTestRatioLpfTimeConstant; }
bool isNotKinematicallyInconsistent() const { return _state != KinematicState::kInconsistent && (_t_since_first_sample > _kTestRatioLpfTimeConstant || _landed); }
void setGate(const float gate) { _gate = gate; }
void run(float z, float z_var, float vz, float vz_var,
float dist_bottom, float dist_bottom_var, uint64_t time_us);
void run(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var,
uint64_t time_u, uint8_t current_posD_reset_coun);
void set_terrain_process_noise(float terrain_process_noise) { _terrain_process_noise = terrain_process_noise; }
void reset()
{
if (_initialized && _state == KinematicState::CONSISTENT) {
_state = KinematicState::UNKNOWN;
}
void reset();
_initialized = false;
}
uint8_t current_posD_reset_count{0};
bool horizontal_motion{false};
void setHorizontalMotion(const bool horizontal_motion) { _horizontal_motion = horizontal_motion; }
bool getHorizontalMotion() const { return _horizontal_motion; }
void setIsLanded(bool landed);
private:
@@ -89,13 +83,14 @@ private:
float _innov{0.f};
float _innov_var{0.f};
uint64_t _time_last_update_us{0};
static constexpr float time_constant{1.f};
AlphaFilter<float> _test_ratio_lpf{time_constant};
static constexpr float _kTestRatioLpfTimeConstant{1.f};
AlphaFilter<float> _test_ratio_lpf{_kTestRatioLpfTimeConstant};
float _gate{1.0f};
KinematicState _state{KinematicState::UNKNOWN};
KinematicState _state{KinematicState::kUnknown};
float _t_since_first_sample{0.f};
uint8_t _last_posD_reset_count{0};
static constexpr float _t_to_init{1.f};
bool _horizontal_motion{false};
bool _landed{false};
};
namespace RangeFilter
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+12 -14
View File
@@ -53,10 +53,10 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
if (_range_sensor.isDataReady()) {
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
_range_sensor.setMaxFogDistance(_params.rng_fog);
_range_sensor.setQualityHysteresis(_params.ekf2_rng_qlty_t);
_range_sensor.setMaxFogDistance(_params.ekf2_rng_fog);
_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
@@ -66,24 +66,22 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
const float dist_var = getRngVar();
_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.5f);
const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f);
if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) {
if (!updated_horizontal_motion && _rng_consistency_check.getHorizontalMotion()) {
_rng_consistency_check.reset();
}
_rng_consistency_check.horizontal_motion = updated_horizontal_motion;
_rng_consistency_check.setIsLanded(!_control_status.flags.in_air);
_rng_consistency_check.setHorizontalMotion(updated_horizontal_motion);
const float z_var = P(State::pos.idx + 2, State::pos.idx + 2);
const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2);
_rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(),
dist_var, imu_sample.time_us);
dist_var, imu_sample.time_us, get_posD_reset_count());
} else if (_range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) {
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setRange(_params.ekf2_min_rng);
_range_sensor.setValidity(true);
} else {
@@ -220,7 +218,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
} else {
if (starting_conditions_passing) {
if (_control_status.flags.opt_flow_terrain) {
if (!aid_src.innovation_rejected) {
if (!aid_src.innovation_rejected && _rng_consistency_check.isNotKinematicallyInconsistent()) {
_control_status.flags.rng_terrain = true;
fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
}
@@ -273,8 +271,8 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
float Ekf::getRngVar() const
{
const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
const float dist_var = sq(_params.range_noise) + dist_dependant_var;
const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom());
const float dist_var = sq(_params.ekf2_rng_noise) + dist_dependant_var;
return dist_var;
}
src/modules/ekf2/EKF/common.h
+1 -1
View File
@@ -426,7 +426,7 @@ struct parameters {
float ekf2_rng_qlty_t{1.0f}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
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{0.5f}; ///< gate size used by the range finder kinematic consistency check
float rng_fog{0.f}; ///< max distance which a blocked range sensor measures (fog, dirt) [m]
float ekf2_rng_fog{0.f}; ///< max distance which a blocked range sensor measures (fog, dirt) [m]
Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m)
#endif // CONFIG_EKF2_RANGE_FINDER
src/modules/ekf2/EKF2.cpp
+1 -1
View File
@@ -163,7 +163,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_rng_a_vmax(_params->ekf2_rng_a_vmax),
_param_ekf2_rng_a_hmax(_params->ekf2_rng_a_hmax),
_param_ekf2_rng_qlty_t(_params->ekf2_rng_qlty_t),
_param_ekf2_rng_k_gate(_params->ekf2_rng_k_gate),
_param_range_kin_consistency_gate(_params->range_kin_consistency_gate),
_param_ekf2_rng_fog(_params->ekf2_rng_fog),
_param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
_param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
src/modules/ekf2/EKF2.hpp
+1 -1
View File
@@ -622,7 +622,7 @@ private:
(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>) _param_ekf2_rng_a_vmax,
(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>) _param_ekf2_rng_a_hmax,
(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>) _param_ekf2_rng_qlty_t,
(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>) _param_ekf2_rng_k_gate,
(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>) _param_range_kin_consistency_gate,
(ParamExtFloat<px4::params::EKF2_RNG_FOG>) _param_ekf2_rng_fog,
(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x,
(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y,
src/modules/ekf2/test/test_EKF_accelerometer.cpp
-3
View File
@@ -197,9 +197,6 @@ TEST_F(EkfAccelerometerTest, imuFallingDetectionBaroRange)
const float bias = CONSTANTS_ONE_G;
_sensor_simulator._imu.setAccelData(Vector3f(0.f, 0.f, -CONSTANTS_ONE_G + bias));
_sensor_simulator.runSeconds(2);
_sensor_simulator._imu.setAccelClipping(false, false, true);
_sensor_simulator.runSeconds(2);
// THEN: the bad vertical is detected because both sources agree
EXPECT_TRUE(_ekf->fault_status_flags().bad_acc_vertical);
}
src/modules/ekf2/test/test_EKF_height_fusion.cpp
+2 -2
View File
@@ -155,7 +155,7 @@ TEST_F(EkfHeightFusionTest, gpsRef)
_ekf_wrapper.enableBaroHeightFusion();
_ekf_wrapper.enableGpsHeightFusion();
_ekf_wrapper.enableRangeHeightFusion();
_sensor_simulator.runSeconds(10);
_sensor_simulator.runSeconds(2);
EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -235,7 +235,7 @@ TEST_F(EkfHeightFusionTest, baroRefFailOver)
_ekf_wrapper.enableGpsHeightFusion();
_ekf_wrapper.enableRangeHeightFusion();
_sensor_simulator.runSeconds(10);
_sensor_simulator.runSeconds(0.1);
EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO);
src/modules/ekf2/test/test_EKF_terrain.cpp
+1 -1
View File
@@ -207,7 +207,7 @@ TEST_F(EkfTerrainTest, testHeightReset)
const float new_baro_height = _sensor_simulator._baro.getData() + 50.f;
_sensor_simulator._baro.setData(new_baro_height);
_sensor_simulator.stopGps(); // prevent from switching to GNSS height
_sensor_simulator.runSeconds(100);
_sensor_simulator.runSeconds(10);
// THEN: a height reset occurred and the estimated distance to the ground remains constant
reset_logging_checker.capturePostResetState();