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

Merge pull request #244 from PX4/pr-rangeAid

Browse Source 
Pr range aid
This commit is contained in:
Paul Riseborough
2017-06-16 17:47:21 +10:00
committed by GitHub
parent d6decb3c84 6f5cffafec
commit 7f1cca5b28
6 changed files with 223 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+5
View File
@@ -256,6 +256,11 @@ struct parameters {
float rng_gnd_clearance{0.1f}; // minimum valid value for range when on ground (m)
float rng_sens_pitch{0.0f}; // Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
float range_noise_scaler{0.0f}; // scaling from range measurement to noise (m/m)
float vehicle_variance_scaler{0.0f}; // gain applied to vehicle height variance used in calculation of height above ground observation variance
float max_hagl_for_range_aid{5.0f}; // maximum height above ground for which we allow to use the range finder as height source (if range_aid == 1)
float max_vel_for_range_aid{1.0f}; // maximum ground velocity for which we allow to use the range finder as height source (if range_aid == 1)
int32_t range_aid{0}; // allow switching primary height source to range finder if certian conditions are met
float range_aid_innov_gate{1.0f}; // gate size used for innovation consistency checks for range aid fusion
// vision position fusion
float ev_innov_gate{5.0f}; // vision estimator fusion innovation consistency gate size (STD)
EKF/control.cpp
+154 -102
View File
@@ -82,8 +82,16 @@ void Ekf::controlFusionModes()
// check for arrival of new sensor data at the fusion time horizon
_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
_delta_time_baro_us = _baro_sample_delayed.time_us;
_baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
// if we have a new baro sample save the delta time between this sample and the last sample which is
// used below for baro offset calculations
if (_baro_data_ready) {
_delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us;
}
// calculate 2,2 element of rotation matrix from sensor frame to earth frame
_R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng;
_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
@@ -102,11 +110,10 @@ void Ekf::controlFusionModes()
controlExternalVisionFusion();
controlOpticalFlowFusion();
controlGpsFusion();
controlBaroFusion();
controlRangeFinderFusion();
controlAirDataFusion();
controlBetaFusion();
controlDragFusion();
controlHeightFusion();
// for efficiency, fusion of direct state observations for position and velocity is performed sequentially
// in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc)
@@ -129,12 +136,8 @@ void Ekf::controlExternalVisionFusion()
// check for a exernal vision measurement that has fallen behind the fusion time horizon
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
// turn on use of external vision measurements for position and height
_control_status.flags.ev_pos = true;
setControlEVHeight();
ECL_INFO("EKF commencing external vision position fusion");
// turn off other forms of height aiding
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
// reset the position, height and velocity
resetPosition();
resetVelocity();
@@ -198,10 +201,7 @@ void Ekf::controlExternalVisionFusion()
// determine if we should use the height observation
if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = true;
setControlEVHeight();
_fuse_height = true;
}
@@ -424,15 +424,6 @@ void Ekf::controlGpsFusion()
}
// Determine if GPS should be used as the height source
if (((_params.vdist_sensor_type == VDIST_SENSOR_GPS)) && !_gps_hgt_faulty) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
_fuse_height = true;
}
} else {
// handle the case where we do not have GPS and have not been using it for an extended period, but are still relying on it
if ((_time_last_imu - _time_last_gps > 10e6) && (_time_last_imu - _time_last_airspeed > 1e6) && (_time_last_imu - _time_last_optflow > 1e6) && _control_status.flags.gps) {
@@ -523,10 +514,7 @@ void Ekf::controlHeightSensorTimeouts()
_gps_hgt_faulty = false;
// reset the height mode
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
setControlGPSHeight();
// request a reset
reset_height = true;
@@ -537,10 +525,7 @@ void Ekf::controlHeightSensorTimeouts()
_baro_hgt_faulty = false;
// reset the height mode
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
setControlBaroHeight();
// request a reset
reset_height = true;
@@ -582,10 +567,7 @@ void Ekf::controlHeightSensorTimeouts()
_baro_hgt_faulty = false;
// reset the height mode
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
setControlBaroHeight();
// request a reset
reset_height = true;
@@ -596,10 +578,7 @@ void Ekf::controlHeightSensorTimeouts()
_gps_hgt_faulty = false;
// reset the height mode
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
setControlGPSHeight();
// request a reset
reset_height = true;
@@ -634,10 +613,7 @@ void Ekf::controlHeightSensorTimeouts()
_baro_hgt_faulty = false;
// reset the height mode
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
setControlBaroHeight();
// request a reset
reset_height = true;
@@ -648,10 +624,7 @@ void Ekf::controlHeightSensorTimeouts()
_rng_hgt_faulty = false;
// reset the height mode
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
_control_status.flags.ev_hgt = false;
setControlRangeHeight();
// request a reset
reset_height = true;
@@ -686,10 +659,7 @@ void Ekf::controlHeightSensorTimeouts()
_baro_hgt_faulty = false;
// reset the height mode
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
setControlBaroHeight();
// request a reset
reset_height = true;
@@ -697,10 +667,7 @@ void Ekf::controlHeightSensorTimeouts()
} else if (reset_to_ev) {
// reset the height mode
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = true;
setControlEVHeight();
// request a reset
reset_height = true;
@@ -724,57 +691,95 @@ void Ekf::controlHeightSensorTimeouts()
}
}
void Ekf::controlBaroFusion()
void Ekf::controlHeightFusion()
{
if (_baro_data_ready) {
// determine if we should use the baro as our height source
uint64_t last_baro_time_us = _baro_sample_delayed.time_us;
if (((_params.vdist_sensor_type == VDIST_SENSOR_BARO) || _control_status.flags.baro_hgt) && !_baro_hgt_faulty) {
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
_fuse_height = true;
// set control flags for the desired primary height source
}
// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
if (!_control_status.flags.baro_hgt) {
float local_time_step = 1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us);
local_time_step = math::constrain(local_time_step,0.0f,1.0f);
last_baro_time_us = _baro_sample_delayed.time_us;
float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset) + _state.pos(2) - _baro_hgt_offset;
_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
}
}
}
void Ekf::controlRangeFinderFusion()
{
// determine if we should use range finder data for height
if (_range_data_ready) {
// set the height data source to range if requested
if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && !_rng_hgt_faulty) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
_control_status.flags.ev_hgt = false;
}
// correct the range data for position offset relative to the IMU
Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
}
// only use range finder as a height observation in the main filter if specifically enabled
if (_control_status.flags.rng_hgt) {
if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
_in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode);
if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) {
setControlRangeHeight();
_fuse_height = true;
}
// we have just switched to using range finder, calculate height sensor offset such that current
// measurment matches our current height estimate
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
if (_terrain_initialised) {
_hgt_sensor_offset = _terrain_vpos;
} else {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
}
}
} else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) {
} else if (_baro_data_ready && !_baro_hgt_faulty && !_in_range_aid_mode) {
setControlBaroHeight();
_fuse_height = true;
// we have just switched to using baro height, we don't need to set a height sensor offset
// since we track a separate _baro_hgt_offset
if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
_hgt_sensor_offset = 0.0f;
}
}
}
// set the height data source to range if requested
if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && !_rng_hgt_faulty) {
setControlRangeHeight();
_fuse_height = _range_data_ready;
}
// Determine if GPS should be used as the height source
if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
_in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode);
if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) {
setControlRangeHeight();
_fuse_height = true;
// we have just switched to using range finder, calculate height sensor offset such that current
// measurment matches our current height estimate
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
if (_terrain_initialised) {
_hgt_sensor_offset = _terrain_vpos;
} else {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
}
}
} else if (_gps_data_ready && !_gps_hgt_faulty && !_in_range_aid_mode) {
setControlGPSHeight();
_fuse_height = true;
// we have just switched to using gps height, calculate height sensor offset such that current
// measurment matches our current height estimate
if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
}
}
}
// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
if (!_control_status.flags.baro_hgt && _baro_data_ready) {
float local_time_step = 1e-6f * _delta_time_baro_us;
local_time_step = math::constrain(local_time_step, 0.0f, 1.0f);
// apply a 10 second first order low pass filter to baro offset
float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt + _state.pos(
2) - _baro_hgt_offset);
_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
}
if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt && !_range_data_ready) {
// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
// and are on the ground, then synthesise a measurement at the expected on ground value
if (!_control_status.flags.in_air) {
@@ -784,7 +789,55 @@ void Ekf::controlRangeFinderFusion()
}
_fuse_height = true;
}
}
bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
{
// if the parameter for range aid is enabled we allow to switch from using the primary height source to using range finder as height source
// under the following conditions
// 1) we are not further than max_range_for_dual_fusion away from the ground
// 2) our ground speed is not higher than max_vel_for_dual_fusion
// 3) Our terrain estimate is stable (needs better checks)
if (_params.range_aid) {
// check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching
bool use_range_finder;
if (in_range_aid_mode) {
use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && _terrain_initialised;
} else {
// if we were not using range aid in the previous iteration then require the current height above terrain to be
// smaller than 70 % of the maximum allowed ground distance for range aid
use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && _terrain_initialised;
}
bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow)
&& (_fault_status.value == 0);
if (horz_vel_valid) {
float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1));
if (in_range_aid_mode) {
use_range_finder &= ground_vel < _params.max_vel_for_range_aid;
} else {
// if we were not using range aid in the previous iteration then require the ground velocity to be
// smaller than 70 % of the maximum allowed ground velocity for range aid
use_range_finder &= ground_vel < 0.7f * _params.max_vel_for_range_aid;
}
} else {
use_range_finder = false;
}
use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f);
return use_range_finder;
} else {
return false;
}
}
@@ -824,15 +877,14 @@ void Ekf::controlAirDataFusion()
void Ekf::controlBetaFusion()
{
// control activation and initialisation/reset of wind states required for synthetic sideslip fusion fusion
// control activation and initialisation/reset of wind states required for synthetic sideslip fusion fusion
// If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates
bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)){
_control_status.flags.wind = false;
}
// If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates
bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
_control_status.flags.wind = false;
}
// Perform synthetic sideslip fusion when in-air and sideslip fuson had been enabled externally in addition to the following criteria:
@@ -855,10 +907,10 @@ void Ekf::controlBetaFusion()
resetWindCovariance();
}
fuseSideslip();
fuseSideslip();
}
}
EKF/ekf.cpp
+1
View File
@@ -57,6 +57,7 @@
#define ISFINITE(x) __builtin_isfinite(x)
#endif
bool Ekf::init(uint64_t timestamp)
{
bool ret = initialise_interface(timestamp);
EKF/ekf.h
+21
View File
@@ -261,6 +261,7 @@ private:
float _imu_collection_time_adj{0.0f}; // the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
uint64_t _time_acc_bias_check{0}; // last time the accel bias check passed (usec)
uint64_t _delta_time_baro_us{0}; // delta time between two consecutive delayed baro samples from the buffer (usec)
Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s
@@ -374,6 +375,9 @@ private:
uint64_t _time_good_vert_accel{0}; // last time a good vertical accel was detected (usec)
bool _bad_vert_accel_detected{false}; // true when bad vertical accelerometer data has been detected
// variables used to control range aid functionality
bool _in_range_aid_mode;
// update the real time complementary filter states. This includes the prediction
// and the correction step
void calculateOutputStates();
@@ -501,12 +505,29 @@ private:
// control for height sensor timeouts, sensor changes and state resets
void controlHeightSensorTimeouts();
// control for combined height fusion mode (implemented for switching between baro and range height)
void controlHeightFusion();
bool rangeAidConditionsMet(bool in_range_aid_mode);
// return the square of two floating point numbers - used in auto coded sections
inline float sq(float var)
{
return var * var;
}
// set control flags to use baro height
void setControlBaroHeight();
// set control flags to use range height
void setControlRangeHeight();
// set control flags to use GPS height
void setControlGPSHeight();
// set control flags to use external vision height
void setControlEVHeight();
// zero the specified range of rows in the state covariance matrix
void zeroRows(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
EKF/ekf_helper.cpp
+41 -1
View File
@@ -158,8 +158,12 @@ void Ekf::resetHeight()
rangeSample range_newest = _range_buffer.get_newest();
if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) {
// correct the range data for position offset relative to the IMU
Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
range_newest.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
// calculate the new vertical position using range sensor
float new_pos_down = _hgt_sensor_offset - range_newest.rng;
float new_pos_down = _hgt_sensor_offset - range_newest.rng * _R_rng_to_earth_2_2;
// update the state and assoicated variance
_state.pos(2) = new_pos_down;
@@ -1162,3 +1166,39 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
}
}
void Ekf::setControlBaroHeight()
{
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
}
void Ekf::setControlRangeHeight()
{
_control_status.flags.rng_hgt = true;
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.ev_hgt = false;
}
void Ekf::setControlGPSHeight()
{
_control_status.flags.gps_hgt = true;
_control_status.flags.baro_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
}
void Ekf::setControlEVHeight()
{
_control_status.flags.ev_hgt = true;
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
}
EKF/vel_pos_fusion.cpp
+1 -1
View File
@@ -159,7 +159,7 @@ void Ekf::fuseVelPosHeight()
fuse_map[5] = true;
// use range finder with tilt correction
_vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,
_params.rng_gnd_clearance));
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
// observation variance - user parameter defined
R[5] = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f);
// innovation gate size