diff --git a/src/ukf.cpp b/src/ukf.cpp
index 5b55a4348..80090c346 100644
--- a/src/ukf.cpp
+++ b/src/ukf.cpp
@@ -292,8 +292,9 @@ namespace RobotLocalization
     {
       state_.noalias() += kalmanGainSubset * innovationSubset;
 
-      // (6) Compute the new estimate error covariance P = P - (K * P_zz * K')
-      estimateErrorCovariance_.noalias() -= (kalmanGainSubset * predictedMeasCovar * kalmanGainSubset.transpose());
+      // (6) Compute the new estimate error covariance P = P - (K * (P_zz+R) * K')
+      estimateErrorCovariance_.noalias() -= (kalmanGainSubset * (predictedMeasCovar + measurementCovarianceSubset) *
+                    kalmanGainSubset.transpose());
 
       wrapStateAngles();