Fixed Kalman Filter error covariance issues.