diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index ca8a48f680..571a592a2f 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -978,17 +978,8 @@ void NavEKF::CovariancePrediction() } } - // Force symmetry on the covariance matrix to prevent ill-conditioning - // of the matrix which would cause the filter to blow-up - for (uint8_t i=0; i<=20; i++) P[i][i] = nextP[i][i]; - for (uint8_t i=1; i<=20; i++) - { - for (uint8_t j=0; j<=i-1; j++) - { - P[i][j] = 0.5f*(nextP[i][j] + nextP[j][i]); - P[j][i] = P[i][j]; - } - } + FixCovarianceErrors(); + perf_end(_perf_CovariancePrediction); }