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

WIP: migrate range height control to use terrain state

This commit is contained in:
bresch 2023-11-24 14:57:53 +01:00
parent 35f1aa9e43
commit be15d3d898
6 changed files with 7 additions and 189 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -423,10 +423,8 @@ struct parameters {
float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec)
float range_noise{0.1f}; ///< observation noise for range finder measurements (m)
float range_innov_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD)
float rng_hgt_bias_nsd{0.13f}; ///< process noise for range height bias estimation (m/s/sqrt(Hz))
float rng_sens_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
float range_noise_scaler{0.0f}; ///< scaling from range measurement to noise (m/m)
const float vehicle_variance_scaler{0.0f}; ///< gain applied to vehicle height variance used in calculation of height above ground observation variance
float max_hagl_for_range_aid{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1)
float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1)
float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion

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

@ -837,10 +837,7 @@ private:
void runTerrainEstimator(const imuSample &imu_delayed);
void predictHagl(const imuSample &imu_delayed);
float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
void controlHaglFakeFusion();
void terrainHandleVerticalPositionReset(float delta_z);
# if defined(CONFIG_EKF2_RANGE_FINDER)
// update the terrain vertical position estimate using a height above ground measurement from the range finder

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

@ -207,13 +207,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
#if defined(CONFIG_EKF2_GNSS)
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_RANGE_FINDER)
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_TERRAIN)
terrainHandleVerticalPositionReset(delta_z);
#endif
#if defined(CONFIG_EKF2_RANGE_FINDER) || defined(CONFIG_EKF2_OPTICAL_FLOW)
_state.terrain_vpos += delta_z;
#endif // CONFIG_EKF2_RANGE_FINDER || CONFIG_EKF2_OPTICAL_FLOW
// Reset the timout timer
_time_last_hgt_fuse = _time_delayed_us;

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

@ -130,7 +130,7 @@ bool Ekf::checkHaglYawResetReq() const
// Check if height has increased sufficiently to be away from ground magnetic anomalies
// and request a yaw reset if not already requested.
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 = (getTerrainVertPos() - _state.pos(2)) > mag_anomalies_max_hagl;
return above_mag_anomalies;
}
#endif // CONFIG_EKF2_TERRAIN

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

