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

ekf2_terr: refactor terrain estimator - rng aiding

Browse Source
This commit is contained in:
bresch 2022-02-07 17:25:20 +01:00 committed by Daniel Agar
parent 5818974f0f
commit 33fd1849e0
3 changed files with 185 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF/ekf.cpp
View File

@ -209,8 +209,8 @@ bool Ekf::initialiseFilter()
increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
}
// try to initialise the terrain estimator
_terrain_initialised = initHagl();
// Initialise the terrain estimator
initHagl();
// reset the essential fusion timeout counters
_time_last_hgt_fuse = _time_last_imu;

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

@ -386,6 +386,7 @@ private:
uint64_t _time_last_fake_pos_fuse{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec)
uint64_t _time_last_gps_yaw_fuse{0}; ///< time the last fusion of GPS yaw measurements were performed (uSec)
uint64_t _time_last_gps_yaw_data{0}; ///< time the last GPS yaw measurement was available (uSec)
uint64_t _time_last_healthy_rng_data{0};
uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
Vector2f _last_known_posNE{}; ///< last known local NE position vector (m)
@ -535,8 +536,6 @@ private:
float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2)
uint8_t _terrain_vpos_reset_counter{0}; ///< number of times _terrain_vpos has been reset
uint64_t _time_last_hagl_fuse{0}; ///< last system time that a range sample was fused by the terrain estimator
uint64_t _time_last_fake_hagl_fuse{0}; ///< last system time that a fake range sample was fused by the terrain estimator
bool _terrain_initialised{false}; ///< true when the terrain estimator has been initialized
bool _hagl_valid{false}; ///< true when the height above ground estimate is valid
terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
@ -679,20 +678,28 @@ private:
bool calcOptFlowBodyRateComp();
// initialise the terrain vertical position estimator
// return true if the initialisation is successful
bool initHagl();
void initHagl();
bool shouldUseRangeFinderForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder); }
bool shouldUseOpticalFlowForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow); }
// run the terrain estimator
void runTerrainEstimator();
void predictHagl();
// update the terrain vertical position estimate using a height above ground measurement from the range finder
void fuseHagl();
void controlHaglRngFusion();
void fuseHaglRng();
void startHaglRngFusion();
void resetHaglRngIfNeeded();
void resetHaglRng();
void stopHaglRngFusion();
float getRngVar();
// update the terrain vertical position estimate using an optical flow measurement
void fuseFlowForTerrain();
void resetHaglFlow();
void controlHaglFakeFusion();
void resetHaglFake();
// reset the heading and magnetic field states using the declination and magnetometer measurements
// return true if successful

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

