From b38458c2ab4487e918eebdf1416bb7e08f52276c Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 10 Oct 2019 16:36:48 +0200 Subject: [PATCH] Terrain estimator: formatting and remove redundant comments --- EKF/control.cpp | 12 ++++---- EKF/ekf.h | 15 ++++------ EKF/ekf_helper.cpp | 2 +- EKF/estimator_interface.h | 12 +++----- EKF/terrain_estimator.cpp | 59 ++++++++++++++++----------------------- 5 files changed, 40 insertions(+), 60 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index d9d9bfd43a..1090fb606a 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -502,7 +502,7 @@ void Ekf::controlOpticalFlowFusion() && !_control_status.flags.opt_flow // we are not yet using flow data && _control_status.flags.tilt_align // we know our tilt attitude && !_inhibit_flow_use - && get_terrain_valid()) // we have a valid distance to ground estimate + && isTerrainEstimateValid()) { // If the heading is not aligned, reset the yaw and magnetic field states if (!_control_status.flags.yaw_align) { @@ -1023,7 +1023,7 @@ void Ekf::controlHeightFusion() // we have just switched to using range finder, calculate height sensor offset such that current // measurement matches our current height estimate if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { - if (get_terrain_valid()) { + if (isTerrainEstimateValid()) { _hgt_sensor_offset = _terrain_vpos; } else { @@ -1070,7 +1070,7 @@ void Ekf::controlHeightFusion() // measurement matches our current height estimate if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { // use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar) - if (_control_status.flags.in_air && get_terrain_valid()) { + if (_control_status.flags.in_air && isTerrainEstimateValid()) { _hgt_sensor_offset = _terrain_vpos; @@ -1105,7 +1105,7 @@ void Ekf::controlHeightFusion() // we have just switched to using range finder, calculate height sensor offset such that current // measurement matches our current height estimate if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { - if (get_terrain_valid()) { + if (isTerrainEstimateValid()) { _hgt_sensor_offset = _terrain_vpos; } else { @@ -1186,7 +1186,7 @@ void Ekf::checkRangeAidSuitability() if (_control_status.flags.in_air && !_rng_hgt_faulty - && get_terrain_valid() + && isTerrainEstimateValid() && horz_vel_valid) { // 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 @@ -1417,7 +1417,7 @@ void Ekf::controlMagFusion() if (!_control_status.flags.mag_align_complete && _control_status.flags.in_air) { // Check if height has increased sufficiently to be away from ground magnetic anomalies // and request a yaw reset if not already requested. - float terrain_vpos_estimate = get_terrain_valid() ? _terrain_vpos : _last_on_ground_posD; + float terrain_vpos_estimate = isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; _mag_yaw_reset_req |= (terrain_vpos_estimate - _state.pos(2)) > 1.5f; } diff --git a/EKF/ekf.h b/EKF/ekf.h index 7f6da0fba5..8d974f2188 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -104,11 +104,8 @@ public: // gets the innovation of the drag specific force measurement void get_drag_innov(float drag_innov[2]); - // gets the innovation variance of the HAGL measurement - void get_hagl_innov_var(float *hagl_innov_var); - - // gets the innovation of the HAGL measurement - void get_hagl_innov(float *hagl_innov); + void getHaglInnovVar(float *hagl_innov_var); + void getHaglInnov(float *hagl_innov); // get the state vector at the delayed time horizon void get_state_delayed(float *state); @@ -197,14 +194,12 @@ public: // check if the EKF is dead reckoning horizontal velocity using inertial data only void update_deadreckoning_status(); - // return true if the terrain estimate is valid - bool get_terrain_valid(); + bool isTerrainEstimateValid(); - // update terrain validity status - void update_terrain_valid(); + void updateTerrainValidity(); // get the estimated terrain vertical position relative to the NED origin - void get_terrain_vert_pos(float *ret); + void getTerrainVertPos(float *ret); // get the terrain variance float get_terrain_var() const { return _terrain_var; } diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index eafd8b767a..6a77075e4f 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1213,7 +1213,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0); soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert; - soln_status.flags.pos_vert_agl = get_terrain_valid(); + soln_status.flags.pos_vert_agl = isTerrainEstimateValid(); soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz; soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel; soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 6139a2ebfa..6a599e156c 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -122,11 +122,8 @@ public: // gets the innovation of the drag specific force measurement virtual void get_drag_innov(float drag_innov[2]) = 0; - // gets the innovation variance of the HAGL measurement - virtual void get_hagl_innov_var(float *hagl_innov_var) = 0; - - // gets the innovation of the HAGL measurement - virtual void get_hagl_innov(float *hagl_innov) = 0; + virtual void getHaglInnovVar(float *hagl_innov_var) = 0; + virtual void getHaglInnov(float *hagl_innov) = 0; // return an array containing the output predictor angular, velocity and position tracking // error magnitudes (rad), (m/s), (m) @@ -268,11 +265,10 @@ public: // return true if the EKF is dead reckoning the position using inertial data only bool inertial_dead_reckoning() {return _is_dead_reckoning;} - // return true if the terrain estimate is valid - virtual bool get_terrain_valid() = 0; + virtual bool isTerrainEstimateValid() = 0; // get the estimated terrain vertical position relative to the NED origin - virtual void get_terrain_vert_pos(float *ret) = 0; + virtual void getTerrainVertPos(float *ret) = 0; // return true if the local position estimate is valid bool local_position_is_valid(); diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index abb65fc889..bab7b8b674 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -103,8 +103,8 @@ void Ekf::runTerrainEstimator() _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise); // process noise due to terrain gradient - _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel( - 1))); + _terrain_var += sq(_imu_sample_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); @@ -117,7 +117,6 @@ void Ekf::runTerrainEstimator() // we do this here to avoid doing those calculations at a high rate _sin_tilt_rng = sinf(_params.rng_sens_pitch); _cos_tilt_rng = cosf(_params.rng_sens_pitch); - } if (_flow_for_terrain_data_ready) { @@ -131,8 +130,7 @@ void Ekf::runTerrainEstimator() } } - // Update terrain validity - update_terrain_valid(); + updateTerrainValidity(); } void Ekf::fuseHagl() @@ -149,7 +147,9 @@ void Ekf::fuseHagl() _hagl_innov = pred_hagl - meas_hagl; // calculate the observation variance adding the variance of the vehicles own height uncertainty - float obs_variance = fmaxf(P[9][9] * _params.vehicle_variance_scaler, 0.0f) + sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng); + float obs_variance = fmaxf(P[9][9] * _params.vehicle_variance_scaler, 0.0f) + + sq(_params.range_noise) + + sq(_params.range_noise_scaler * _range_sample_delayed.rng); // calculate the innovation variance - limiting it to prevent a badly conditioned fusion _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); @@ -168,18 +168,20 @@ void Ekf::fuseHagl() // record last successful fusion event _time_last_hagl_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_hagl = false; + } else { // If we have been rejecting range data for too long, reset to measurement if ((_time_last_imu - _time_last_hagl_fuse) > (uint64_t)10E6) { _terrain_vpos = _state.pos(2) + meas_hagl; _terrain_var = obs_variance; + } else { _innov_check_fail_status.flags.reject_hagl = true; } } + } else { _innov_check_fail_status.flags.reject_hagl = true; - return; } } @@ -218,14 +220,14 @@ void Ekf::fuseFlowForTerrain() // rotate into body frame Vector3f vel_body = earth_to_body * vel_rel_earth; - float t0 = q0*q0 - q1*q1 - q2*q2 + q3*q3; + float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; // constrain terrain to minimum allowed value and predict height above ground _terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2)); float pred_hagl = _terrain_vpos - _state.pos(2); // Calculate observation matrix for flow around the vehicle x axis - float Hx = vel_body(1) * t0 /(pred_hagl * pred_hagl); + float Hx = vel_body(1) * t0 / (pred_hagl * pred_hagl); // Constrain terrain variance to be non-negative _terrain_var = fmaxf(_terrain_var, 0.0f); @@ -237,7 +239,7 @@ void Ekf::fuseFlowForTerrain() float Kx = _terrain_var * Hx / _flow_innov_var[0]; // calculate prediced optical flow about x axis - float pred_flow_x = vel_body(1) * earth_to_body(2,2) / pred_hagl; + float pred_flow_x = vel_body(1) * earth_to_body(2, 2) / pred_hagl; // calculate flow innovation (x axis) _flow_innov[0] = pred_flow_x - opt_flow_rate(0); @@ -258,7 +260,7 @@ void Ekf::fuseFlowForTerrain() } // Calculate observation matrix for flow around the vehicle y axis - float Hy = -vel_body(0) * t0 /(pred_hagl * pred_hagl); + float Hy = -vel_body(0) * t0 / (pred_hagl * pred_hagl); // Calculuate innovation variance _flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS; @@ -267,7 +269,7 @@ void Ekf::fuseFlowForTerrain() float Ky = _terrain_var * Hy / _flow_innov_var[1]; // calculate prediced optical flow about y axis - float pred_flow_y = -vel_body(0) * earth_to_body(2,2) / pred_hagl; + float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) / pred_hagl; // calculate flow innovation (y axis) _flow_innov[1] = pred_flow_y - opt_flow_rate(1); @@ -286,45 +288,37 @@ void Ekf::fuseFlowForTerrain() } } -// return true if the terrain height estimate is valid -bool Ekf::get_terrain_valid() +bool Ekf::isTerrainEstimateValid() { return _hagl_valid; } -// determine terrain validity -void Ekf::update_terrain_valid() +void Ekf::updateTerrainValidity() { // we have been fusing range finder measurements in the last 5 seconds - bool recent_range_fusion = (_time_last_imu - _time_last_hagl_fuse) < 5*1000*1000; + bool recent_range_fusion = (_time_last_imu - _time_last_hagl_fuse) < (uint64_t)5e6; // 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 - bool recent_flow_for_terrain_fusion = ((_time_last_imu - _time_last_of_fuse) < 5*1000*1000) && !_control_status.flags.opt_flow; + bool recent_flow_for_terrain_fusion = ((_time_last_imu - _time_last_of_fuse) < (uint64_t)5e6) + && !_control_status.flags.opt_flow; - - if (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion)) { - - _hagl_valid = true; - - } else { - _hagl_valid = false; - } + _hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion)); } // get the estimated vertical position of the terrain relative to the NED origin -void Ekf::get_terrain_vert_pos(float *ret) +void Ekf::getTerrainVertPos(float *ret) { memcpy(ret, &_terrain_vpos, sizeof(float)); } -void Ekf::get_hagl_innov(float *hagl_innov) +void Ekf::getHaglInnov(float *hagl_innov) { memcpy(hagl_innov, &_hagl_innov, sizeof(_hagl_innov)); } -void Ekf::get_hagl_innov_var(float *hagl_innov_var) +void Ekf::getHaglInnovVar(float *hagl_innov_var) { memcpy(hagl_innov_var, &_hagl_innov_var, sizeof(_hagl_innov_var)); } @@ -342,10 +336,5 @@ void Ekf::checkRangeDataContinuity() _dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 4e6f); - if (_dt_last_range_update_filt_us < 2e6f) { - _range_data_continuous = true; - - } else { - _range_data_continuous = false; - } + _range_data_continuous = (_dt_last_range_update_filt_us < 2e6f); }