diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index b5f0fc8eca..b0409a37e3 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -79,7 +79,7 @@ void Ekf::fuseSideslip() rel_wind = earth_to_body * rel_wind; // perform fusion of assumed sideslip = 0 - if (rel_wind(0) > 5.0f){ + if (rel_wind(0) > 7.0f){ // Calculate the observation jacobians @@ -205,12 +205,6 @@ void Ekf::fuseSideslip() } } } - - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P[row][column] = P[row][column] - KHP[row][column]; - } - } // if the covariance correction will result in a negative variance, then // the covariance marix is unhealthy and must be corrected