diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp index 7e2564fc19..697e027e6d 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp @@ -44,13 +44,12 @@ EKFGSF_yaw::EKFGSF_yaw() void EKFGSF_yaw::reset() { - _vel_data_updated = false; _ekf_gsf_vel_fuse_started = false; _gsf_yaw_variance = INFINITY; } -void EKFGSF_yaw::update(const imuSample &imu_sample, bool in_air) +void EKFGSF_yaw::predict(const imuSample &imu_sample, bool in_air) { const Vector3f accel = imu_sample.delta_vel / imu_sample.delta_vel_dt; @@ -85,64 +84,60 @@ void EKFGSF_yaw::update(const imuSample &imu_sample, bool in_air) } } - // we don't start running the EKF part of the algorithm until there are regular velocity observations - if (!_ekf_gsf_vel_fuse_started && _vel_data_updated) { - - _vel_data_updated = false; - - initialiseEKFGSF(_vel_NE, _vel_accuracy); - - ahrsAlignYaw(); - - // don't start until in air or velocity is not negligible - if (in_air || _vel_NE.longerThan(_vel_accuracy)) { - _ekf_gsf_vel_fuse_started = true; - } - } - for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { predictEKF(model_index, imu_sample.delta_ang, imu_sample.delta_ang_dt, imu_sample.delta_vel, imu_sample.delta_vel_dt, in_air); } +} - if (_ekf_gsf_vel_fuse_started) { - if (_vel_data_updated) { - _vel_data_updated = false; +void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy, const bool in_air) +{ + // we don't start running the EKF part of the algorithm until there are regular velocity observations + if (!_ekf_gsf_vel_fuse_started) { - bool bad_update = false; + initialiseEKFGSF(vel_NE, vel_accuracy); - 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, _vel_NE, _vel_accuracy)) { - bad_update = true; + ahrsAlignYaw(); + + // don't start until in air or velocity is not negligible + if (in_air || vel_NE.longerThan(vel_accuracy)) { + _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, vel_NE, vel_accuracy)) { + bad_update = true; + } + } + + if (!bad_update) { + float total_weight = 0.0f; + // 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); } - if (!bad_update) { - float total_weight = 0.0f; - // calculate weighting for each model assuming a normal distribution - const float min_weight = 1e-5f; - uint8_t n_weight_clips = 0; + // normalise the weighting function + if (n_weight_clips < N_MODELS_EKFGSF) { + _model_weights /= total_weight; - 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 - reset(); - } + } else { + // all weights have collapsed due to excessive innovation variances so reset filters + reset(); } } @@ -481,10 +476,3 @@ Matrix3f EKFGSF_yaw::ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g) return ret; } - -void EKFGSF_yaw::setVelocity(const Vector2f &velocity, float accuracy) -{ - _vel_NE = velocity; - _vel_accuracy = accuracy; - _vel_data_updated = true; -} diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.h b/src/modules/ekf2/EKF/EKFGSF_yaw.h index 6c03bb3dc0..3719aec7ce 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.h +++ b/src/modules/ekf2/EKF/EKFGSF_yaw.h @@ -59,10 +59,11 @@ public: EKFGSF_yaw(); // Update Filter States - this should be called whenever new IMU data is available - void update(const imuSample &imu_sample, bool in_air = false); + void predict(const imuSample &imu_sample, bool in_air = false); - void setVelocity(const Vector2f &velocity, // NE velocity measurement (m/s) - float accuracy); // 1-sigma accuracy of velocity measurement (m/s) + void fuseVelocity(const Vector2f &vel_NE, // NE velocity measurement (m/s) + float vel_accuracy, // 1-sigma accuracy of velocity measurement (m/s) + bool in_air); void setTrueAirspeed(float true_airspeed) { _true_airspeed = true_airspeed; } @@ -137,9 +138,6 @@ private: matrix::Vector2f innov{}; // Velocity N,E innovation (m/s) } _ekf_gsf[N_MODELS_EKFGSF] {}; - bool _vel_data_updated{}; // true when velocity data has been updated - Vector2f _vel_NE{}; // NE velocity observations (m/s) - float _vel_accuracy{}; // 1-sigma accuracy of velocity observations (m/s) bool _ekf_gsf_vel_fuse_started{}; // true when the EKF's have started fusing velocity data and the prediction and update processing is active // initialise states and covariance data for the GSF and EKF filters diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index aa118f8537..83670b5655 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -51,7 +51,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } // run EKF-GSF yaw estimator once per imu_delayed update - _yawEstimator.update(imu_delayed, _control_status.flags.in_air && !_control_status.flags.vehicle_at_rest); + _yawEstimator.predict(imu_delayed, _control_status.flags.in_air && !_control_status.flags.vehicle_at_rest); _gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL); @@ -224,7 +224,7 @@ void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel) && (vel_var < _params.req_sacc) && vel_xy.isAllFinite()) { - _yawEstimator.setVelocity(vel_xy, vel_var); + _yawEstimator.fuseVelocity(vel_xy, vel_var, _control_status.flags.in_air); // Try to align yaw using estimate if available if (((_params.gnss_ctrl & static_cast(GnssCtrl::VEL))