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

ekf2: improve preflight checks and publish status

Browse Source 
Separate the vertical and horizontal checks for use by local position validity reporting
Add checking of yaw using a decaying envelope filter to the horizontal checks.
Publish the check result to estimator_status topic.
This commit is contained in:
Paul Riseborough
2017-12-04 09:09:54 +00:00
committed by Lorenz Meier
parent 92bcc63418
commit fc80be0917
1 changed files with 48 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+48 -17
View File
@@ -143,15 +143,21 @@ private:
float _last_valid_variance[3] = {}; ///< variances for the last valid magnetometer XYZ bias estimates (mGauss**2) float _last_valid_variance[3] = {}; ///< variances for the last valid magnetometer XYZ bias estimates (mGauss**2)
// Used to filter velocity innovations during pre-flight checks // Used to filter velocity innovations during pre-flight checks
bool _vel_innov_preflt_fail = false; ///< true if the norm of the filtered innovation vector is too large before flight bool _preflt_horiz_fail = false; ///< true if preflight horizontal innovation checks are failed
Vector3f _vel_innov_lpf_ned = {}; ///< Preflight low pass filtered velocity innovations (m/sec) bool _preflt_vert_fail = false; ///< true if preflight vertical innovation checks are failed
bool _preflt_fail = false; ///< true if any preflight innovation checks are failed
Vector2f _vel_ne_innov_lpf = {}; ///< Preflight low pass filtered NE axis velocity innovations (m/sec)
float _vel_d_innov_lpf = {}; ///< Preflight low pass filtered D axis velocity innovations (m/sec)
float _hgt_innov_lpf = 0.0f; ///< Preflight low pass filtered height innovation (m) float _hgt_innov_lpf = 0.0f; ///< Preflight low pass filtered height innovation (m)
float _yaw_innov_magnitude_lpf = 0.0f; ///< Preflight low pass filtered yaw innovation magntitude (rad)
static constexpr float _innov_lpf_tau_inv = 0.2f; ///< Preflight low pass filter time constant inverse (1/sec) static constexpr float _innov_lpf_tau_inv = 0.2f; ///< Preflight low pass filter time constant inverse (1/sec)
static constexpr float _vel_innov_test_lim = static constexpr float _vel_innov_test_lim =
0.5f; ///< Maximum permissible velocity innovation to pass pre-flight checks (m/sec) 0.5f; ///< Maximum permissible velocity innovation to pass pre-flight checks (m/sec)
static constexpr float _hgt_innov_test_lim = static constexpr float _hgt_innov_test_lim =
1.5f; ///< Maximum permissible height innovation to pass pre-flight checks (m) 1.5f; ///< Maximum permissible height innovation to pass pre-flight checks (m)
static constexpr float _yaw_innov_test_lim =
0.25f; ///< Maximum permissible yaw innovation to pass pre-flight checks (rad)
const float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim; ///< preflight velocity innovation spike limit (m/sec) const float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim; ///< preflight velocity innovation spike limit (m/sec)
const float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim; ///< preflight position innovation spike limit (m) const float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim; ///< preflight position innovation spike limit (m)
@@ -937,10 +943,10 @@ void Ekf2::run()
lpos.az = vel_deriv[2]; lpos.az = vel_deriv[2];
// TODO: better status reporting // TODO: better status reporting
lpos.xy_valid = _ekf.local_position_is_valid() && !_vel_innov_preflt_fail; lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_horiz_fail;
lpos.z_valid = !_vel_innov_preflt_fail; lpos.z_valid = !_preflt_vert_fail;
lpos.v_xy_valid = _ekf.local_position_is_valid() && !_vel_innov_preflt_fail; lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_horiz_fail;
lpos.v_z_valid = !_vel_innov_preflt_fail; lpos.v_z_valid = !_preflt_vert_fail;
// Position of local NED origin in GPS / WGS84 frame // Position of local NED origin in GPS / WGS84 frame
map_projection_reference_s ekf_origin; map_projection_reference_s ekf_origin;
@@ -987,7 +993,7 @@ void Ekf2::run()
// publish vehicle local position data // publish vehicle local position data
_vehicle_local_position_pub.update(); _vehicle_local_position_pub.update();
if (_ekf.global_position_is_valid() && !_vel_innov_preflt_fail) { if (_ekf.global_position_is_valid() && !_preflt_fail) {
// generate and publish global position data // generate and publish global position data
vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get(); vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get();
@@ -1092,6 +1098,7 @@ void Ekf2::run()
_ekf.get_ekf_soln_status(&status.solution_status_flags); _ekf.get_ekf_soln_status(&status.solution_status_flags);
_ekf.get_imu_vibe_metrics(status.vibe); _ekf.get_imu_vibe_metrics(status.vibe);
status.time_slip = _last_time_slip_us / 1e6f; status.time_slip = _last_time_slip_us / 1e6f;
status.pre_flt_fail = _preflt_fail;
if (_estimator_status_pub == nullptr) { if (_estimator_status_pub == nullptr) {
_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status); _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status);
@@ -1245,23 +1252,47 @@ void Ekf2::run()
// calculate noise filtered velocity innovations which are used for pre-flight checking // calculate noise filtered velocity innovations which are used for pre-flight checking
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
// calculate coefficients for LPF applied to innovation sequences
float alpha = constrain(sensors.accelerometer_integral_dt / 1.e6f * _innov_lpf_tau_inv, 0.0f, 1.0f); float alpha = constrain(sensors.accelerometer_integral_dt / 1.e6f * _innov_lpf_tau_inv, 0.0f, 1.0f);
float beta = 1.0f - alpha; float beta = 1.0f - alpha;
_vel_innov_lpf_ned(0) = beta * _vel_innov_lpf_ned(0) + alpha * constrain(innovations.vel_pos_innov[0],
-_vel_innov_spike_lim, _vel_innov_spike_lim); // filter the velocity and innvovations
_vel_innov_lpf_ned(1) = beta * _vel_innov_lpf_ned(1) + alpha * constrain(innovations.vel_pos_innov[1], _vel_ne_innov_lpf(0) = beta * _vel_ne_innov_lpf(0) + alpha * constrain(innovations.vel_pos_innov[0],
-_vel_innov_spike_lim, _vel_innov_spike_lim); -_vel_innov_spike_lim, _vel_innov_spike_lim);
_vel_innov_lpf_ned(2) = beta * _vel_innov_lpf_ned(2) + alpha * constrain(innovations.vel_pos_innov[2], _vel_ne_innov_lpf(1) = beta * _vel_ne_innov_lpf(1) + alpha * constrain(innovations.vel_pos_innov[1],
-_vel_innov_spike_lim, _vel_innov_spike_lim); -_vel_innov_spike_lim, _vel_innov_spike_lim);
_vel_d_innov_lpf = beta * _vel_d_innov_lpf + alpha * constrain(innovations.vel_pos_innov[2],
-_vel_innov_spike_lim, _vel_innov_spike_lim);
// filter the yaw innovations using a decaying envelope filter to prevent innovation sign changes due to angle wrapping allowinging large innvoations to pass checks after filtering.
_yaw_innov_magnitude_lpf = fmaxf(beta * _yaw_innov_magnitude_lpf,
fminf(fabsf(innovations.heading_innov), 2.0f * _yaw_innov_test_lim));
_hgt_innov_lpf = beta * _hgt_innov_lpf + alpha * constrain(innovations.vel_pos_innov[5], -_hgt_innov_spike_lim, _hgt_innov_lpf = beta * _hgt_innov_lpf + alpha * constrain(innovations.vel_pos_innov[5], -_hgt_innov_spike_lim,
_hgt_innov_spike_lim); _hgt_innov_spike_lim);
_vel_innov_preflt_fail = ((_vel_innov_lpf_ned.norm() > _vel_innov_test_lim)
|| (fabsf(_hgt_innov_lpf) > _hgt_innov_test_lim)); // check the yaw and horizontal velocity innovations
float vel_ne_innov_length = sqrtf(innovations.vel_pos_innov[0] * innovations.vel_pos_innov[0] +
innovations.vel_pos_innov[1] * innovations.vel_pos_innov[1]);
_preflt_horiz_fail = (_vel_ne_innov_lpf.norm() > _vel_innov_test_lim)
|| (vel_ne_innov_length > 2.0f * _vel_innov_test_lim)
|| (_yaw_innov_magnitude_lpf > _yaw_innov_test_lim);
// check the vertical velocity and position innovations
_preflt_vert_fail = (fabsf(_vel_d_innov_lpf) > _vel_innov_test_lim)
|| (fabsf(innovations.vel_pos_innov[2]) > 2.0f * _vel_innov_test_lim)
|| (fabsf(_hgt_innov_lpf) > _hgt_innov_test_lim);
// master pass-fail status
_preflt_fail = _preflt_horiz_fail || _preflt_vert_fail;
} else { } else {
_vel_innov_lpf_ned.zero(); _vel_ne_innov_lpf.zero();
_vel_d_innov_lpf = 0.0f;
_hgt_innov_lpf = 0.0f; _hgt_innov_lpf = 0.0f;
_vel_innov_preflt_fail = false; _preflt_horiz_fail = false;
_preflt_vert_fail = false;
_preflt_fail = false;
} }
if (_estimator_innovations_pub == nullptr) { if (_estimator_innovations_pub == nullptr) {