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

EKF: Use common timeout test for reporting deadreckoning

Browse Source 
Use a parameter to set the time-out threshold
Use a class variable for the time-out flag
Clean up comments
This commit is contained in:
Paul Riseborough
2017-03-15 07:57:38 +01:00
committed by Lorenz Meier
parent 6ac7e2f807
commit 040639837e
5 changed files with 33 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+6 -2
View File
@@ -276,7 +276,9 @@ struct parameters {
float acc_bias_learn_tc; // time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
unsigned no_gps_timeout_max; // maximum time we allow dead reckoning while both gps position and velocity measurements are being
// rejected
// rejected before attempting to reset the states to the GPS measurement (usec)
unsigned no_aid_timeout_max; // maximum lapsed time from last fusion of measurements that constrain drift before
// the EKF will report that it is dead-reckoning (usec)
// Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility.
parameters()
@@ -385,7 +387,9 @@ struct parameters {
acc_bias_learn_gyr_lim = 3.0f;
acc_bias_learn_tc = 0.5f;
no_gps_timeout_max = 7e6; // maximum seven seconds of dead reckoning time for gps
// dead reckoning timers
no_gps_timeout_max = 7e6;
no_aid_timeout_max = 1e6;
}
};
EKF/control.cpp
+6 -1
View File
@@ -107,10 +107,15 @@ void Ekf::controlFusionModes()
controlAirDataFusion();
controlBetaFusion();
// for efficiency, fusion of direct state observations for position ad velocity is performed sequentially
// 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)
controlVelPosFusion();
// report dead reckoning if we are no longer fusing measurements that constrain velocity drift
_is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > _params.no_aid_timeout_max)
&& (_time_last_imu - _time_last_vel_fuse > _params.no_aid_timeout_max)
&& (_time_last_imu - _time_last_of_fuse > _params.no_aid_timeout_max);
}
void Ekf::controlExternalVisionFusion()
EKF/ekf_helper.cpp
+17 -40
View File
@@ -741,27 +741,17 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
}
// report dead reckoning if it is more than a second since we fused in measurements that constrain velocity drift
uint64_t dead_reckoning_duration = math::min(math::min((_time_last_imu - _time_last_pos_fuse),(_time_last_imu - _time_last_vel_fuse)),(_time_last_imu - _time_last_of_fuse));
bool is_dead_reckoning = dead_reckoning_duration > 1e6;
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal velocity error
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
// The reason is that complete rejection of measurements is often casued by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting is overly optimistic in these situations
float pos_err_alt = 0.0f;
if (is_dead_reckoning) {
// calculate a conservative estimate of the error in our position
if (_control_status.flags.gps || _control_status.flags.ev_pos) {
pos_err_alt = math::max(pos_err_alt, sqrtf(_vel_pos_innov[3]*_vel_pos_innov[3] + _vel_pos_innov[4]*_vel_pos_innov[4]));
}
hpos_err = math::max(hpos_err, pos_err_alt);
if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) {
hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3]*_vel_pos_innov[3] + _vel_pos_innov[4]*_vel_pos_innov[4]));
}
memcpy(ekf_eph, &hpos_err, sizeof(float));
memcpy(ekf_epv, &vpos_err, sizeof(float));
memcpy(dead_reckoning, &is_dead_reckoning, sizeof(bool));
memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool));
}
// get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
@@ -781,27 +771,17 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
}
// report dead reckoning if it is more than a second since we fused in measurements that constrain velocity drift
uint64_t dead_reckoning_duration = math::min(math::min((_time_last_imu - _time_last_pos_fuse),(_time_last_imu - _time_last_vel_fuse)),(_time_last_imu - _time_last_of_fuse));
bool is_dead_reckoning = dead_reckoning_duration > 1e6;
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal velocity error
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
// The reason is that complete rejection of measurements is often casued by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting is overly optimistic in these situations
float pos_err_alt = 0.0f;
if (is_dead_reckoning) {
// calculate a conservative estimate of the error in our position
if (_control_status.flags.gps || _control_status.flags.ev_pos) {
pos_err_alt = math::max(pos_err_alt, sqrtf(_vel_pos_innov[3]*_vel_pos_innov[3] + _vel_pos_innov[4]*_vel_pos_innov[4]));
}
hpos_err = math::max(hpos_err, pos_err_alt);
if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) {
hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3]*_vel_pos_innov[3] + _vel_pos_innov[4]*_vel_pos_innov[4]));
}
memcpy(ekf_eph, &hpos_err, sizeof(float));
memcpy(ekf_epv, &vpos_err, sizeof(float));
memcpy(dead_reckoning, &is_dead_reckoning, sizeof(bool));
memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool));
}
// get the 1-sigma horizontal and vertical velocity uncertainty
@@ -820,27 +800,24 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon
}
// report dead reckoning if it is more than a second since we fused in measurements that constrain velocity drift
bool is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > 1e6) && (_time_last_imu - _time_last_vel_fuse > 1e6) && (_time_last_imu - _time_last_of_fuse > 1e6);
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal velocity error
// The reason is that complete rejection of measurements is often be casued by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting provides an overly optimistic assessment in these situations
float vel_err_alt = 0.0f;
if (is_dead_reckoning) {
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting is overly optimistic in these situations
float vel_err_conservative = 0.0f;
if (_is_dead_reckoning) {
if (_control_status.flags.opt_flow) {
float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
vel_err_alt = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * sqrtf(_flow_innov[0]*_flow_innov[0] + _flow_innov[1]*_flow_innov[1]);
vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * sqrtf(_flow_innov[0]*_flow_innov[0] + _flow_innov[1]*_flow_innov[1]);
}
if (_control_status.flags.gps || _control_status.flags.ev_pos) {
vel_err_alt = math::max(vel_err_alt, sqrtf(_vel_pos_innov[0]*_vel_pos_innov[0] + _vel_pos_innov[1]*_vel_pos_innov[1]));
vel_err_conservative = math::max(vel_err_conservative, sqrtf(_vel_pos_innov[0]*_vel_pos_innov[0] + _vel_pos_innov[1]*_vel_pos_innov[1]));
}
hvel_err = math::max(hvel_err, vel_err_alt);
hvel_err = math::max(hvel_err, vel_err_conservative);
}
memcpy(ekf_evh, &hvel_err, sizeof(float));
memcpy(ekf_evv, &vvel_err, sizeof(float));
memcpy(dead_reckoning, &is_dead_reckoning, sizeof(bool));
memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool));
}
// get EKF innovation consistency check status information comprising of:
EKF/estimator_interface.cpp
+1
View File
@@ -73,6 +73,7 @@ EstimatorInterface::EstimatorInterface():
_tas_test_ratio(0.0f),
_terr_test_ratio(0.0f),
_beta_test_ratio(0.0f),
_is_dead_reckoning(false),
_vibe_metrics{},
_time_last_imu(0),
_time_last_gps(0),
EKF/estimator_interface.h
+3 -1
View File
@@ -336,9 +336,11 @@ protected:
float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios
float _tas_test_ratio; // tas innovation consistency check ratio
float _terr_test_ratio; // height above terrain measurement innovation consistency check ratio
float _beta_test_ratio; // sideslip innovation consistency check ratio
float _beta_test_ratio; // sideslip innovation consistency check ratio
innovation_fault_status_u _innov_check_fail_status{};
bool _is_dead_reckoning; // true if we are no longer fusing measurements that constrain horizontal velocity drift
// IMU vibration monitoring
Vector3f _delta_ang_prev; // delta angle from the previous IMU measurement
Vector3f _delta_vel_prev; // delta velocity from the previous IMU measurement