dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Prevent flow motion check false positives 

The previous implementation could false trigger if there was significant vibration below 200Hz during startup.
This commit is contained in:
Paul Riseborough 2018-05-09 07:47:17 +10:00
parent bdf5b3e003
commit bf902e5eca
3 changed files with 15 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
EKF/control.cpp
View File

@ -339,10 +339,12 @@ void Ekf::controlOpticalFlowFusion()
{ {
// Check if motion is un-suitable for use of optical flow // Check if motion is un-suitable for use of optical flow
if (!_control_status.flags.in_air) { if (!_control_status.flags.in_air) {
bool motion_is_excessive = ((_accel_mag_filt > 14.7f) // When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation
|| (_accel_mag_filt < 4.9f) float accel_norm = _accel_vec_filt.norm();
|| (_ang_rate_mag_filt > _params.flow_rate_max) bool motion_is_excessive = ((accel_norm > 14.7f) // accel greater than 1.5g
|| (_R_to_earth(2,2) < 0.866f)); || (accel_norm < 4.9f) // accel less than 0.5g
|| (_ang_rate_mag_filt > _params.flow_rate_max) // angular rate exceeds flow sensor limit
|| (_R_to_earth(2,2) < 0.866f)); // tilted more than 30 degrees
if (motion_is_excessive) { if (motion_is_excessive) {
_time_bad_motion_us = _imu_sample_delayed.time_us; _time_bad_motion_us = _imu_sample_delayed.time_us;

10
EKF/covariance.cpp
View File

@ -163,9 +163,13 @@ void Ekf::predictCovariance()
float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f); float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f);
// inhibit learning of imu acccel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected // inhibit learning of imu acccel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
float alpha = 1.0f - math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f); float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
_ang_rate_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), alpha * _ang_rate_mag_filt); float beta = 1.0f - alpha;
_accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), alpha * _accel_mag_filt); _ang_rate_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_mag_filt);
_accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_mag_filt);
_accel_vec_filt(0) = alpha * dt_inv * _imu_sample_delayed.delta_vel(0) + beta * _accel_vec_filt(0);
_accel_vec_filt(1) = alpha * dt_inv * _imu_sample_delayed.delta_vel(1) + beta * _accel_vec_filt(1);
_accel_vec_filt(2) = alpha * dt_inv * _imu_sample_delayed.delta_vel(2) + beta * _accel_vec_filt(2);
if (_ang_rate_mag_filt > _params.acc_bias_learn_gyr_lim if (_ang_rate_mag_filt > _params.acc_bias_learn_gyr_lim
|| _accel_mag_filt > _params.acc_bias_learn_acc_lim || _accel_mag_filt > _params.acc_bias_learn_acc_lim

3
EKF/ekf.h
View File

@ -402,7 +402,8 @@ private:
// variables used to inhibit accel bias learning // variables used to inhibit accel bias learning
bool _accel_bias_inhibit{false}; ///< true when the accel bias learning is being inhibited bool _accel_bias_inhibit{false}; ///< true when the accel bias learning is being inhibited
float _accel_mag_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (m/sec**2) Vector3f _accel_vec_filt{}; ///< acceleration vector after application of a low pass filter (m/sec**2)
float _accel_mag_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
float _ang_rate_mag_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec) float _ang_rate_mag_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec)
Vector3f _prev_dvel_bias_var; ///< saved delta velocity XYZ bias variances (m/sec)**2 Vector3f _prev_dvel_bias_var; ///< saved delta velocity XYZ bias variances (m/sec)**2