@ -112,17 +112,8 @@ void Ekf::controlRangeHeightFusion()
innov_gate,
aid_src);
// update the bias estimator before updating the main filter but after
// using its current state to compute the vertical position innovation
if (measurement_valid && _range_sensor.isDataHealthy()) {
bias_est.setMaxStateNoise(sqrtf(measurement_var));
bias_est.setProcessNoiseSpectralDensity(_params.rng_hgt_bias_nsd);
bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
}
// determine if we should use height aiding
const bool do_conditional_range_aid = (_params.rng_ctrl == RngCtrl::CONDITIONAL) && isConditionalRangeAidSuitable();
const bool continuing_conditions_passing = ((_params.rng_ctrl == RngCtrl::ENABLED) || do_conditional_range_aid)
const bool continuing_conditions_passing = (_params.rng_ctrl == RngCtrl::ENABLED);
&& measurement_valid
&& _range_sensor.isDataHealthy();
@ -142,8 +133,8 @@ void Ekf::controlRangeHeightFusion()
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
_information_events.flags.reset_hgt_to_rng = true;
resetVerticalPositionTo(-(measurement - bias_est.getBias()));
bias_est.setBias(_state.pos(2) + measurement);
resetVerticalPositionTo(-(measurement - _state.terrain_vpos));
_state.terrain_vpos = _state.pos(2) + measurement;
// reset vertical velocity
resetVerticalVelocityToZero();
@ -200,47 +191,6 @@ void Ekf::controlRangeHeightFusion()
}
}
bool Ekf::isConditionalRangeAidSuitable()
{
#if defined(CONFIG_EKF2_TERRAIN)
if (_control_status.flags.in_air
&& _range_sensor.isHealthy()
&& isTerrainEstimateValid()) {
// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
float range_hagl_max = _params.max_hagl_for_range_aid;
float max_vel_xy = _params.max_vel_for_range_aid;
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);
if (!_control_status.flags.rng_hgt) {
range_hagl_max = 0.7f * _params.max_hagl_for_range_aid;
max_vel_xy = 0.7f * _params.max_vel_for_range_aid;
is_hagl_stable = (hagl_test_ratio < 0.01f);
}
const float range_hagl = _terrain_vpos - _state.pos(2);
const bool is_in_range = (range_hagl < range_hagl_max);
bool is_below_max_speed = true;
if (isHorizontalAidingActive()) {
is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy);
}
return is_in_range && is_hagl_stable && is_below_max_speed;
}
#endif // CONFIG_EKF2_TERRAIN
return false;
}
void Ekf::stopRngHgtFusion()
{
if (_control_status.flags.rng_hgt) {

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

@ -45,11 +45,6 @@
void Ekf::initHagl()
{
// assume a ground clearance
_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
// use the ground clearance value as our uncertainty
_terrain_var = sq(_params.rng_gnd_clearance);
#if defined(CONFIG_EKF2_RANGE_FINDER)
_aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us;
@ -73,38 +68,6 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
// as an overestimated distance leads to an overestimated velocity, causing a dangerous behavior.
initHagl();
}
predictHagl(imu_delayed);
#if defined(CONFIG_EKF2_RANGE_FINDER)
controlHaglRngFusion();
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
controlHaglFlowFusion();
#endif // CONFIG_EKF2_OPTICAL_FLOW
controlHaglFakeFusion();
// constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) {
_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
}
}
void Ekf::predictHagl(const imuSample &imu_delayed)
{
// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle
// process noise due to errors in vehicle height estimate
_terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise);
// process noise due to terrain gradient
_terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient)
* (sq(_state.vel(0)) + sq(_state.vel(1)));
// limit the variance to prevent it becoming badly conditioned
_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
}
#if defined(CONFIG_EKF2_RANGE_FINDER)
@ -183,13 +146,6 @@ void Ekf::controlHaglRngFusion()
}
}
float Ekf::getRngVar() const
{
return fmaxf(P(State::pos.idx + 2, State::pos.idx + 2) * _params.vehicle_variance_scaler, 0.0f)
+ sq(_params.range_noise)
+ sq(_params.range_noise_scaler * _range_sensor.getRange());
}
void Ekf::resetHaglRng()
{
_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
@ -343,81 +299,6 @@ void Ekf::resetHaglFlow()
_aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us;
}
void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow)
{
flow.fused = true;
const float R_LOS = flow.observation_variance[0];
// calculate the height above the ground of the optical flow camera. Since earth frame is NED
// a positive offset in earth frame leads to a smaller height above the ground.
float range = predictFlowRange();
const float state = _terrain_vpos; // linearize both axes using the same state value
Vector2f innov_var;
float H;
sym::TerrEstComputeFlowXyInnovVarAndHx(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &innov_var, &H);
innov_var.copyTo(flow.innovation_variance);
if ((flow.innovation_variance[0] < R_LOS)
|| (flow.innovation_variance[1] < R_LOS)) {
// we need to reinitialise the covariance matrix and abort this fusion step
ECL_ERR("Opt flow error - covariance reset");
_terrain_var = 100.0f;
return;
}
// run the innovation consistency check and record result
setEstimatorAidStatusTestRatio(flow, math::max(_params.flow_innov_gate, 1.f));
_innov_check_fail_status.flags.reject_optflow_X = (flow.test_ratio[0] > 1.f);
_innov_check_fail_status.flags.reject_optflow_Y = (flow.test_ratio[1] > 1.f);
// if either axis fails we abort the fusion
if (flow.innovation_rejected) {
return;
}
// fuse observation axes sequentially
for (uint8_t index = 0; index <= 1; index++) {
if (index == 0) {
// everything was already computed above
} else if (index == 1) {
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
sym::TerrEstComputeFlowYInnovVarAndH(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &flow.innovation_variance[1], &H);
// recalculate the innovation using the updated state
const Vector2f vel_body = predictFlowVelBody();
range = predictFlowRange();
flow.innovation[1] = (-vel_body(0) / range) - flow.observation[1];
if (flow.innovation_variance[1] < R_LOS) {
// we need to reinitialise the covariance matrix and abort this fusion step
ECL_ERR("Opt flow error - covariance reset");
_terrain_var = 100.0f;
return;
}
}
float Kfusion = _terrain_var * H / flow.innovation_variance[index];
_terrain_vpos += Kfusion * flow.innovation[0];
// constrain terrain to minimum allowed value and predict height above ground
_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
// guard against negative variance
_terrain_var = fmaxf(_terrain_var - Kfusion * H * _terrain_var, sq(0.01f));
}
_fault_status.flags.bad_optflow_X = false;
_fault_status.flags.bad_optflow_Y = false;
flow.time_last_fuse = _time_delayed_us;
flow.fused = true;
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
void Ekf::controlHaglFakeFusion()
{
if (!_control_status.flags.in_air
@ -467,7 +348,3 @@ bool Ekf::isTerrainEstimateValid() const
return valid;
}
void Ekf::terrainHandleVerticalPositionReset(const float delta_z) {
_terrain_vpos += delta_z;
}