From 506bffab8213de4c54f6f1352af0bf14530ce116 Mon Sep 17 00:00:00 2001 From: Luminita Totu Date: Mon, 17 Jan 2022 15:14:18 +0100 Subject: [PATCH 1/2] Adding matrix R in UKF covariance update in correct (711) --- src/ukf.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/ukf.cpp b/src/ukf.cpp index 5b55a4348..0de29b2be 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(); From e6d56e024b9d5b02d562dcb6ddf8606f53726da1 Mon Sep 17 00:00:00 2001 From: Luminita Totu Date: Mon, 17 Jan 2022 16:43:18 +0100 Subject: [PATCH 2/2] lint --- src/ukf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ukf.cpp b/src/ukf.cpp index 0de29b2be..80090c346 100644 --- a/src/ukf.cpp +++ b/src/ukf.cpp @@ -293,7 +293,7 @@ namespace RobotLocalization state_.noalias() += kalmanGainSubset * innovationSubset; // (6) Compute the new estimate error covariance P = P - (K * (P_zz+R) * K') - estimateErrorCovariance_.noalias() -= (kalmanGainSubset * (predictedMeasCovar + measurementCovarianceSubset) * + estimateErrorCovariance_.noalias() -= (kalmanGainSubset * (predictedMeasCovar + measurementCovarianceSubset) * kalmanGainSubset.transpose()); wrapStateAngles();