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

ekf2: remove legacy functions for terrain estimation

Browse Source
This commit is contained in:
Paul Riseborough 2022-01-25 18:28:04 +11:00 committed by bresch
parent d1fd00b92a
commit ea8465c277
7 changed files with 55 additions and 223 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/ekf2/EKF/control.cpp
View File

@ -909,8 +909,8 @@ void Ekf::controlHeightFusion()
}
// Fuse range finder data if available
const bool use_for_hagl = _terrain_initialised && shouldUseRangeFinderForHagl();
if ((use_for_hagl || _control_status.flags.rng_hgt) && _range_sensor.isDataHealthy()) {
controlHaglFusion();
if ((_control_status.flags.rng_hgt || _hagl_sensor_status.flags.range_finder) && _range_sensor.isDataHealthy()) {
fuseHaglAllStates();
}
}
@ -922,7 +922,7 @@ void Ekf::checkRangeAidSuitability()
&& 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
const float range_hagl = _terrain_vpos - _state.pos(2);
const float range_hagl = _state.posd_terrain - _state.pos(2);
const float range_hagl_max = _is_range_aid_suitable ? _params.max_hagl_for_range_aid : (_params.max_hagl_for_range_aid * 0.7f);
const bool is_in_range = range_hagl < range_hagl_max;

17
src/modules/ekf2/EKF/covariance.cpp
View File

@ -865,7 +865,7 @@ void Ekf::predictCovariance()
}
if (_terrain_initialised) {
if (_params.terrain_fusion_mode > 0) {
nextP(0,24) = P(0,24) - P(1,24)*PS11 + P(10,24)*PS6 + P(11,24)*PS7 + P(12,24)*PS9 - P(2,24)*PS12 - P(3,24)*PS13;
nextP(1,24) = P(0,24)*PS11 + P(1,24) - P(10,24)*PS34 + P(11,24)*PS9 - P(12,24)*PS7 + P(2,24)*PS13 - P(3,24)*PS12;
nextP(2,24) = P(0,24)*PS12 - P(1,24)*PS13 - P(10,24)*PS9 - P(11,24)*PS34 + P(12,24)*PS6 + P(2,24) + P(3,24)*PS11;
@ -1093,17 +1093,12 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
}
// terrain vertical position
if (!_terrain_initialised) {
P.uncorrelateCovarianceSetVariance<1>(24, 0.0f);
// constrain variances
P(24, 24) = math::constrain(P(24, 24), 0.0f, P_lim[8]);
} else {
// constrain variances
P(24, 24) = math::constrain(P(24, 24), 0.0f, P_lim[8]);
// force symmetry
if (force_symmetry) {
P.makeRowColSymmetric<1>(24);
}
// force symmetry
if (force_symmetry) {
P.makeRowColSymmetric<1>(24);
}
}

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

@ -108,9 +108,6 @@ bool Ekf::update()
// control fusion of observation data
controlFusionModes();
// run a separate filter for terrain estimation
runTerrainEstimator();
updated = true;
// run EKF-GSF yaw estimator

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

@ -228,13 +228,13 @@ public:
uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; }
// get the estimated terrain vertical position relative to the NED origin
float getTerrainVertPos() const { return _terrain_vpos; };
float getTerrainVertPos() const { return _state.posd_terrain; };
// get the number of times the vertical terrain position has been reset
uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
// get the terrain variance
float get_terrain_var() const { return _terrain_var; }
float get_terrain_var() const { return P(24,24); }
Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s
Vector3f getAccelBias() const { return _state.delta_vel_bias / _dt_ekf_avg; } // get the accelerometer bias in m/s**2
@ -534,10 +534,9 @@ private:
Vector3f _prev_dvel_bias_var{}; ///< saved delta velocity XYZ bias variances (m/sec)**2
// Terrain height state estimation
float _terrain_vpos{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
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
uint8_t _terrain_vpos_reset_counter{0}; ///< number of times _state.posd_terrain 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 _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
@ -690,12 +689,10 @@ private:
// initialise the terrain vertical position estimator
void initHagl();
void runTerrainEstimator();
void predictHagl();
void controlHaglFusion();
// update the terrain vertical position estimate using a height above ground measurement from the range finder
void controlHaglRngFusion();
void fuseHaglRng();
void startHaglRngFusion();
void resetHaglRngIfNeeded();
void resetHaglRng();
@ -707,7 +704,6 @@ private:
void startHaglFlowFusion();
void resetHaglFlow();
void stopHaglFlowFusion();
void fuseFlowForTerrain();
void controlHaglFakeFusion();
void resetHaglFake();
@ -848,7 +844,7 @@ private:
bool otherHeadingSourcesHaveStopped();
void checkHaglYawResetReq();
float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
float getTerrainVPos() const { return isTerrainEstimateValid() ? _state.posd_terrain : _last_on_ground_posD; }
void runOnGroundYawReset();
bool isYawResetAuthorized() const { return !_is_yaw_fusion_inhibited; }

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

@ -74,7 +74,7 @@ void Ekf::resetHorizontalVelocityToOpticalFlow()
_information_events.flags.reset_vel_to_flow = true;
ECL_INFO("reset velocity to flow");
// constrain height above ground to be above minimum possible
const float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
const float heightAboveGndEst = fmaxf((_state.posd_terrain - _state.pos(2)), _params.rng_gnd_clearance);
// calculate absolute distance from focal point to centre of frame assuming a flat earth
const float range = heightAboveGndEst / _range_sensor.getCosTilt();
@ -781,7 +781,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
if (_control_status.flags.opt_flow) {
float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * _flow_innov.norm();
vel_err_conservative = math::max((_state.posd_terrain - _state.pos(2)), gndclearance) * _flow_innov.norm();
}
if (_control_status.flags.gps) {
@ -818,7 +818,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
// Calculate optical flow limits
// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
const float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_terrain_vpos - _state.pos(2)), 0.0f);
const float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_state.posd_terrain - _state.pos(2)), 0.0f);
const float flow_hagl_min = _flow_min_distance;
const float flow_hagl_max = _flow_max_distance;
@ -1304,7 +1304,7 @@ void Ekf::startRngAidHgtFusion()
// calculate height sensor offset such that current
// measurement matches our current height estimate
_hgt_sensor_offset = _terrain_vpos;
_hgt_sensor_offset = _state.posd_terrain;
}
}

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