@ -43,44 +43,9 @@
#include <mathlib/mathlib.h>
bool Ekf::initHagl()
void Ekf::initHagl()
{
bool initialized = false;
if (!_control_status.flags.in_air) {
// if on ground, do not trust the range sensor, but 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);
_time_last_fake_hagl_fuse = _time_last_imu;
initialized = true;
} else if (shouldUseRangeFinderForHagl()
&& _range_sensor.isDataHealthy()) {
// if we have a fresh measurement, use it to initialise the terrain estimator
_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
// initialise state variance to variance of measurement
_terrain_var = sq(_params.range_noise);
// success
initialized = true;
} else if (shouldUseOpticalFlowForHagl()
&& _flow_for_terrain_data_ready) {
// initialise terrain vertical position to origin as this is the best guess we have
_terrain_vpos = fmaxf(0.0f, _state.pos(2));
_terrain_var = 100.0f;
initialized = true;
} else {
// no information - cannot initialise
}
if (initialized) {
// has initialized with valid data
_time_last_hagl_fuse = _time_last_imu;
}
return initialized;
resetHaglFake();
}
void Ekf::runTerrainEstimator()
@ -90,47 +55,154 @@ void Ekf::runTerrainEstimator()
_last_on_ground_posD = _state.pos(2);
}
// Perform initialisation check and
// on ground, continuously reset the terrain estimator
if (!_terrain_initialised || !_control_status.flags.in_air) {
_terrain_initialised = initHagl();
predictHagl();
} else {
// Fuse range finder data if available
controlHaglRngFusion();
// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle
if (shouldUseOpticalFlowForHagl()
&& _flow_for_terrain_data_ready) {
fuseFlowForTerrain();
_flow_for_terrain_data_ready = false;
}
// process noise due to errors in vehicle height estimate
_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);
controlHaglFakeFusion();
// 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)));
// limit the variance to prevent it becoming badly conditioned
_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
// Fuse range finder data if available
if (shouldUseRangeFinderForHagl()
&& _range_sensor.isDataHealthy()) {
fuseHagl();
}
if (shouldUseOpticalFlowForHagl()
&& _flow_for_terrain_data_ready) {
fuseFlowForTerrain();
_flow_for_terrain_data_ready = false;
}
// 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);
}
// 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);
}
updateTerrainValidity();
}
void Ekf::fuseHagl()
void Ekf::predictHagl()
{
// 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_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)));
// limit the variance to prevent it becoming badly conditioned
_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
}
void Ekf::controlHaglRngFusion()
{
if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder)
|| _control_status.flags.rng_fault) {
stopHaglRngFusion();
return;
}
if (_range_sensor.isDataHealthy()) {
const bool continuing_conditions_passing = _control_status.flags.in_air;
//const bool continuing_conditions_passing = _control_status.flags.in_air && !_control_status.flags.rng_hgt; // TODO: should not be fused when using range height
const bool starting_conditions_passing = continuing_conditions_passing && _range_sensor.isRegularlySendingData();
_time_last_healthy_rng_data = _time_last_imu;
if (_hagl_sensor_status.flags.range_finder) {
if (continuing_conditions_passing) {
fuseHaglRng();
// We have been rejecting range data for too long
const uint64_t timeout = static_cast<uint64_t>(_params.terrain_timeout * 1e6f);
const bool is_fusion_failing = isTimedOut(_time_last_hagl_fuse, timeout);
if (is_fusion_failing) {
if (_range_sensor.getDistBottom() > 2.f * _params.rng_gnd_clearance) {
// Data seems good, attempt a reset
resetHaglRng();
} else if (starting_conditions_passing) {
// The sensor can probably not detect the ground properly
// declare the sensor faulty and stop the fusion
_control_status.flags.rng_fault = true;
_range_sensor.setFaulty();
stopHaglRngFusion();
} else {
// This could be a temporary issue, stop the fusion without declaring the sensor faulty
stopHaglRngFusion();
}
}
} else {
stopHaglRngFusion();
}
} else {
if (starting_conditions_passing) {
startHaglRngFusion();
}
}
} else if (_hagl_sensor_status.flags.range_finder && isTimedOut(_time_last_healthy_rng_data, _params.reset_timeout_max)) {
// No data anymore. Stop until it comes back.
stopHaglRngFusion();
}
}
void Ekf::startHaglRngFusion()
{
_hagl_sensor_status.flags.range_finder = true;
resetHaglRngIfNeeded();
}
void Ekf::resetHaglRngIfNeeded()
{
if (_hagl_sensor_status.flags.flow) {
const float meas_hagl = _range_sensor.getDistBottom();
const float pred_hagl = _terrain_vpos - _state.pos(2);
const float hagl_innov = pred_hagl - meas_hagl;
const float obs_variance = getRngVar();
const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
const float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
const float hagl_test_ratio = sq(hagl_innov) / (sq(gate_size) * hagl_innov_var);
// Reset the state to the measurement only if the test ratio is large,
// otherwise let it converge through the fusion
if (hagl_test_ratio > 0.2f) {
resetHaglRng();
} else {
fuseHaglRng();
}
} else {
resetHaglRng();
}
}
float Ekf::getRngVar()
{
return fmaxf(P(9, 9) * _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();
_terrain_var = getRngVar();
_terrain_vpos_reset_counter++;
_time_last_hagl_fuse = _time_last_imu;
}
void Ekf::stopHaglRngFusion()
{
_hagl_sensor_status.flags.range_finder = false;
}
void Ekf::fuseHaglRng()
{
// get a height above ground measurement from the range finder assuming a flat earth
const float meas_hagl = _range_sensor.getDistBottom();
@ -142,9 +214,7 @@ void Ekf::fuseHagl()
_hagl_innov = pred_hagl - meas_hagl;
// calculate the observation variance adding the variance of the vehicles own height uncertainty
const float obs_variance = fmaxf(P(9, 9) * _params.vehicle_variance_scaler, 0.0f)
+ sq(_params.range_noise)
+ sq(_params.range_noise_scaler * _range_sensor.getRange());
const float obs_variance = getRngVar();
// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
@ -165,20 +235,17 @@ void Ekf::fuseHagl()
_innov_check_fail_status.flags.reject_hagl = false;
} else {
// If we have been rejecting range data for too long, reset to measurement
const uint64_t timeout = static_cast<uint64_t>(_params.terrain_timeout * 1e6f);
if (isTimedOut(_time_last_hagl_fuse, timeout)) {
_terrain_vpos = _state.pos(2) + meas_hagl;
_terrain_var = obs_variance;
_terrain_vpos_reset_counter++;
} else {
_innov_check_fail_status.flags.reject_hagl = true;
}
_innov_check_fail_status.flags.reject_hagl = true;
}
}
void Ekf::resetHaglFlow()
{
_terrain_vpos = fmaxf(0.0f, _state.pos(2));
_terrain_var = 100.0f;
_terrain_vpos_reset_counter++;
}
void Ekf::fuseFlowForTerrain()
{
// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
@ -279,6 +346,24 @@ void Ekf::fuseFlowForTerrain()
}
}
void Ekf::controlHaglFakeFusion()
{
if (!_control_status.flags.in_air
&& !_hagl_sensor_status.flags.range_finder
&& !_hagl_sensor_status.flags.flow) {
resetHaglFake();
}
}
void Ekf::resetHaglFake()
{
// 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);
_time_last_hagl_fuse = _time_last_imu;
}
void Ekf::updateTerrainValidity()
{
// we have been fusing range finder measurements in the last 5 seconds
@ -288,10 +373,7 @@ void Ekf::updateTerrainValidity()
// this can only be the case if the main filter does not fuse optical flow
const bool recent_flow_for_terrain_fusion = isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6);
_hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion));
_hagl_sensor_status.flags.range_finder = shouldUseRangeFinderForHagl()
&& recent_range_fusion && (_time_last_fake_hagl_fuse != _time_last_hagl_fuse);
_hagl_valid = (recent_range_fusion || recent_flow_for_terrain_fusion);
_hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl() && recent_flow_for_terrain_fusion;
}