From 9c26e71ef6014bb983556778899c912f472083c4 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Thu, 8 Oct 2015 11:53:37 +0200 Subject: [PATCH] Added robust airspeed measurement --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 +++- src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 6 ++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 8fcaf29f47..88ba758cb3 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -870,7 +870,6 @@ void AttitudePositionEstimatorEKF::publishControlState() // XXX need to announce change of Z reference somehow elegantly _ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset; - //_local_pos.z_stable = _ekf->states[9]; _ctrl_state.pos_variance[0] = _ekf->P[7][7]; _ctrl_state.pos_variance[1] = _ekf->P[8][8]; @@ -883,6 +882,9 @@ void AttitudePositionEstimatorEKF::publishControlState() _ctrl_state.q[2] = _ekf->states[2]; _ctrl_state.q[3] = _ekf->states[3]; + /* Airspeed (Groundspeed - Windspeed) */ + _ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); + /* Attitude Rates */ _ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; _ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index b920cb9458..839918728c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -49,6 +49,8 @@ #define M_PI_F static_cast(M_PI) #endif +#define MIN_AIRSPEED_MEAS 5.0f + constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f; AttPosEKF::AttPosEKF() : @@ -1687,7 +1689,7 @@ void AttPosEKF::FuseAirspeed() // Calculate the predicted airspeed VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); // Perform fusion of True Airspeed measurement - if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) + if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > MIN_AIRSPEED_MEAS)) { // Calculate observation jacobians SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); @@ -2595,7 +2597,7 @@ void AttPosEKF::CovarianceInit() P[13][13] = sq(0.2f*dtIMU); //Wind velocities - P[14][14] = 0.0f; + P[14][14] = 0.01f; P[15][15] = P[14][14]; //Earth magnetic field