diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index c78ab046e0..25d7a7764b 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -211,12 +211,7 @@ void Ekf::predictCovariance() dvxVar = dvyVar = dvzVar = sq(dt * accel_noise); // force symmetry on the covariance matrix before we use it for the prediction step - for (unsigned row = 1; row < _k_num_states; row++) { - for (unsigned column = 0; column < row; column++) { - P[row][column] = 0.5f * (P[row][column] + P[column][row]); - P[column][row] = P[row][column]; - } - } + makeSymmetrical(); // predict the covariance