diff --git a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp
index 43d45cced9871fd09594c2a0db66e0fb63ff43e0..318ed6eb747bc0c92f9935050e385854b5df29e2 100644
--- a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp
+++ b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp
@@ -186,8 +186,8 @@ UnscentedKalmanFilter<SystemModelT>::calculateClosestPosSemidefMatrix(
 {
     const StateCovT new_P = ((cov + cov.transpose()) / 2.0f);
     Eigen::EigenSolver<StateCovT> solver(new_P);
-    Eigen::Matrix<FloatT, 6, 6> D = solver.eigenvalues().real().asDiagonal();
-    const Eigen::Matrix<FloatT, 6, 6> V = solver.eigenvectors().real();
+    Eigen::Matrix<FloatT, dim::state, dim::state> D = solver.eigenvalues().real().asDiagonal();
+    const Eigen::Matrix<FloatT, dim::state, dim::state> V = solver.eigenvectors().real();
     D = D.cwiseMax(0);
 
     return (V * D * V.inverse());