fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-21 23:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Fix non GPS aiding data reset logic (#418)

Browse Source 
* EKF: Move optical flow specific state reset to helper functions

* EKF: Ensure loss of optical flow aiding is handled correctly

If data is only source of aiding and has been rejected for too long - reset using flow data as a velocity reference.
If flow data is unavailable for too long - declare optical flow use stopped.
Use consistent time periods for all resets

* EKF: Ensure loss of external vision aiding is handled correctly

If data is only source of aiding and has been rejected for too long - reset using data as a position.
Don't reset velocity if there is another source of aiding constraining it.
If data is unavailable for too long, declare external vision use stopped.
Use consistent time periods for all resets.

* EKF: Update parameter documentation

Make the distinction between the no_gps_timeout_max and no_aid_timeout_max parameters clearer

* EKF: make class variable units consistent with documentation

* EKF: Don't reset states when optical flow use commences if using external vision

* EKF: Stop optical flow fusion when on ground if excessive movement is detected.

* EKF: fix terrain estimator vulnerabilities

Reset estimate to sensor value if rejected for 10 seconds
Protect against user motion when on ground.
Fix unnecessary duplication of terrain validity check and separate validity update and reporting.

* EKF: remove unnecessary Info console prints

Optical flow use information can be obtained from the estimator_status.control_mode_flags message

* EKF: fix inaccurate comment

* EKF: remove duplicate calculation from terrain validity accessor function
This commit is contained in:
Paul Riseborough
2018-04-09 18:35:15 +10:00
committed by GitHub
parent ba2b9dfdd9
commit 02055acee2
6 changed files with 172 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/terrain_estimator.cpp
+33 -17
View File
@@ -109,6 +109,9 @@ void Ekf::runTerrainEstimator()
_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
}
}
// Update terrain validity
update_terrain_valid();
}
void Ekf::fuseHagl()
@@ -134,37 +137,50 @@ void Ekf::fuseHagl()
float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
_terr_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
if (_terr_test_ratio <= 1.0f) {
// calculate the Kalman gain
float gain = _terrain_var / _hagl_innov_var;
// correct the state
_terrain_vpos -= gain * _hagl_innov;
// correct the variance
_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
// record last successful fusion event
_time_last_hagl_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_hagl = false;
} else {
_innov_check_fail_status.flags.reject_hagl = true;
if (!_inhibit_gndobs_use) {
if (_terr_test_ratio <= 1.0f) {
// calculate the Kalman gain
float gain = _terrain_var / _hagl_innov_var;
// correct the state
_terrain_vpos -= gain * _hagl_innov;
// correct the variance
_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
// 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;
}
}
// return true if the terrain estimate is valid
// return true if the terrain height estimate is valid
bool Ekf::get_terrain_valid()
{
return _hagl_valid;
}
// determine terrain validity
void Ekf::update_terrain_valid()
{
if (_terrain_initialised && _range_data_continuous && !_control_status.flags.rng_stuck &&
(_time_last_imu - _time_last_hagl_fuse < (uint64_t)5e6)) {
return true;
_hagl_valid = true;
} else {
return false;
_hagl_valid = false;
}
}