fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 22:04:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Terrain estimator: formatting and remove redundant comments

Browse Source
This commit is contained in:
bresch 2019-10-10 16:36:48 +02:00 committed by Paul Riseborough
parent 0aef0eda59
commit b38458c2ab
5 changed files with 40 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
EKF/control.cpp
View File

@ -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;
}

15
EKF/ekf.h
View File

@ -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; }

2
EKF/ekf_helper.cpp
View File

@ -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;

12
EKF/estimator_interface.h
View File

@ -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();

59
EKF/terrain_estimator.cpp
View File

@ -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);
}