From c8e6d93cc0f9146d46db9de306bec170e39d025f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 15 Jul 2021 12:46:06 -0400 Subject: [PATCH] ekf2: backend apply code style (generated code still exempt) --- src/modules/ekf2/EKF/EKFGSF_yaw.cpp | 64 +++++-- src/modules/ekf2/EKF/EKFGSF_yaw.h | 26 +-- src/modules/ekf2/EKF/airspeed_fusion.cpp | 20 +- src/modules/ekf2/EKF/common.h | 8 +- src/modules/ekf2/EKF/control.cpp | 192 +++++++++++-------- src/modules/ekf2/EKF/covariance.cpp | 102 +++++----- src/modules/ekf2/EKF/drag_fusion.cpp | 11 ++ src/modules/ekf2/EKF/ekf_helper.cpp | 67 ++++--- src/modules/ekf2/EKF/estimator_interface.cpp | 4 +- src/modules/ekf2/EKF/estimator_interface.h | 5 +- src/modules/ekf2/EKF/gps_checks.cpp | 4 +- src/modules/ekf2/EKF/gps_yaw_fusion.cpp | 8 +- src/modules/ekf2/EKF/mag_control.cpp | 35 ++-- src/modules/ekf2/EKF/mag_fusion.cpp | 59 ++++-- src/modules/ekf2/EKF/optflow_fusion.cpp | 5 +- src/modules/ekf2/EKF/sideslip_fusion.cpp | 8 +- src/modules/ekf2/EKF/terrain_estimator.cpp | 10 +- 17 files changed, 387 insertions(+), 241 deletions(-) diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp index f382e9d246..ab6786f115 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp @@ -13,7 +13,7 @@ EKFGSF_yaw::EKFGSF_yaw() _ahrs_accel.zero(); } -void EKFGSF_yaw::update(const imuSample& imu_sample, +void EKFGSF_yaw::update(const imuSample &imu_sample, bool run_EKF, // set to true when flying or movement is suitable for yaw estimation float airspeed, // true airspeed used for centripetal accel compensation - set to 0 when not required. const Vector3f &imu_gyro_bias) // estimated rate gyro bias (rad/sec) @@ -39,11 +39,13 @@ void EKFGSF_yaw::update(const imuSample& imu_sample, const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f; const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f; const bool ok_to_align = (accel_norm_sq > sq(lower_accel_limit)) && (accel_norm_sq < sq(upper_accel_limit)); + if (ok_to_align) { initialiseEKFGSF(); ahrsAlignTilt(); _ahrs_ekf_gsf_tilt_aligned = true; } + return; } @@ -52,6 +54,7 @@ void EKFGSF_yaw::update(const imuSample& imu_sample, // AHRS prediction cycle for each model - this always runs _ahrs_accel_fusion_gain = ahrsCalcAccelGain(); + for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { predictEKF(model_index); } @@ -61,14 +64,18 @@ void EKFGSF_yaw::update(const imuSample& imu_sample, if (!_ekf_gsf_vel_fuse_started) { initialiseEKFGSF(); ahrsAlignYaw(); + // Initialise to gyro bias estimate from main filter because there could be a large // uncorrected rate gyro bias error about the gravity vector for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { _ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias; } + _ekf_gsf_vel_fuse_started = true; + } else { bool bad_update = false; + for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { // subsequent measurements are fused as direct state observations if (!updateEKF(model_index)) { @@ -81,24 +88,29 @@ void EKFGSF_yaw::update(const imuSample& imu_sample, // calculate weighting for each model assuming a normal distribution const float min_weight = 1e-5f; uint8_t n_weight_clips = 0; + for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { _model_weights(model_index) = gaussianDensity(model_index) * _model_weights(model_index); + if (_model_weights(model_index) < min_weight) { n_weight_clips++; _model_weights(model_index) = min_weight; } + total_weight += _model_weights(model_index); } // normalise the weighting function if (n_weight_clips < N_MODELS_EKFGSF) { _model_weights /= total_weight; + } else { // all weights have collapsed due to excessive innovation variances so reset filters initialiseEKFGSF(); } } } + } else if (_ekf_gsf_vel_fuse_started && !_run_ekf_gsf) { // wait to fly again _ekf_gsf_vel_fuse_started = false; @@ -108,18 +120,21 @@ void EKFGSF_yaw::update(const imuSample& imu_sample, // To avoid issues with angle wrapping, the yaw state is converted to a vector with length // equal to the weighting value before it is summed. Vector2f yaw_vector; + for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { yaw_vector(0) += _model_weights(model_index) * cosf(_ekf_gsf[model_index].X(2)); yaw_vector(1) += _model_weights(model_index) * sinf(_ekf_gsf[model_index].X(2)); } - _gsf_yaw = atan2f(yaw_vector(1),yaw_vector(0)); + + _gsf_yaw = atan2f(yaw_vector(1), yaw_vector(0)); // calculate a composite variance for the yaw state from a weighted average of the variance for each model // models with larger innovations are weighted less _gsf_yaw_variance = 0.0f; + for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { const float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw); - _gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2,2) + yaw_delta * yaw_delta); + _gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2, 2) + yaw_delta * yaw_delta); } // prevent the same velocity data being used more than once @@ -138,6 +153,7 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index) // Perform angular rate correction using accel data and reduce correction as accel magnitude moves away from 1 g (reduces drift when vehicle picked up and moved). // During fixed wing flight, compensate for centripetal acceleration assuming coordinated turns and X axis forward Vector3f tilt_correction; + if (_ahrs_accel_fusion_gain > 0.0f) { Vector3f accel = _ahrs_accel; @@ -158,13 +174,16 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index) // Gyro bias estimation constexpr float gyro_bias_limit = 0.05f; const float spinRate = ang_rate.length(); + if (spinRate < 0.175f) { _ahrs_ekf_gsf[model_index].gyro_bias -= tilt_correction * (_gyro_bias_gain * _delta_ang_dt); - _ahrs_ekf_gsf[model_index].gyro_bias = matrix::constrain(_ahrs_ekf_gsf[model_index].gyro_bias, -gyro_bias_limit, gyro_bias_limit); + _ahrs_ekf_gsf[model_index].gyro_bias = matrix::constrain(_ahrs_ekf_gsf[model_index].gyro_bias, -gyro_bias_limit, + gyro_bias_limit); } // delta angle from previous to current frame - const Vector3f delta_angle_corrected = _delta_ang + (tilt_correction - _ahrs_ekf_gsf[model_index].gyro_bias) * _delta_ang_dt; + const Vector3f delta_angle_corrected = _delta_ang + (tilt_correction - _ahrs_ekf_gsf[model_index].gyro_bias) * + _delta_ang_dt; // Apply delta angle to rotation matrix _ahrs_ekf_gsf[model_index].R = ahrsPredictRotMat(_ahrs_ekf_gsf[model_index].R, delta_angle_corrected); @@ -182,7 +201,7 @@ void EKFGSF_yaw::ahrsAlignTilt() const Vector3f down_in_bf = -_delta_vel.normalized(); // Calculate earth frame North axis unit vector rotated into body frame, orthogonal to 'down_in_bf' - const Vector3f i_vec_bf(1.0f,0.0f,0.0f); + const Vector3f i_vec_bf(1.0f, 0.0f, 0.0f); Vector3f north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf.dot(down_in_bf)); north_in_bf.normalize(); @@ -207,7 +226,7 @@ void EKFGSF_yaw::ahrsAlignYaw() { // Align yaw angle for each model for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) { - Dcmf& R = _ahrs_ekf_gsf[model_index].R; + Dcmf &R = _ahrs_ekf_gsf[model_index].R; const float yaw = wrap_pi(_ekf_gsf[model_index].X(2)); R = updateYawInRotMat(yaw, R); @@ -226,10 +245,10 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index) } // Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock - const Dcmf& R = _ahrs_ekf_gsf[model_index].R; + const Dcmf &R = _ahrs_ekf_gsf[model_index].R; _ekf_gsf[model_index].X(2) = shouldUse321RotationSequence(R) ? - getEuler321Yaw(R) : - getEuler312Yaw(R); + getEuler321Yaw(R) : + getEuler312Yaw(R); // calculate delta velocity in a horizontal front-right frame const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel; @@ -284,8 +303,9 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index) // constrain variances const float min_var = 1e-6f; + for (unsigned index = 0; index < 3; index++) { - _ekf_gsf[model_index].P(index,index) = fmaxf(_ekf_gsf[model_index].P(index,index),min_var); + _ekf_gsf[model_index].P(index, index) = fmaxf(_ekf_gsf[model_index].P(index, index), min_var); } } @@ -374,8 +394,9 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index) // constrain variances const float min_var = 1e-6f; + for (unsigned index = 0; index < 3; index++) { - _ekf_gsf[model_index].P(index,index) = fmaxf(_ekf_gsf[model_index].P(index,index),min_var); + _ekf_gsf[model_index].P(index, index) = fmaxf(_ekf_gsf[model_index].P(index, index), min_var); } // test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1] @@ -421,6 +442,7 @@ void EKFGSF_yaw::initialiseEKFGSF() memset(&_ekf_gsf, 0, sizeof(_ekf_gsf)); const float yaw_increment = 2.0f * _m_pi / (float)N_MODELS_EKFGSF; + for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) { // evenly space initial yaw estimates in the region between +-Pi _ekf_gsf[model_index].X(2) = -_m_pi + (0.5f * yaw_increment) + ((float)model_index * yaw_increment); @@ -428,11 +450,11 @@ void EKFGSF_yaw::initialiseEKFGSF() // take velocity states and corresponding variance from last measurement _ekf_gsf[model_index].X(0) = _vel_NE(0); _ekf_gsf[model_index].X(1) = _vel_NE(1); - _ekf_gsf[model_index].P(0,0) = sq(_vel_accuracy); - _ekf_gsf[model_index].P(1,1) = _ekf_gsf[model_index].P(0,0); + _ekf_gsf[model_index].P(0, 0) = sq(_vel_accuracy); + _ekf_gsf[model_index].P(1, 1) = _ekf_gsf[model_index].P(0, 0); // use half yaw interval for yaw uncertainty - _ekf_gsf[model_index].P(2,2) = sq(0.5f * yaw_increment); + _ekf_gsf[model_index].P(2, 2) = sq(0.5f * yaw_increment); } } @@ -444,19 +466,23 @@ float EKFGSF_yaw::gaussianDensity(const uint8_t model_index) const return _m_2pi_inv * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist); } -bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const +bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], + float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const { if (_ekf_gsf_vel_fuse_started) { *yaw_composite = _gsf_yaw; *yaw_variance = _gsf_yaw_variance; + for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) { yaw[model_index] = _ekf_gsf[model_index].X(2); innov_VN[model_index] = _ekf_gsf[model_index].innov(0); innov_VE[model_index] = _ekf_gsf[model_index].innov(1); weight[model_index] = _model_weights(model_index); } + return true; } + return false; } @@ -496,23 +522,25 @@ Matrix3f EKFGSF_yaw::ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g) // Renormalise rows for (uint8_t r = 0; r < 3; r++) { const float rowLengthSq = ret.row(r).norm_squared(); + if (rowLengthSq > FLT_EPSILON) { // Use linear approximation for inverse sqrt taking advantage of the row length being close to 1.0 const float rowLengthInv = 1.5f - 0.5f * rowLengthSq; ret.row(r) *= rowLengthInv; } - } + } return ret; } bool EKFGSF_yaw::getYawData(float *yaw, float *yaw_variance) const { - if(_ekf_gsf_vel_fuse_started) { + if (_ekf_gsf_vel_fuse_started) { *yaw = _gsf_yaw; *yaw_variance = _gsf_yaw_variance; return true; } + return false; } diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.h b/src/modules/ekf2/EKF/EKFGSF_yaw.h index 60d5556953..ebc28a77ed 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.h +++ b/src/modules/ekf2/EKF/EKFGSF_yaw.h @@ -29,16 +29,16 @@ using namespace estimator; class EKFGSF_yaw { public: - EKFGSF_yaw(); + EKFGSF_yaw(); - // Update Filter States - this should be called whenever new IMU data is available + // Update Filter States - this should be called whenever new IMU data is available void update(const imuSample &imu_sample, - bool run_EKF, // set to true when flying or movement is suitable for yaw estimation - float airspeed, // true airspeed used for centripetal accel compensation - set to 0 when not required. - const Vector3f &imu_gyro_bias); // estimated rate gyro bias (rad/sec) + bool run_EKF, // set to true when flying or movement is suitable for yaw estimation + float airspeed, // true airspeed used for centripetal accel compensation - set to 0 when not required. + const Vector3f &imu_gyro_bias); // estimated rate gyro bias (rad/sec) void setVelocity(const Vector2f &velocity, // NE velocity measurement (m/s) - float accuracy); // 1-sigma accuracy of velocity measurement (m/s) + float accuracy); // 1-sigma accuracy of velocity measurement (m/s) // get solution data for logging bool getLogData(float *yaw_composite, @@ -48,9 +48,9 @@ public: float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const; - // get yaw estimate and the corresponding variance - // return false if no yaw estimate available - bool getYawData(float *yaw, float *yaw_variance) const; + // get yaw estimate and the corresponding variance + // return false if no yaw estimate available + bool getYawData(float *yaw, float *yaw_variance) const; private: @@ -68,14 +68,14 @@ private: float _delta_vel_dt{}; // _delta_vel integration time interval (sec) float _true_airspeed{}; // true airspeed used for centripetal accel compensation (m/s) - struct _ahrs_ekf_gsf_struct{ + struct _ahrs_ekf_gsf_struct { Dcmf R; // matrix that rotates a vector from body to earth frame Vector3f gyro_bias; // gyro bias learned and used by the quaternion calculation bool aligned; // true when AHRS has been aligned float vel_NE[2]; // NE velocity vector from last GPS measurement (m/s) bool fuse_gps; // true when GPS should be fused on that frame float accel_dt; // time step used when generating _simple_accel_FR data (sec) - } _ahrs_ekf_gsf[N_MODELS_EKFGSF]{}; + } _ahrs_ekf_gsf[N_MODELS_EKFGSF] {}; bool _ahrs_ekf_gsf_tilt_aligned{}; // true the initial tilt alignment has been calculated float _ahrs_accel_fusion_gain{}; // gain from accel vector tilt error to rate gyro correction used by AHRS calculation @@ -99,13 +99,13 @@ private: // Declarations used by a bank of N_MODELS_EKFGSF EKFs - struct _ekf_gsf_struct{ + struct _ekf_gsf_struct { matrix::Vector3f X; // Vel North (m/s), Vel East (m/s), yaw (rad)s matrix::SquareMatrix P; // covariance matrix matrix::SquareMatrix S_inverse; // inverse of the innovation covariance matrix float S_det_inverse; // inverse of the innovation covariance matrix determinant matrix::Vector2f innov; // Velocity N,E innovation (m/s) - } _ekf_gsf[N_MODELS_EKFGSF]{}; + } _ekf_gsf[N_MODELS_EKFGSF] {}; bool _vel_data_updated{}; // true when velocity data has been updated bool _run_ekf_gsf{}; // true when operating condition is suitable for to run the GSF and EKF models and fuse velocity data diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index d9bad18211..868e8b46a9 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -55,7 +55,7 @@ void Ekf::fuseAirspeed() // Variance for true airspeed measurement - (m/sec)^2 const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * - math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); + math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); // determine if we need the sideslip fusion to correct states other than wind const bool update_wind_only = !_is_wind_dead_reckoning; @@ -65,11 +65,13 @@ void Ekf::fuseAirspeed() const float HK1 = ve - vwe; const float HK2 = ecl::powf(HK0, 2) + ecl::powf(HK1, 2) + ecl::powf(vd, 2); const float v_tas_pred = sqrtf(HK2); // predicted airspeed + //const float HK3 = powf(HK2, -1.0F/2.0F); if (v_tas_pred < 1.0f) { // calculation can be badly conditioned for very low airspeed values so don't fuse this time return; } + const float HK3 = 1.0f / v_tas_pred; const float HK4 = HK0*HK3; const float HK5 = HK1*HK3; @@ -86,11 +88,13 @@ void Ekf::fuseAirspeed() //const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS); // innovation variance - check for badly conditioned calculation - _airspeed_innov_var = (-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS); + _airspeed_innov_var = (-HK10 * HK14 + HK10 * HK9 + HK12 * HK13 - HK13 * HK15 + HK6 * HK7 * vd + R_TAS); + if (_airspeed_innov_var < R_TAS) { // // Reset the estimator covariance matrix // if we are getting aiding from other sources, warn and reset the wind states and covariances only - const char* action_string = nullptr; + const char *action_string = nullptr; + if (update_wind_only) { resetWindStates(); resetWindCovariance(); @@ -101,12 +105,14 @@ void Ekf::fuseAirspeed() _state.wind_vel.setZero(); action_string = "full"; } + ECL_ERR("airspeed badly conditioned - %s covariance reset", action_string); _fault_status.flags.bad_airspeed = true; return; } + const float HK16 = HK3 / _airspeed_innov_var; _fault_status.flags.bad_airspeed = false; @@ -119,6 +125,7 @@ void Ekf::fuseAirspeed() Hfusion.at<23>() = -HK5; Vector24f Kfusion; // Kalman gain vector + if (!update_wind_only) { // we have no other source of aiding, so use airspeed measurements to correct states for (unsigned row = 0; row <= 3; row++) { @@ -164,7 +171,8 @@ void Ekf::fuseAirspeed() void Ekf::get_true_airspeed(float *tas) const { - const float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2))); + const float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq( + _state.vel(2))); memcpy(tas, &tempvar, sizeof(float)); } @@ -174,8 +182,8 @@ void Ekf::get_true_airspeed(float *tas) const void Ekf::resetWindStates() { const float euler_yaw = shouldUse321RotationSequence(_R_to_earth) - ? getEuler321Yaw(_state.quat_nominal) - : getEuler312Yaw(_state.quat_nominal); + ? getEuler321Yaw(_state.quat_nominal) + : getEuler312Yaw(_state.quat_nominal); if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) { // estimate wind using zero sideslip assumption and airspeed measurement if airspeed available diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 933a0004bb..5b18bafc7d 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -99,7 +99,7 @@ struct imuSample { Vector3f delta_vel; ///< delta velocity in body frame (integrated accelerometer measurements) (m/sec) float delta_ang_dt; ///< delta angle integration period (sec) float delta_vel_dt; ///< delta velocity integration period (sec) - bool delta_vel_clipping[3]{}; ///< true (per axis) if this sample contained any accelerometer clipping + bool delta_vel_clipping[3] {}; ///< true (per axis) if this sample contained any accelerometer clipping }; struct gpsSample { @@ -300,8 +300,8 @@ struct parameters { float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data // vision position fusion - float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) - float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD) + float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) + float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD) // optical flow fusion float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec) @@ -487,7 +487,7 @@ union filter_control_status_u { uint32_t value; }; - // Mavlink bitmask containing state of estimator solution +// Mavlink bitmask containing state of estimator solution union ekf_solution_status { struct { uint16_t attitude : 1; ///< 0 - True if the attitude estimate is good diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 5eb8796c26..a74cfbebe5 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -59,7 +59,8 @@ void Ekf::controlFusionModes() _control_status.flags.tilt_align = true; // send alignment status message to the console - const char* height_source = nullptr; + const char *height_source = nullptr; + if (_control_status.flags.baro_hgt) { height_source = "baro"; @@ -76,9 +77,10 @@ void Ekf::controlFusionModes() height_source = "unknown"; } - if (height_source){ + + if (height_source) { ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)", - (unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length); + (unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length); } } } @@ -100,7 +102,9 @@ void Ekf::controlFusionModes() // if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value. // this is useful if there is a lot of interference on the sensor measurement. - if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) && (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps))) { + if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) && (_NED_origin_initialised + || PX4_ISFINITE(_mag_declination_gps))) { + const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0); _mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred); _control_status.flags.synthetic_mag_z = true; @@ -120,16 +124,16 @@ void Ekf::controlFusionModes() } { - // Get range data from buffer and check validity - const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress()); - _range_sensor.setDataReadiness(is_rng_data_ready); + // Get range data from buffer and check validity + const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress()); + _range_sensor.setDataReadiness(is_rng_data_ready); - // update range sensor angle parameters in case they have changed - _range_sensor.setPitchOffset(_params.rng_sens_pitch); - _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); - _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); + // update range sensor angle parameters in case they have changed + _range_sensor.setPitchOffset(_params.rng_sens_pitch); + _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); + _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); - _range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth); + _range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth); } if (_range_sensor.isDataHealthy()) { @@ -195,7 +199,9 @@ void Ekf::controlExternalVisionFusion() // if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames // needs to be calculated and the observations rotated into the EKF frame of reference - if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVVEL)) && !_control_status.flags.ev_yaw) { + if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS) + || (_params.fusion_mode & MASK_USE_EVVEL)) && !_control_status.flags.ev_yaw) { + // rotate EV measurements into the EKF Navigation frame calcExtVisRotMat(); } @@ -218,7 +224,9 @@ void Ekf::controlExternalVisionFusion() } // external vision yaw aiding selection logic - if (!_inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { + if (!_inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw + && _control_status.flags.tilt_align) { + // don't start using EV data unless data is arriving frequently if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) { if (resetYawToEv()) { @@ -242,6 +250,7 @@ void Ekf::controlExternalVisionFusion() // Use an incremental position fusion method for EV position data if GPS is also used if (_params.fusion_mode & MASK_USE_GPS) { _fuse_hpos_as_odom = true; + } else { _fuse_hpos_as_odom = false; } @@ -277,10 +286,12 @@ void Ekf::controlExternalVisionFusion() // use the absolute position Vector3f ev_pos_meas = _ev_sample_delayed.pos; Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar); + if (_params.fusion_mode & MASK_ROTATE_EV) { ev_pos_meas = _R_ev_to_ekf * ev_pos_meas; ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose(); } + _ev_pos_innov(0) = _state.pos(0) - ev_pos_meas(0); _ev_pos_innov(1) = _state.pos(1) - ev_pos_meas(1); @@ -326,7 +337,7 @@ void Ekf::controlExternalVisionFusion() ev_vel_innov_gates.setAll(fmaxf(_params.ev_vel_innov_gate, 1.0f)); - fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates,_last_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio); + fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates, _last_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio); fuseVerticalVelocity(_ev_vel_innov, ev_vel_innov_gates, _last_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio); } @@ -366,8 +377,7 @@ void Ekf::controlOpticalFlowFusion() const float delta_time_min = fmaxf(0.7f * _delta_time_of, 0.001f); const float delta_time_max = fminf(1.3f * _delta_time_of, 0.2f); - const bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min - && _flow_sample_delayed.dt <= delta_time_max; + const bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min && _flow_sample_delayed.dt <= delta_time_max; const bool is_body_rate_comp_available = calcOptFlowBodyRateComp(); if (is_quality_good @@ -405,19 +415,19 @@ void Ekf::controlOpticalFlowFusion() // Check if we are in-air and require optical flow to control position drift const bool is_flow_required = _control_status.flags.in_air - && (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently - || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow) - || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad + && (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently + || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow) + || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad // inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation const bool preflight_motion_not_ok = !_control_status.flags.in_air - && ((_imu_sample_delayed.time_us > (_time_good_motion_us + (uint64_t)1E5)) - || (_imu_sample_delayed.time_us < (_time_bad_motion_us + (uint64_t)5E6))); + && ((_imu_sample_delayed.time_us > (_time_good_motion_us + (uint64_t)1E5)) + || (_imu_sample_delayed.time_us < (_time_bad_motion_us + (uint64_t)5E6))); const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid(); _inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required) - || !_control_status.flags.tilt_align; + || !_control_status.flags.tilt_align; // Handle cases where we are using optical flow but we should not use it anymore if (_control_status.flags.opt_flow) { @@ -431,9 +441,8 @@ void Ekf::controlOpticalFlowFusion() // optical flow fusion mode selection logic if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user - && !_control_status.flags.opt_flow // we are not yet using flow data - && !_inhibit_flow_use) - { + && !_control_status.flags.opt_flow // we are not yet using flow data + && !_inhibit_flow_use) { // If the heading is valid and use is not inhibited , start using optical flow aiding if (_control_status.flags.yaw_align) { // set the flag and reset the fusion timeout @@ -472,7 +481,9 @@ void Ekf::controlOpticalFlowFusion() } } - } else if (_control_status.flags.opt_flow && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us + (uint64_t)10e6)) { + } else if (_control_status.flags.opt_flow + && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us + (uint64_t)10e6)) { + stopFlowFusion(); } } @@ -483,9 +494,9 @@ void Ekf::updateOnGroundMotionForOpticalFlowChecks() const float accel_norm = _accel_vec_filt.norm(); const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit - || (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit - || (_ang_rate_magnitude_filt > _flow_max_rate) // angular rate exceeds flow sensor limit - || (_R_to_earth(2,2) < cosf(math::radians(30.0f)))); // tilted excessively + || (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit + || (_ang_rate_magnitude_filt > _flow_max_rate) // angular rate exceeds flow sensor limit + || (_R_to_earth(2, 2) < cosf(math::radians(30.0f)))); // tilted excessively if (motion_is_excessive) { _time_bad_motion_us = _imu_sample_delayed.time_us; @@ -536,24 +547,29 @@ void Ekf::controlGpsFusion() } } else if (_control_status.flags.gps - && (!(_params.fusion_mode & MASK_USE_GPS) || !_control_status.flags.yaw_align)) { + && (!(_params.fusion_mode & MASK_USE_GPS) || !_control_status.flags.yaw_align)) { + stopGpsFusion(); } // Handle the case where we are using GPS and another source of aiding and GPS is failing checks if (_control_status.flags.gps && gps_checks_failing && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { stopGpsFusion(); + // Reset position state to external vision if we are going to use absolute values if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) { resetHorizontalPosition(); } + _warning_events.flags.gps_quality_poor = true; ECL_WARN("GPS quality poor - stopping use"); } // handle case where we are not currently using GPS, but need to align yaw angle using EKF-GSF before // we can start using GPS - const bool align_yaw_using_gsf = !_control_status.flags.gps && _do_ekfgsf_yaw_reset && isTimedOut(_ekfgsf_yaw_reset_time, 5000000); + const bool align_yaw_using_gsf = !_control_status.flags.gps && _do_ekfgsf_yaw_reset + && isTimedOut(_ekfgsf_yaw_reset_time, 5000000); + if (align_yaw_using_gsf) { if (resetYawToEKFGSF()) { _ekfgsf_yaw_reset_time = _time_last_imu; @@ -566,9 +582,9 @@ void Ekf::controlGpsFusion() // We are relying on aiding to constrain drift so after a specified time // with no aiding we need to do something bool do_vel_pos_reset = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_of_fuse, _params.reset_timeout_max); + && isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max) + && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) + && isTimedOut(_time_last_of_fuse, _params.reset_timeout_max); // We haven't had an absolute position fix for a longer time so need to do something do_vel_pos_reset = do_vel_pos_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max); @@ -604,6 +620,7 @@ void Ekf::controlGpsFusion() (_time_last_hor_pos_fuse > _time_last_on_ground_us); bool is_yaw_failure = false; + if ((recent_takeoff_nav_failure || inflight_nav_failure) && _time_last_hor_vel_fuse > 0) { // Do sanity check to see if the innovation failures is likely caused by a yaw angle error // by measuring the angle between the velocity estimate and the last velocity observation @@ -720,12 +737,14 @@ void Ekf::controlGpsFusion() stopGpsFusion(); _warning_events.flags.gps_data_stopped = true; ECL_WARN("GPS data stopped"); - } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { + + } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) + && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { // Handle the case where we are fusing another position source along GPS, // stop waiting for GPS after 1 s of lost signal stopGpsFusion(); _warning_events.flags.gps_data_stopped_using_alternate = true; - ECL_WARN("GPS data stopped, using only EV, OF or air data" ); + ECL_WARN("GPS data stopped, using only EV, OF or air data"); } } @@ -747,10 +766,10 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) const bool is_gps_yaw_data_intermittent = !isRecent(_time_last_gps_yaw_data, 2 * GPS_MAX_INTERVAL); const bool starting_conditions_passing = continuing_conditions_passing - && _control_status.flags.tilt_align - && gps_checks_passing - && !is_gps_yaw_data_intermittent - && !_gps_hgt_intermittent; + && _control_status.flags.tilt_align + && gps_checks_passing + && !is_gps_yaw_data_intermittent + && !_gps_hgt_intermittent; _time_last_gps_yaw_data = _time_last_imu; @@ -779,6 +798,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) // This could be a temporary issue, stop the fusion without declaring the sensor faulty stopGpsYawFusion(); } + // TODO: should we give a new reset credit when the fusion does not fail for some time? } @@ -788,7 +808,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) } } else { - if (starting_conditions_passing) { + if (starting_conditions_passing) { // Try to activate GPS yaw fusion if (resetYawToGps()) { _control_status.flags.yaw_align = true; @@ -798,8 +818,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) } } - } else if (_control_status.flags.gps_yaw - && isTimedOut(_time_last_gps_yaw_data, _params.reset_timeout_max)) { + } else if (_control_status.flags.gps_yaw && isTimedOut(_time_last_gps_yaw_data, _params.reset_timeout_max)) { // No yaw data in the message anymore. Stop until it comes back. stopGpsYawFusion(); } @@ -834,8 +853,8 @@ void Ekf::controlHeightSensorTimeouts() if (hgt_fusion_timeout || continuous_bad_accel_hgt) { bool request_height_reset = false; - const char* failing_height_source = nullptr; - const char* new_height_source = nullptr; + const char *failing_height_source = nullptr; + const char *new_height_source = nullptr; if (_control_status.flags.baro_hgt) { // check if GPS height is available @@ -847,7 +866,7 @@ void Ekf::controlHeightSensorTimeouts() // reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data const bool reset_to_gps = !_gps_hgt_intermittent && - ((gps_hgt_accurate && !prev_bad_vert_accel) || _baro_hgt_faulty); + ((gps_hgt_accurate && !prev_bad_vert_accel) || _baro_hgt_faulty); if (reset_to_gps) { // set height sensor health @@ -873,12 +892,11 @@ void Ekf::controlHeightSensorTimeouts() // check the baro height source for consistency and freshness const baroSample &baro_init = _baro_buffer.get_newest(); const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset); - const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9,9)) * sq(_params.baro_innov_gate); + const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9, 9)) * sq(_params.baro_innov_gate); // if baro data is acceptable and GPS data is inaccurate, reset height to baro const bool reset_to_baro = !_baro_hgt_faulty && - ((baro_data_consistent && !gps_hgt_accurate) || - _gps_hgt_intermittent); + ((baro_data_consistent && !gps_hgt_accurate) || _gps_hgt_intermittent); if (reset_to_baro) { startBaroHgtFusion(); @@ -918,8 +936,8 @@ void Ekf::controlHeightSensorTimeouts() failing_height_source = "ev"; new_height_source = "ev"; - // Fallback to rangefinder data if available } else if (_range_sensor.isHealthy()) { + // Fallback to rangefinder data if available setControlRangeHeight(); request_height_reset = true; failing_height_source = "ev"; @@ -957,6 +975,7 @@ void Ekf::checkVerticalAccelerationHealth() // Don't use stale innovation data. bool is_inertial_nav_falling = false; bool are_vertical_pos_and_vel_independant = false; + if (isRecent(_vert_pos_fuse_attempt_time_us, 1000000)) { if (isRecent(_vert_vel_fuse_time_us, 1000000)) { // If vertical position and velocity come from independent sensors then we can @@ -966,6 +985,7 @@ void Ekf::checkVerticalAccelerationHealth() are_vertical_pos_and_vel_independant = !(using_gps_for_both || using_ev_for_both); is_inertial_nav_falling |= _vert_vel_innov_ratio > _params.vert_innov_test_lim && _vert_pos_innov_ratio > 0.0f; is_inertial_nav_falling |= _vert_pos_innov_ratio > _params.vert_innov_test_lim && _vert_vel_innov_ratio > 0.0f; + } else { // only height sensing available is_inertial_nav_falling = _vert_pos_innov_ratio > _params.vert_innov_test_lim; @@ -977,17 +997,19 @@ void Ekf::checkVerticalAccelerationHealth() const bool is_clipping = _imu_sample_delayed.delta_vel_clipping[0] || _imu_sample_delayed.delta_vel_clipping[1] || _imu_sample_delayed.delta_vel_clipping[2]; - if (is_clipping &&_clip_counter < clip_count_limit) { + + if (is_clipping && _clip_counter < clip_count_limit) { _clip_counter++; + } else if (_clip_counter > 0) { _clip_counter--; } + const bool is_clipping_frequently = _clip_counter > 0; // if vertical velocity and position are independent and agree, then do not require evidence of clipping if // innovations are large - const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_clipping_frequently) && - is_inertial_nav_falling; + const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_clipping_frequently) && is_inertial_nav_falling; if (bad_vert_accel) { _time_bad_vert_accel = _time_last_imu; @@ -1091,21 +1113,27 @@ void Ekf::controlHeightFusion() if (!_gps_hgt_intermittent && _gps_checks_passed) { // GPS quality OK startGpsHgtFusion(); + } else if (!_baro_hgt_faulty) { // Use baro as a fallback startBaroHgtFusion(); } + } else if (_control_status.flags.baro_hgt && !do_range_aid && !_gps_hgt_intermittent && _gps_checks_passed) { // In baro fallback mode and GPS has recovered so start using it startGpsHgtFusion(); } + if (_control_status.flags.gps_hgt && _gps_data_ready) { fuse_height = true; + } else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) { fuse_height = true; + } else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { fuse_height = true; } + break; case VDIST_SENSOR_EV: @@ -1114,18 +1142,21 @@ void Ekf::controlHeightFusion() if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) { fuse_height = true; setControlEVHeight(); + if (!_control_status_prev.flags.rng_hgt) { - resetHeight(); + resetHeight(); } } if (_control_status.flags.ev_hgt && _ev_data_ready) { - fuse_height = true; + fuse_height = true; + } else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) { - fuse_height = true; + fuse_height = true; + } else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { - fuse_height = true; - } + fuse_height = true; + } break; } @@ -1174,9 +1205,10 @@ void Ekf::controlHeightFusion() } } } + // fuse height information - fuseVerticalPosition(_baro_hgt_innov,baro_hgt_innov_gate, - baro_hgt_obs_var, _baro_hgt_innov_var,_baro_hgt_test_ratio); + fuseVerticalPosition(_baro_hgt_innov, baro_hgt_innov_gate, + baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio); } else if (_control_status.flags.gps_hgt) { Vector2f gps_hgt_innov_gate; @@ -1187,23 +1219,23 @@ void Ekf::controlHeightFusion() // innovation gate size gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f); // fuse height information - fuseVerticalPosition(_gps_pos_innov,gps_hgt_innov_gate, - gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio); + fuseVerticalPosition(_gps_pos_innov, gps_hgt_innov_gate, + gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio); } else if (_control_status.flags.rng_hgt) { Vector2f rng_hgt_innov_gate; Vector3f rng_hgt_obs_var; // use range finder with tilt correction _rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(), - _params.rng_gnd_clearance)) - _hgt_sensor_offset; + _params.rng_gnd_clearance)) - _hgt_sensor_offset; // observation variance - user parameter defined rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f); // innovation gate size rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f); // fuse height information - fuseVerticalPosition(_rng_hgt_innov,rng_hgt_innov_gate, - rng_hgt_obs_var, _rng_hgt_innov_var,_rng_hgt_test_ratio); + fuseVerticalPosition(_rng_hgt_innov, rng_hgt_innov_gate, + rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio); } else if (_control_status.flags.ev_hgt) { Vector2f ev_hgt_innov_gate; @@ -1215,8 +1247,8 @@ void Ekf::controlHeightFusion() // innovation gate size ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f); // fuse height information - fuseVerticalPosition(_ev_pos_innov,ev_hgt_innov_gate, - ev_hgt_obs_var, _ev_pos_innov_var,_ev_pos_test_ratio); + fuseVerticalPosition(_ev_pos_innov, ev_hgt_innov_gate, + ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio); } } } @@ -1257,6 +1289,7 @@ void Ekf::controlAirDataFusion() // If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates const bool airspeed_timed_out = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6); const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6); + if (_control_status.flags.wind && (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)))) { _control_status.flags.wind = false; @@ -1291,6 +1324,7 @@ void Ekf::controlBetaFusion() // Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fuson had been enabled externally: const bool beta_fusion_time_triggered = isTimedOut(_time_last_beta_fuse, _params.beta_avg_ft_us); + if (beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) { @@ -1315,15 +1349,16 @@ void Ekf::controlDragFusion() !_using_synthetic_position && _control_status.flags.in_air && !_mag_inhibit_yaw_reset_req) { - if (!_control_status.flags.wind) { - // reset the wind states and covariances when starting drag accel fusion - _control_status.flags.wind = true; - resetWindStates(); - resetWindCovariance(); - } else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) { - fuseDrag(); - } + if (!_control_status.flags.wind) { + // reset the wind states and covariances when starting drag accel fusion + _control_status.flags.wind = true; + resetWindStates(); + resetWindCovariance(); + + } else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) { + fuseDrag(); + } } } @@ -1355,6 +1390,7 @@ void Ekf::controlFakePosFusion() } } + _time_last_fake_pos = _time_last_imu; Vector3f fake_pos_obs_var; @@ -1376,7 +1412,7 @@ void Ekf::controlFakePosFusion() const Vector2f fake_pos_innov_gate(3.0f, 3.0f); fuseHorizontalPosition(_gps_pos_innov, fake_pos_innov_gate, fake_pos_obs_var, - _gps_pos_innov_var, _gps_pos_test_ratio, true); + _gps_pos_innov_var, _gps_pos_test_ratio, true); } } else { @@ -1398,7 +1434,7 @@ void Ekf::controlAuxVelFusion() _last_vel_obs_var = _aux_vel_innov_var; fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar, - _aux_vel_innov_var, _aux_vel_test_ratio); + _aux_vel_innov_var, _aux_vel_test_ratio); // Can be enabled after bit for this is added to EKF_AID_MASK // fuseVerticalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar, diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index c73471ebcd..978db13cbe 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -140,7 +140,7 @@ void Ekf::predictCovariance() _accel_vec_filt = alpha * dt_inv * _imu_sample_delayed.delta_vel + beta * _accel_vec_filt; const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim - || _accel_magnitude_filt > _params.acc_bias_learn_acc_lim; + || _accel_magnitude_filt > _params.acc_bias_learn_acc_lim; const bool do_inhibit_all_axes = (_params.fusion_mode & MASK_INHIBIT_ACC_BIAS) || is_manoeuvre_level_high @@ -172,7 +172,7 @@ void Ekf::predictCovariance() // Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned float mag_I_sig; - if (_control_status.flags.mag_3D && (P(16,16) + P(17,17) + P(18,18)) < 0.1f) { + if (_control_status.flags.mag_3D && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) { mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f); } else { @@ -182,7 +182,7 @@ void Ekf::predictCovariance() // Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned float mag_B_sig; - if (_control_status.flags.mag_3D && (P(19,19) + P(20,20) + P(21,21)) < 0.1f) { + if (_control_status.flags.mag_3D && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) { mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f); } else { @@ -210,15 +210,15 @@ void Ekf::predictCovariance() // These are kinematic states and their error growth is controlled separately by the IMU noise variances // delta angle bias states - process_noise.slice<3,1>(10,0) = sq(d_ang_bias_sig); + process_noise.slice<3, 1>(10, 0) = sq(d_ang_bias_sig); // delta_velocity bias states - process_noise.slice<3,1>(13,0) = sq(d_vel_bias_sig); + process_noise.slice<3, 1>(13, 0) = sq(d_vel_bias_sig); // earth frame magnetic field states - process_noise.slice<3,1>(16,0) = sq(mag_I_sig); + process_noise.slice<3, 1>(16, 0) = sq(mag_I_sig); // body frame magnetic field states - process_noise.slice<3,1>(19,0) = sq(mag_B_sig); + process_noise.slice<3, 1>(19, 0) = sq(mag_B_sig); // wind velocity states - process_noise.slice<2,1>(22,0) = sq(wind_vel_sig); + process_noise.slice<2, 1>(22, 0) = sq(wind_vel_sig); // assign IMU noise variances // inputs to the system are 3 delta angles and 3 delta velocities @@ -246,11 +246,13 @@ void Ekf::predictCovariance() dvxVar = sq(dt * BADACC_BIAS_PNOISE); _fault_status.flags.bad_acc_clipping = true; } + // delta velocity Y: increase process noise if sample contained any Y axis clipping if (_imu_sample_delayed.delta_vel_clipping[1]) { dvyVar = sq(dt * BADACC_BIAS_PNOISE); _fault_status.flags.bad_acc_clipping = true; } + // delta velocity Z: increase process noise if sample contained any Z axis clipping if (_imu_sample_delayed.delta_vel_clipping[2]) { dvzVar = sq(dt * BADACC_BIAS_PNOISE); @@ -585,9 +587,9 @@ void Ekf::predictCovariance() // process noise contribution for delta angle states can be very small compared to // the variances, therefore use algorithm to minimise numerical error - for (unsigned i = 10; i <=12; i++) { - const int index = i-10; - nextP(i,i) = kahanSummation(nextP(i,i), process_noise(i), _delta_angle_bias_var_accum(index)); + for (unsigned i = 10; i <= 12; i++) { + const int index = i - 10; + nextP(i, i) = kahanSummation(nextP(i, i), process_noise(i), _delta_angle_bias_var_accum(index)); } if (!_accel_bias_inhibit[0]) { @@ -610,7 +612,7 @@ void Ekf::predictCovariance() // add process noise that is not from the IMU // process noise contribution for delta velocity states can be very small compared to // the variances, therefore use algorithm to minimise numerical error - nextP(13,13) = kahanSummation(nextP(13,13), process_noise(13), _delta_vel_bias_var_accum(0)); + nextP(13, 13) = kahanSummation(nextP(13, 13), process_noise(13), _delta_vel_bias_var_accum(0)); } else { nextP.uncorrelateCovarianceSetVariance<1>(13, _prev_dvel_bias_var(0)); @@ -640,7 +642,7 @@ void Ekf::predictCovariance() // add process noise that is not from the IMU // process noise contribution for delta velocity states can be very small compared to // the variances, therefore use algorithm to minimise numerical error - nextP(14,14) = kahanSummation(nextP(14,14), process_noise(14), _delta_vel_bias_var_accum(1)); + nextP(14, 14) = kahanSummation(nextP(14, 14), process_noise(14), _delta_vel_bias_var_accum(1)); } else { nextP.uncorrelateCovarianceSetVariance<1>(14, _prev_dvel_bias_var(1)); @@ -670,7 +672,7 @@ void Ekf::predictCovariance() // add process noise that is not from the IMU // process noise contribution for delta velocity states can be very small compared to // the variances, therefore use algorithm to minimise numerical error - nextP(15,15) = kahanSummation(nextP(15,15), process_noise(15), _delta_vel_bias_var_accum(2)); + nextP(15, 15) = kahanSummation(nextP(15, 15), process_noise(15), _delta_vel_bias_var_accum(2)); } else { nextP.uncorrelateCovarianceSetVariance<1>(15, _prev_dvel_bias_var(2)); @@ -802,7 +804,7 @@ void Ekf::predictCovariance() // add process noise that is not from the IMU for (unsigned i = 16; i <= 21; i++) { - nextP(i,i) += process_noise(i); + nextP(i, i) += process_noise(i); } } @@ -862,18 +864,18 @@ void Ekf::predictCovariance() // add process noise that is not from the IMU for (unsigned i = 22; i <= 23; i++) { - nextP(i,i) += process_noise(i); + nextP(i, i) += process_noise(i); } } // stop position covariance growth if our total position variance reaches 100m // this can happen if we lose gps for some time - if ((P(7,7) + P(8,8)) > 1e4f) { + if ((P(7, 7) + P(8, 8)) > 1e4f) { for (uint8_t i = 7; i <= 8; i++) { for (uint8_t j = 0; j < _k_num_states; j++) { - nextP(i,j) = P(i,j); - nextP(j,i) = P(j,i); + nextP(i, j) = P(i, j); + nextP(j, i) = P(j, i); } } } @@ -881,13 +883,13 @@ void Ekf::predictCovariance() // covariance matrix is symmetrical, so copy upper half to lower half for (unsigned row = 1; row < _k_num_states; row++) { for (unsigned column = 0 ; column < row; column++) { - P(row,column) = P(column,row) = nextP(column,row); + P(row, column) = P(column, row) = nextP(column, row); } } // copy variances (diagonals) for (unsigned i = 0; i < _k_num_states; i++) { - P(i,i) = nextP(i,i); + P(i, i) = nextP(i, i); } // fix gross errors in the covariance matrix and ensure rows and @@ -915,22 +917,22 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) for (int i = 0; i <= 3; i++) { // quaternion states - P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[0]); + P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[0]); } for (int i = 4; i <= 6; i++) { // NED velocity states - P(i,i) = math::constrain(P(i,i), 1e-6f, P_lim[1]); + P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[1]); } for (int i = 7; i <= 9; i++) { // NED position states - P(i,i) = math::constrain(P(i,i), 1e-6f, P_lim[2]); + P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[2]); } for (int i = 10; i <= 12; i++) { // gyro bias states - P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[3]); + P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[3]); } // force symmetry on the quaternion, velocity and position state covariances @@ -953,10 +955,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // Skip the check for the inhibited axis continue; } - if (P(stateIndex,stateIndex) > maxStateVar) { - maxStateVar = P(stateIndex,stateIndex); - } else if (P(stateIndex,stateIndex) < minSafeStateVar) { + if (P(stateIndex, stateIndex) > maxStateVar) { + maxStateVar = P(stateIndex, stateIndex); + + } else if (P(stateIndex, stateIndex) < minSafeStateVar) { resetRequired = true; } } @@ -972,7 +975,9 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // Skip the check for the inhibited axis continue; } - P(stateIndex,stateIndex) = math::constrain(P(stateIndex,stateIndex), minAllowedStateVar, sq(0.1f * CONSTANTS_ONE_G * _dt_ekf_avg)); + + P(stateIndex, stateIndex) = math::constrain(P(stateIndex, stateIndex), minAllowedStateVar, + sq(0.1f * CONSTANTS_ONE_G * _dt_ekf_avg)); } // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero @@ -987,12 +992,12 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim - && ( (down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps) - || (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel) ) - && ( (down_dvel_bias * _gps_pos_innov(2) < 0.0f && _control_status.flags.gps_hgt) - || (down_dvel_bias * _baro_hgt_innov(2) < 0.0f && _control_status.flags.baro_hgt) - || (down_dvel_bias * _rng_hgt_innov(2) < 0.0f && _control_status.flags.rng_hgt) - || (down_dvel_bias * _ev_pos_innov(2) < 0.0f && _control_status.flags.ev_hgt) ) ); + && ((down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps) + || (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel)) + && ((down_dvel_bias * _gps_pos_innov(2) < 0.0f && _control_status.flags.gps_hgt) + || (down_dvel_bias * _baro_hgt_innov(2) < 0.0f && _control_status.flags.baro_hgt) + || (down_dvel_bias * _rng_hgt_innov(2) < 0.0f && _control_status.flags.rng_hgt) + || (down_dvel_bias * _ev_pos_innov(2) < 0.0f && _control_status.flags.ev_hgt))); // record the pass/fail if (!bad_acc_bias) { @@ -1028,11 +1033,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) } else { // constrain variances for (int i = 16; i <= 18; i++) { - P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[5]); + P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[5]); } for (int i = 19; i <= 21; i++) { - P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[6]); + P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[6]); } // force symmetry @@ -1050,7 +1055,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) } else { // constrain variances for (int i = 22; i <= 23; i++) { - P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[7]); + P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[7]); } // force symmetry @@ -1062,14 +1067,17 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected -bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f& KHP) { +bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP) +{ bool healthy = true; + for (int i = 0; i < _k_num_states; i++) { if (P(i, i) < KHP(i, i)) { P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); healthy = false; } } + return healthy; } @@ -1079,7 +1087,8 @@ void Ekf::resetMagRelatedCovariances() resetMagCov(); } -void Ekf::resetQuatCov(){ +void Ekf::resetQuatCov() +{ zeroQuatCov(); // define the initial angle uncertainty as variances for a rotation vector @@ -1152,14 +1161,15 @@ void Ekf::resetWindCovariance() // it is safer to remove all existing correlations to other states at this time P.uncorrelateCovarianceSetVariance<2>(22, 0.0f); - P(22,22) = R_TAS*sq(cos_yaw) + R_yaw*sq(-Wx*sin_yaw - Wy*cos_yaw) + initial_wind_var_body_y*sq(sin_yaw); - P(22,23) = R_TAS*sin_yaw*cos_yaw + R_yaw*(-Wx*sin_yaw - Wy*cos_yaw)*(Wx*cos_yaw - Wy*sin_yaw) - initial_wind_var_body_y*sin_yaw*cos_yaw; - P(23,22) = P(22,23); - P(23,23) = R_TAS*sq(sin_yaw) + R_yaw*sq(Wx*cos_yaw - Wy*sin_yaw) + initial_wind_var_body_y*sq(cos_yaw); + P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw); + P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) - + initial_wind_var_body_y * sin_yaw * cos_yaw; + P(23, 22) = P(22, 23); + P(23, 23) = R_TAS * sq(sin_yaw) + R_yaw * sq(Wx * cos_yaw - Wy * sin_yaw) + initial_wind_var_body_y * sq(cos_yaw); // Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed - P(22,22) += P(4,4); - P(23,23) += P(5,5); + P(22, 22) += P(4, 4); + P(23, 23) += P(5, 5); } else { // without airspeed, start with a small initial uncertainty to improve the initial estimate diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index 754a81e794..6435faa69b 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -98,8 +98,10 @@ void Ekf::fuseDrag() // equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed // parallel to the rotor disc and mass flow through the rotor disc. float pred_acc = 0.0f; // predicted drag acceleration + if (axis_index == 0) { float Kacc; // Derivative of specific force wrt airspeed + if (using_mcoef && using_bcoef_x) { // Use a combination of bluff body and propeller momentum drag const float bcoef_inv = 1.0f / _params.bcoef_x; @@ -108,10 +110,12 @@ void Ekf::fuseDrag() const float airspeed = (_params.bcoef_x / rho) * (- mcoef_corrrected + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc))); Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected); pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(0)) * drag_sign - rel_wind_body(0) * mcoef_corrrected; + } else if (using_mcoef) { // Use propeller momentum drag only Kacc = fmaxf(1e-1f, mcoef_corrrected); pred_acc = - rel_wind_body(0) * mcoef_corrrected; + } else if (using_bcoef_x) { // Use bluff body drag only // The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic @@ -120,6 +124,7 @@ void Ekf::fuseDrag() const float bcoef_inv = 1.0f / _params.bcoef_x; Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed); pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(0)) * drag_sign; + } else { // skip this axis continue; @@ -165,6 +170,7 @@ void Ekf::fuseDrag() if (_drag_innov_var(0) < R_ACC) { return; } + const float HK32 = Kacc / _drag_innov_var(0); // Observation Jacobians @@ -208,6 +214,7 @@ void Ekf::fuseDrag() } else if (axis_index == 1) { float Kacc; // Derivative of specific force wrt airspeed + if (using_mcoef && using_bcoef_y) { // Use a combination of bluff body and propeller momentum drag const float bcoef_inv = 1.0f / _params.bcoef_y; @@ -216,10 +223,12 @@ void Ekf::fuseDrag() const float airspeed = (_params.bcoef_y / rho) * (- mcoef_corrrected + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc))); Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected); pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(1)) * drag_sign - rel_wind_body(1) * mcoef_corrrected; + } else if (using_mcoef) { // Use propeller momentum drag only Kacc = fmaxf(1e-1f, mcoef_corrrected); pred_acc = - rel_wind_body(1) * mcoef_corrrected; + } else if (using_bcoef_y) { // Use bluff body drag only // The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic @@ -228,6 +237,7 @@ void Ekf::fuseDrag() const float bcoef_inv = 1.0f / _params.bcoef_y; Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed); pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(1)) * drag_sign; + } else { // nothing more to do return; @@ -273,6 +283,7 @@ void Ekf::fuseDrag() // calculation is badly conditioned return; } + const float HK32 = Kacc / _drag_innov_var(1); // Observation Jacobians diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 5449bf611c..dfb513396f 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -262,12 +262,14 @@ void Ekf::resetHeight() if (_control_status.flags.rng_hgt) { // a fallback from any other height source to rangefinder happened - if(!_control_status_prev.flags.rng_hgt) { + if (!_control_status_prev.flags.rng_hgt) { if (_control_status.flags.in_air && isTerrainEstimateValid()) { - _hgt_sensor_offset = _terrain_vpos; + _hgt_sensor_offset = _terrain_vpos; + } else if (_control_status.flags.in_air) { - _hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2); + _hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2); + } else { _hgt_sensor_offset = _params.rng_gnd_clearance; } @@ -729,6 +731,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons // reset altitude _gps_alt_ref = altitude; resetVerticalPositionTo(_gps_alt_ref - current_alt); + } else { // reset altitude _gps_alt_ref = altitude; @@ -1042,9 +1045,9 @@ void Ekf::uncorrelateQuatFromOtherStates() void Ekf::update_deadreckoning_status() { const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel) - && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) - || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max) - || isRecent(_time_last_delpos_fuse, _params.no_aid_timeout_max)); + && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) + || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max) + || isRecent(_time_last_delpos_fuse, _params.no_aid_timeout_max)); const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max); const bool airDataAiding = _control_status.flags.wind && isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) && @@ -1316,8 +1319,7 @@ void Ekf::updateBaroHgtOffset() const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f); // apply a 10 second first order low pass filter to baro offset - const float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt + _state.pos(2) - - _baro_hgt_offset); + const 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); } } @@ -1340,19 +1342,22 @@ Vector3f Ekf::getVisionVelocityInEkfFrame() const const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; // rotate measurement into correct earth frame if required - switch(_ev_sample_delayed.vel_frame) { - case velocity_frame_t::BODY_FRAME_FRD: - vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body); - break; - case velocity_frame_t::LOCAL_FRAME_FRD: - const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; - if (_params.fusion_mode & MASK_ROTATE_EV) - { - vel = _R_ev_to_ekf *_ev_sample_delayed.vel - vel_offset_earth; - } else { - vel = _ev_sample_delayed.vel - vel_offset_earth; - } - break; + switch (_ev_sample_delayed.vel_frame) { + case velocity_frame_t::BODY_FRAME_FRD: + vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body); + break; + + case velocity_frame_t::LOCAL_FRAME_FRD: + const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; + + if (_params.fusion_mode & MASK_ROTATE_EV) { + vel = _R_ev_to_ekf * _ev_sample_delayed.vel - vel_offset_earth; + + } else { + vel = _ev_sample_delayed.vel - vel_offset_earth; + } + + break; } return vel; @@ -1363,17 +1368,17 @@ Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const Matrix3f ev_vel_cov = _ev_sample_delayed.velCov; // rotate measurement into correct earth frame if required - switch(_ev_sample_delayed.vel_frame) { - case velocity_frame_t::BODY_FRAME_FRD: - ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose(); - break; + switch (_ev_sample_delayed.vel_frame) { + case velocity_frame_t::BODY_FRAME_FRD: + ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose(); + break; - case velocity_frame_t::LOCAL_FRAME_FRD: - if(_params.fusion_mode & MASK_ROTATE_EV) - { - ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose(); - } - break; + case velocity_frame_t::LOCAL_FRAME_FRD: + if (_params.fusion_mode & MASK_ROTATE_EV) { + ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose(); + } + + break; } return ev_vel_cov.diag(); diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 6562d9a73b..ca05f8a401 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -520,7 +520,9 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate) _obs_buffer_length = math::min(_obs_buffer_length, _imu_buffer_length); - if (!_imu_buffer.allocate(_imu_buffer_length) || !_output_buffer.allocate(_imu_buffer_length) || !_output_vert_buffer.allocate(_imu_buffer_length)) { + if (!_imu_buffer.allocate(_imu_buffer_length) || !_output_buffer.allocate(_imu_buffer_length) + || !_output_vert_buffer.allocate(_imu_buffer_length)) { + printBufferAllocationFailed("IMU and output"); return false; } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index d9ba423043..f7421752de 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -113,6 +113,7 @@ public: } else { _time_last_in_air = _time_last_imu; } + _control_status.flags.in_air = in_air; } @@ -248,8 +249,8 @@ public: // Getter for the baro sample on the delayed time horizon const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; } - const bool& global_origin_valid() const { return _NED_origin_initialised; } - const map_projection_reference_s& global_origin() const { return _pos_ref; } + const bool &global_origin_valid() const { return _NED_origin_initialised; } + const map_projection_reference_s &global_origin() const { return _pos_ref; } void print_status(); diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index 35c4454361..a284ef94f6 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -215,8 +215,8 @@ bool Ekf::gps_is_good(const gps_message &gps) // Check the magnitude of the filtered horizontal GPS velocity const Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel_ned.xy()), - -10.0f * _params.req_hdrift, - 10.0f * _params.req_hdrift); + -10.0f * _params.req_hdrift, + 10.0f * _params.req_hdrift); _gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef); _gps_drift_metrics[2] = _gps_velNE_filt.norm(); _gps_check_fail_status.flags.hspeed = (_gps_drift_metrics[2] > _params.req_hdrift); diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index dd18df4c1a..a0c66f2ec3 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -66,7 +66,7 @@ void Ekf::fuseGpsYaw() } // calculate predicted antenna yaw angle - const float predicted_hdg = atan2f(ant_vec_ef(1),ant_vec_ef(0)); + const float predicted_hdg = atan2f(ant_vec_ef(1), ant_vec_ef(0)); // using magnetic heading process noise // TODO extend interface to use yaw uncertainty provided by GPS if available @@ -83,6 +83,7 @@ void Ekf::fuseGpsYaw() const float HK7 = ecl::powf(q0, 2) - ecl::powf(q3, 2); const float HK8 = HK4*(HK5 - HK6 + HK7); const float HK9 = HK3 - HK8; + if (fabsf(HK9) < 1e-3f) { return; } @@ -100,6 +101,7 @@ void Ekf::fuseGpsYaw() const float HK18 = 2/HK17; // const float HK19 = 1.0F/(-HK3 + HK8); const float HK19_inverse = -HK3 + HK8; + if (fabsf(HK19_inverse) < 1e-6f) { return; } @@ -122,7 +124,7 @@ void Ekf::fuseGpsYaw() _heading_innov_var = (-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW); if (_heading_innov_var < R_YAW) { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned + // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned _fault_status.flags.bad_hdg = true; // we reinitialise the covariance matrix and abort this fusion step @@ -132,7 +134,7 @@ void Ekf::fuseGpsYaw() } _fault_status.flags.bad_hdg = false; - const float HK32 = HK18/_heading_innov_var; + const float HK32 = HK18 / _heading_innov_var; // calculate the innovation and define the innovation gate const float innov_gate = math::max(_params.heading_innov_gate, 1.0f); diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 500405536f..8d1e6a0e17 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -50,11 +50,11 @@ void Ekf::controlMagFusion() // yaw fusion is run selectively to enable yaw gyro bias learning when stationary on // ground and to prevent uncontrolled yaw variance growth if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) { - if (noOtherYawAidingThanMag()) - { + if (noOtherYawAidingThanMag()) { _is_yaw_fusion_inhibited = true; fuseHeading(); } + return; } @@ -81,12 +81,14 @@ void Ekf::controlMagFusion() // there are large external disturbances or the more accurate 3-axis fusion switch (_params.mag_fusion_type) { default: + /* fallthrough */ case MAG_FUSE_TYPE_AUTO: selectMagAuto(); break; case MAG_FUSE_TYPE_INDOOR: + /* fallthrough */ case MAG_FUSE_TYPE_HEADING: startMagHdgFusion(); @@ -169,8 +171,12 @@ void Ekf::runInAirYawReset() if (_mag_yaw_reset_req && isYawResetAuthorized()) { bool has_realigned_yaw = false; - if (canRealignYawUsingGps()) { has_realigned_yaw = realignYawGPS(); } - else if (canResetMagHeading()) { has_realigned_yaw = resetMagHeading(_mag_lpf.getState()); } + if (canRealignYawUsingGps()) { + has_realigned_yaw = realignYawGPS(); + + } else if (canResetMagHeading()) { + has_realigned_yaw = resetMagHeading(_mag_lpf.getState()); + } if (has_realigned_yaw) { _mag_yaw_reset_req = false; @@ -266,6 +272,7 @@ void Ekf::checkMagDeclRequired() void Ekf::checkMagInhibition() { _is_yaw_fusion_inhibited = shouldInhibitMag(); + if (!_is_yaw_fusion_inhibited) { _mag_use_not_inhibit_us = _imu_sample_delayed.time_us; } @@ -286,8 +293,8 @@ bool Ekf::shouldInhibitMag() const const bool user_selected = (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR); const bool heading_not_required_for_navigation = !_control_status.flags.gps - && !_control_status.flags.ev_pos - && !_control_status.flags.ev_vel; + && !_control_status.flags.ev_pos + && !_control_status.flags.ev_vel; return (user_selected && heading_not_required_for_navigation) || isStrongMagneticDisturbance(); @@ -297,8 +304,8 @@ void Ekf::checkMagFieldStrength() { if (_params.check_mag_strength) { _control_status.flags.mag_field_disturbed = _NED_origin_initialised - ? !isMeasuredMatchingGpsMagStrength() - : !isMeasuredMatchingAverageMagStrength(); + ? !isMeasuredMatchingGpsMagStrength() + : !isMeasuredMatchingAverageMagStrength(); } else { _control_status.flags.mag_field_disturbed = false; @@ -323,13 +330,14 @@ bool Ekf::isMeasuredMatchingAverageMagStrength() const bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, const float gate) { return (measured >= expected - gate) - && (measured <= expected + gate); + && (measured <= expected + gate); } void Ekf::runMagAndMagDeclFusions() { if (_control_status.flags.mag_3D) { run3DMagAndDeclFusions(); + } else if (_control_status.flags.mag_hdg) { fuseHeading(); } @@ -351,6 +359,7 @@ void Ekf::run3DMagAndDeclFusions() // declination angle at a higher uncertainty to allow some learning of // declination angle over time. fuseMag(); + if (_control_status.flags.mag_dec) { fuseDeclination(0.5f); } @@ -359,10 +368,10 @@ void Ekf::run3DMagAndDeclFusions() bool Ekf::otherHeadingSourcesHaveStopped() { - // detect rising edge of noOtherYawAidingThanMag() - bool result = noOtherYawAidingThanMag() && _non_mag_yaw_aiding_running_prev; + // detect rising edge of noOtherYawAidingThanMag() + bool result = noOtherYawAidingThanMag() && _non_mag_yaw_aiding_running_prev; - _non_mag_yaw_aiding_running_prev = !noOtherYawAidingThanMag(); + _non_mag_yaw_aiding_running_prev = !noOtherYawAidingThanMag(); - return result; + return result; } diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 36eefd6c04..679c0338ca 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -436,6 +436,7 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation) const float SA2 = SA0*q0 + SA1*q1; const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3); float SA4, SA5_inv; + if (sq(SA3) > 1e-6f) { SA4 = 1.0F/sq(SA3); SA5_inv = sq(SA2)*SA4 + 1; @@ -448,6 +449,7 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation) const float SB2 = SB0*q3 + SB1*q2; const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3); float SB3, SB5_inv; + if (sq(SB2) > 1e-6f) { SB3 = 1.0F/sq(SB2); SB5_inv = SB3*sq(SB4) + 1; @@ -484,8 +486,10 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation) // calculate the yaw innovation and wrap to the interval between +-pi float innovation; + if (zero_innovation) { innovation = 0.0f; + } else { innovation = wrap_pi(atan2f(_R_to_earth(1, 0), _R_to_earth(0, 0)) - measurement); } @@ -516,6 +520,7 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation) const float SA2 = SA0*q0 - SA1*q1; const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3); float SA4, SA5_inv; + if (sq(SA3) > 1e-6f) { SA4 = 1.0F/sq(SA3); SA5_inv = sq(SA2)*SA4 + 1; @@ -528,6 +533,7 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation) const float SB2 = -SB0*q3 + SB1*q2; const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3); float SB3, SB5_inv; + if (sq(SB2) > 1e-6f) { SB3 = 1.0F/sq(SB2); SB5_inv = SB3*sq(SB4) + 1; @@ -563,8 +569,10 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation) } float innovation; + if (zero_innovation) { innovation = 0.0f; + } else { // calculate the the innovation and wrap to the interval between +-pi innovation = wrap_pi(atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)) - measurement); @@ -578,16 +586,18 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation) } // update quaternion states and covariances using the yaw innovation, yaw observation variance and yaw Jacobian -void Ekf::updateQuaternion(const float innovation, const float variance, const float gate_sigma, const Vector4f& yaw_jacobian) +void Ekf::updateQuaternion(const float innovation, const float variance, const float gate_sigma, + const Vector4f &yaw_jacobian) { // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero // calculate the innovation variance _heading_innov_var = variance; + for (unsigned row = 0; row <= 3; row++) { float tmp = 0.0f; for (uint8_t col = 0; col <= 3; col++) { - tmp += P(row,col) * yaw_jacobian(col); + tmp += P(row, col) * yaw_jacobian(col); } _heading_innov_var += yaw_jacobian(row) * tmp; @@ -617,7 +627,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f for (uint8_t row = 0; row <= 15; row++) { for (uint8_t col = 0; col <= 3; col++) { - Kfusion(row) += P(row,col) * yaw_jacobian(col); + Kfusion(row) += P(row, col) * yaw_jacobian(col); } Kfusion(row) *= heading_innov_var_inv; @@ -626,7 +636,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f if (_control_status.flags.wind) { for (uint8_t row = 22; row <= 23; row++) { for (uint8_t col = 0; col <= 3; col++) { - Kfusion(row) += P(row,col) * yaw_jacobian(col); + Kfusion(row) += P(row, col) * yaw_jacobian(col); } Kfusion(row) *= heading_innov_var_inv; @@ -680,11 +690,11 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f KH[3] = Kfusion(row) * yaw_jacobian(3); for (unsigned column = 0; column < _k_num_states; column++) { - float tmp = KH[0] * P(0,column); - tmp += KH[1] * P(1,column); - tmp += KH[2] * P(2,column); - tmp += KH[3] * P(3,column); - KHP(row,column) = tmp; + float tmp = KH[0] * P(0, column); + tmp += KH[1] * P(1, column); + tmp += KH[2] * P(2, column); + tmp += KH[3] * P(3, column); + KHP(row, column) = tmp; } } @@ -711,6 +721,7 @@ void Ekf::fuseHeading() // Calculate the observation variance float R_YAW; + if (_control_status.flags.mag_hdg) { // using magnetic heading tuning parameter R_YAW = sq(_params.mag_heading_noise); @@ -747,6 +758,7 @@ void Ekf::fuseHeading() // handle special case where yaw measurement is unavailable bool fuse_zero_innov = false; + if (_is_yaw_fusion_inhibited) { // The yaw measurement cannot be trusted but we need to fuse something to prevent a badly // conditioned covariance matrix developing over time. @@ -755,12 +767,14 @@ void Ekf::fuseHeading() // unconstrained quaternion variance growth and record the predicted heading // to use as an observation when movement ceases. // TODO a better way of determining when this is necessary - const float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3); + const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3); + if (sumQuatVar > _params.quat_max_variance) { fuse_zero_innov = true; R_YAW = 0.25f; } + _last_static_yaw = predicted_hdg; } else { @@ -770,6 +784,7 @@ void Ekf::fuseHeading() measured_hdg = _last_static_yaw; } + } else { _last_static_yaw = predicted_hdg; @@ -798,6 +813,7 @@ void Ekf::fuseHeading() // handle special case where yaw measurement is unavailable bool fuse_zero_innov = false; + if (_is_yaw_fusion_inhibited) { // The yaw measurement cannot be trusted but we need to fuse something to prevent a badly // conditioned covariance matrix developing over time. @@ -806,12 +822,14 @@ void Ekf::fuseHeading() // unconstrained quaterniion variance growth and record the predicted heading // to use as an observation when movement ceases. // TODO a better way of determining when this is necessary - const float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3); + const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3); + if (sumQuatVar > _params.quat_max_variance) { fuse_zero_innov = true; R_YAW = 0.25f; } + _last_static_yaw = predicted_hdg; } else { @@ -821,6 +839,7 @@ void Ekf::fuseHeading() measured_hdg = _last_static_yaw; } + } else { _last_static_yaw = predicted_hdg; @@ -848,6 +867,7 @@ void Ekf::fuseDeclination(float decl_sigma) // calculation is badly conditioned close to +-90 deg declination return; } + const float HK0 = ecl::powf(magN, -2); const float HK1 = HK0*ecl::powf(magE, 2) + 1.0F; const float HK2 = 1.0F/HK1; @@ -859,6 +879,7 @@ void Ekf::fuseDeclination(float decl_sigma) const float HK8 = HK5*P(16,16) - P(16,17); const float innovation_variance = -HK0*HK6*HK7 + HK7*HK8*magE/ecl::powf(magN, 3) + R_DECL; float HK9; + if (innovation_variance > R_DECL) { HK9 = HK4/innovation_variance; } else { @@ -875,6 +896,7 @@ void Ekf::fuseDeclination(float decl_sigma) // Calculate the Kalman gains Vector24f Kfusion; + for (unsigned row = 0; row <= 15; row++) { Kfusion(row) = -HK9*(HK5*P(row,16) - P(row,17)); } @@ -903,15 +925,17 @@ void Ekf::limitDeclination() // set to 50% of the horizontal strength from geo tables if location is known float decl_reference; float h_field_min = 0.001f; + if (_params.mag_declination_source & MASK_USE_GEO_DECL) { // use parameter value until GPS is available, then use value returned by geo library if (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) { decl_reference = _mag_declination_gps; - h_field_min = fmaxf(h_field_min , 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps)); + h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps)); } else { decl_reference = math::radians(_params.mag_declination_deg); } + } else { // always use the parameter value decl_reference = math::radians(_params.mag_declination_deg); @@ -920,18 +944,21 @@ void Ekf::limitDeclination() // do not allow the horizontal field length to collapse - this will make the declination fusion badly conditioned // and can result in a reversal of the NE field states which the filter cannot recover from // apply a circular limit - float h_field = sqrtf(_state.mag_I(0)*_state.mag_I(0) + _state.mag_I(1)*_state.mag_I(1)); + float h_field = sqrtf(_state.mag_I(0) * _state.mag_I(0) + _state.mag_I(1) * _state.mag_I(1)); + if (h_field < h_field_min) { if (h_field > 0.001f * h_field_min) { const float h_scaler = h_field_min / h_field; _state.mag_I(0) *= h_scaler; _state.mag_I(1) *= h_scaler; + } else { // too small to scale radially so set to expected value const float mag_declination = getMagDeclination(); _state.mag_I(0) = 2.0f * h_field_min * cosf(mag_declination); _state.mag_I(1) = 2.0f * h_field_min * sinf(mag_declination); } + h_field = h_field_min; } @@ -939,17 +966,19 @@ void Ekf::limitDeclination() constexpr float decl_tolerance = 0.5f; const float decl_max = decl_reference + decl_tolerance; const float decl_min = decl_reference - decl_tolerance; - const float decl_estimate = atan2f(_state.mag_I(1) , _state.mag_I(0)); + const float decl_estimate = atan2f(_state.mag_I(1), _state.mag_I(0)); + if (decl_estimate > decl_max) { _state.mag_I(0) = h_field * cosf(decl_max); _state.mag_I(1) = h_field * sinf(decl_max); + } else if (decl_estimate < decl_min) { _state.mag_I(0) = h_field * cosf(decl_min); _state.mag_I(1) = h_field * sinf(decl_min); } } -float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f& mag_meas, const Vector3f& mag_earth_predicted) +float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted) { // theoretical magnitude of the magnetometer Z component value given X and Y sensor measurement and our knowledge // of the earth magnetic field vector at the current location diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 109d3f2a01..f2845bba12 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -240,7 +240,7 @@ void Ekf::fuseOptFlow() float test_ratio[2]; test_ratio[0] = sq(_flow_innov(0)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(0)); test_ratio[1] = sq(_flow_innov(1)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(1)); - _optflow_test_ratio = math::max(test_ratio[0],test_ratio[1]); + _optflow_test_ratio = math::max(test_ratio[0], test_ratio[1]); for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { if (test_ratio[obs_index] > 1.0f) { @@ -262,6 +262,7 @@ void Ekf::fuseOptFlow() // fuse observation axes sequentially SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians Vector24f Kfusion; // Optical flow Kalman gains + for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { // calculate observation Jocobians and Kalman gains @@ -339,7 +340,7 @@ bool Ekf::calcOptFlowBodyRateComp() return false; } - const bool use_flow_sensor_gyro = PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2)); + const bool use_flow_sensor_gyro = PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2)); if (use_flow_sensor_gyro) { diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index d63ace44ba..3c5698b74b 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -137,7 +137,8 @@ void Ekf::fuseSideslip() _fault_status.flags.bad_sideslip = true; // if we are getting aiding from other sources, warn and reset the wind states and covariances only - const char* action_string = nullptr; + const char *action_string = nullptr; + if (update_wind_only) { resetWindStates(); resetWindCovariance(); @@ -148,12 +149,14 @@ void Ekf::fuseSideslip() _state.wind_vel.setZero(); action_string = "full"; } + ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string); return; } + _fault_status.flags.bad_sideslip = false; - const float HK52 = HK16/_beta_innov_var; + const float HK52 = HK16 / _beta_innov_var; // Calculate predicted sideslip angle and innovation using small angle approximation _beta_innov = rel_wind_body(1) / rel_wind_body(0); @@ -184,6 +187,7 @@ void Ekf::fuseSideslip() // Calculate Kalman gains Vector24f Kfusion; + if (!update_wind_only) { Kfusion(0) = HK38*HK52; diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 872e707299..31b3024611 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -142,9 +142,9 @@ void Ekf::fuseHagl() _hagl_innov = pred_hagl - meas_hagl; // calculate the observation variance adding the variance of the vehicles own height uncertainty - const float obs_variance = fmaxf(P(9,9) * _params.vehicle_variance_scaler, 0.0f) - + sq(_params.range_noise) - + sq(_params.range_noise_scaler * _range_sensor.getRange()); + const float obs_variance = fmaxf(P(9, 9) * _params.vehicle_variance_scaler, 0.0f) + + sq(_params.range_noise) + + sq(_params.range_noise_scaler * _range_sensor.getRange()); // calculate the innovation variance - limiting it to prevent a badly conditioned fusion _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); @@ -167,6 +167,7 @@ void Ekf::fuseHagl() } else { // If we have been rejecting range data for too long, reset to measurement const uint64_t timeout = static_cast(_params.terrain_timeout * 1e6f); + if (isTimedOut(_time_last_hagl_fuse, timeout)) { _terrain_vpos = _state.pos(2) + meas_hagl; _terrain_var = obs_variance; @@ -290,8 +291,7 @@ void Ekf::updateTerrainValidity() _hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion)); _hagl_sensor_status.flags.range_finder = shouldUseRangeFinderForHagl() - && recent_range_fusion - && (_time_last_fake_hagl_fuse != _time_last_hagl_fuse); + && recent_range_fusion && (_time_last_fake_hagl_fuse != _time_last_hagl_fuse); _hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl() && recent_flow_for_terrain_fusion; }