[FIX] Fix problem due to wrong initialization

lagadic · Jun 17, 2024 · a2de711 · a2de711
1 parent 8283097
commit a2de711
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/modules/core/src/math/kalman/vpUnscentedKalman.cpp b/modules/core/src/math/kalman/vpUnscentedKalman.cpp
@@ -58,8 +58,16 @@ vpUnscentedKalman::vpUnscentedKalman(const vpMatrix &Q, const vpMatrix &R, std::
 
 void vpUnscentedKalman::init(const vpColVector &mu0, const vpMatrix &P0)
 {
+  if ((mu0.getRows() != P0.getCols()) || (mu0.getRows() != P0.getRows())) {
+    throw(vpException(vpException::dimensionError, "Initial state X0 and process covariance matrix P0 sizes mismatch"));
+  }
+  if ((m_Q.getCols() != P0.getCols()) || (m_Q.getRows() != P0.getRows())) {
+    throw(vpException(vpException::dimensionError, "Initial process covariance matrix P0 and Q matrix sizes mismatch"));
+  }
   m_Xest = mu0;
   m_Pest = P0;
+  m_mu = mu0;
+  m_Ppred = P0;
 }
 
 void vpUnscentedKalman::filter(const vpColVector &z, const double &dt, const vpColVector &u)