diff --git a/EKF/ekf.h b/EKF/ekf.h index 9636897cc7..a9c0a7d74a 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -545,9 +545,6 @@ private: // jacobian : 4x1 vector of partial derivatives of observation wrt each quaternion state void updateQuaternion(const float innovation, const float variance, const float gate_sigma, const float (&yaw_jacobian)[4]); - // shrinks the yaw axis uncertainty of quaternion covariances by fusing a zero innovation yaw observation - void shrinkYawVariance(); - // fuse the yaw angle obtained from a dual antenna GPS unit void fuseGpsAntYaw(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 8d92f8ff0b..df8b466923 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1800,18 +1800,3 @@ void Ekf::runYawEKFGSF() yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc); } } - -void Ekf::shrinkYawVariance() -{ - if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) { - // rolled more than pitched so use 321 rotation order to define yaw angle - // and fuse a zero innovation yaw to shrink quaternion yaw variance - fuseYaw321(0.0f, 0.25f, true); - - } else { - // pitched more than rolled so use 312 rotation order to define yaw angle - // and fuse a zero innovation yaw to shrink quaternion yaw variance - fuseYaw312(0.0f, 0.25f, true); - - } -}