@ -86,7 +86,7 @@ void Ekf::fuseOptFlow()
const Vector3f vel_body = earth_to_body * vel_rel_earth;
// height above ground of the IMU
float heightAboveGndEst = _terrain_vpos - _state.pos(2);
float heightAboveGndEst = _state.posd_terrain - _state.pos(2);
// calculate the sensor position relative to the IMU in earth frame
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;

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

@ -48,7 +48,7 @@ void Ekf::initHagl()
resetHaglFake();
}
void Ekf::runTerrainEstimator()
void Ekf::controlHaglFusion()
{
// If we are on ground, store the local position and time to use as a reference
if (!_control_status.flags.in_air) {
@ -56,35 +56,18 @@ void Ekf::runTerrainEstimator()
_control_status.flags.rng_fault = false;
}
predictHagl();
controlHaglRngFusion();
controlHaglFlowFusion();
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);
// constrain terrain position to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
if (_state.posd_terrain - _state.pos(2) < _params.rng_gnd_clearance) {
_state.posd_terrain = _params.rng_gnd_clearance + _state.pos(2);
}
updateTerrainValidity();
}
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)
@ -103,7 +86,7 @@ void Ekf::controlHaglRngFusion()
if (_hagl_sensor_status.flags.range_finder) {
if (continuing_conditions_passing) {
fuseHaglRng();
/* fuseHaglRng(); */ // done when fusing range finder
// We have been rejecting range data for too long
const uint64_t timeout = static_cast<uint64_t>(_params.terrain_timeout * 1e6f);
@ -153,11 +136,11 @@ 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 pred_hagl = _state.posd_terrain - _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 hagl_innov_var = fmaxf(P(24, 24) + 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);
@ -168,7 +151,7 @@ void Ekf::resetHaglRngIfNeeded()
resetHaglRng();
} else {
fuseHaglRng();
/* fuseHaglRng(); */
}
} else {
@ -185,8 +168,8 @@ float Ekf::getRngVar()
void Ekf::resetHaglRng()
{
_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
_terrain_var = getRngVar();
_state.posd_terrain = _state.pos(2) + _range_sensor.getDistBottom();
P(24, 24) = getRngVar();
_terrain_vpos_reset_counter++;
_time_last_hagl_fuse = _time_last_imu;
}
@ -196,43 +179,6 @@ 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();
// predict the hagl from the vehicle position and terrain height
const float pred_hagl = _terrain_vpos - _state.pos(2);
// calculate the innovation
_hagl_innov = pred_hagl - meas_hagl;
// calculate the observation variance adding the variance of the vehicles own height uncertainty
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);
// perform an innovation consistency check and only fuse data if it passes
const float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
_hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
if (_hagl_test_ratio <= 1.0f) {
// calculate the Kalman gain
const 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;
}
}
void Ekf::controlHaglFlowFusion()
{
if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)) {
@ -252,8 +198,8 @@ void Ekf::controlHaglFlowFusion()
if (continuing_conditions_passing) {
// TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon
fuseFlowForTerrain();
_flow_data_ready = false;
/* fuseFlowForTerrain(); */
/* _flow_data_ready = false; */ // done in flow fusion
// TODO: do something when failing continuously the innovation check
/* const bool is_fusion_failing = isTimedOut(_time_last_flow_terrain_fuse, _params.reset_timeout_max); */
@ -283,8 +229,8 @@ void Ekf::startHaglFlowFusion()
{
_hagl_sensor_status.flags.flow = true;
// TODO: do a reset instead of trying to fuse the data?
fuseFlowForTerrain();
_flow_data_ready = false;
/* fuseFlowForTerrain(); */
/* _flow_data_ready = false; */
}
void Ekf::stopHaglFlowFusion()
@ -295,111 +241,11 @@ void Ekf::stopHaglFlowFusion()
void Ekf::resetHaglFlow()
{
// TODO: use the flow data
_terrain_vpos = fmaxf(0.0f, _state.pos(2));
_terrain_var = 100.0f;
_state.posd_terrain = fmaxf(0.0f, _state.pos(2));
P(24, 24) = 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
// correct for gyro bias errors in the data used to do the motion compensation
// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias);
// get latest estimated orientation
const float q0 = _state.quat_nominal(0);
const float q1 = _state.quat_nominal(1);
const float q2 = _state.quat_nominal(2);
const float q3 = _state.quat_nominal(3);
// calculate the optical flow observation variance
const float R_LOS = calcOptFlowMeasVar();
// get rotation matrix from earth to body
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
// calculate the sensor position relative to the IMU
const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
// calculate the velocity of the sensor relative to the imu in body frame
// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;
// calculate the velocity of the sensor in the earth frame
const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
// rotate into body frame
const Vector3f vel_body = earth_to_body * vel_rel_earth;
const 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));
const float pred_hagl_inv = 1.f / (_terrain_vpos - _state.pos(2));
// Calculate observation matrix for flow around the vehicle x axis
const float Hx = vel_body(1) * t0 * pred_hagl_inv * pred_hagl_inv;
// Constrain terrain variance to be non-negative
_terrain_var = fmaxf(_terrain_var, 0.0f);
// Cacluate innovation variance
_flow_innov_var(0) = Hx * Hx * _terrain_var + R_LOS;
// calculate the kalman gain for the flow x measurement
const float Kx = _terrain_var * Hx / _flow_innov_var(0);
// calculate prediced optical flow about x axis
const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) * pred_hagl_inv;
// calculate flow innovation (x axis)
_flow_innov(0) = pred_flow_x - opt_flow_rate(0);
// calculate correction term for terrain variance
const float KxHxP = Kx * Hx * _terrain_var;
// innovation consistency check
const float gate_size = fmaxf(_params.flow_innov_gate, 1.0f);
float flow_test_ratio = sq(_flow_innov(0)) / (sq(gate_size) * _flow_innov_var(0));
// do not perform measurement update if badly conditioned
if (flow_test_ratio <= 1.0f) {
_terrain_vpos += Kx * _flow_innov(0);
// guard against negative variance
_terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f);
_time_last_flow_terrain_fuse = _time_last_imu;
}
// Calculate observation matrix for flow around the vehicle y axis
const float Hy = -vel_body(0) * t0 * pred_hagl_inv * pred_hagl_inv;
// Calculuate innovation variance
_flow_innov_var(1) = Hy * Hy * _terrain_var + R_LOS;
// calculate the kalman gain for the flow y measurement
const float Ky = _terrain_var * Hy / _flow_innov_var(1);
// calculate prediced optical flow about y axis
const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) * pred_hagl_inv;
// calculate flow innovation (y axis)
_flow_innov(1) = pred_flow_y - opt_flow_rate(1);
// calculate correction term for terrain variance
const float KyHyP = Ky * Hy * _terrain_var;
// innovation consistency check
flow_test_ratio = sq(_flow_innov(1)) / (sq(gate_size) * _flow_innov_var(1));
if (flow_test_ratio <= 1.0f) {
_terrain_vpos += Ky * _flow_innov(1);
// guard against negative variance
_terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f);
_time_last_flow_terrain_fuse = _time_last_imu;
}
}
void Ekf::controlHaglFakeFusion()
{
if (!_control_status.flags.in_air
@ -412,9 +258,9 @@ void Ekf::controlHaglFakeFusion()
void Ekf::resetHaglFake()
{
// assume a ground clearance
_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
_state.posd_terrain = _state.pos(2) + _params.rng_gnd_clearance;
// use the ground clearance value as our uncertainty
_terrain_var = sq(_params.rng_gnd_clearance);
P(24, 24) = sq(_params.rng_gnd_clearance);
_time_last_hagl_fuse = _time_last_imu;
}
@ -436,7 +282,7 @@ void Ekf::fuseHaglAllStates()
const float meas_hagl = _range_sensor.getDistBottom();
// predict the hagl from the vehicle position and terrain height
const float pred_hagl = _terrain_vpos - _state.pos(2);
const float pred_hagl = _state.posd_terrain - _state.pos(2);
// calculate the innovation
_hagl_innov = pred_hagl - meas_hagl;
@ -456,23 +302,25 @@ void Ekf::fuseHaglAllStates()
bool is_fused = false;
if (_hagl_test_ratio <= 1.0f) {
// calculate the Kalman gain
const float HK0 = 1.0F/_hagl_innov_var;
const float HK0 = 1.0f/_hagl_innov_var;
// calculate the observation Jacobians and Kalman gains
SparseVector25f<9,24> Hfusion; // Optical flow observation Jacobians
SparseVector25f<9,24> Hfusion;
Vector25f Kfusion;
if (_control_status.flags.rng_hgt) {
Hfusion.at<9>() = -1.0f;
if (shouldUseRangeFinderForHagl()) {
if (_hagl_sensor_status.flags.range_finder || _hagl_sensor_status.flags.flow) {
for (uint8_t index=0; index<=23; index++) {
Kfusion(index) = HK0*(P(index,24) - P(index,9));
}
} else {
for (uint8_t index=0; index<=23; index++) {
Kfusion(index) = - HK0*P(index,9);
}
}
} else {
for (unsigned row=0; row<=23; row++) {
// update of all states other than terrain is inhibited
@ -480,44 +328,40 @@ void Ekf::fuseHaglAllStates()
}
}
if (shouldUseRangeFinderForHagl()) {
if (_hagl_sensor_status.flags.range_finder) {
Hfusion.at<24>() = 1.0f;
if (_control_status.flags.rng_hgt) {
Kfusion(24) = HK0*(P(24,24) - P(24,9));
} else {
Kfusion(24) = HK0*P(24,24);
}
} else {
Kfusion(24) = 0.0f;
}
is_fused = measurementUpdate(Kfusion, Hfusion, _hagl_innov);
}
if (is_fused) {
// record last successful fusion event
if (shouldUseRangeFinderForHagl()) {
if (_hagl_sensor_status.flags.range_finder) {
_time_last_hagl_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_hagl = false;
}
if (_control_status.flags.rng_hgt) {
_time_last_hgt_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_ver_pos = false;
}
} else {
if (shouldUseRangeFinderForHagl()) {
// 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)) {
_state.posd_terrain = _state.pos(2) + meas_hagl;
P.uncorrelateCovarianceSetVariance<1>(24, 0.0f);
P(24,24) = obs_variance;
_terrain_vpos_reset_counter++;
} else {
_innov_check_fail_status.flags.reject_hagl = true;
}
if (_hagl_sensor_status.flags.range_finder) {
_innov_check_fail_status.flags.reject_hagl = true;
}
if (_control_status.flags.rng_hgt) {
_innov_check_fail_status.flags.reject_ver_pos = true;
}