[CORE] It is now possible to call successively several times predict

lagadic · Jun 3, 2024 · bd02c0c · bd02c0c
1 parent d1cd200
commit bd02c0c
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 3 deletions.
diff --git a/modules/core/include/visp3/core/vpUnscentedKalman.h b/modules/core/include/visp3/core/vpUnscentedKalman.h
@@ -384,6 +384,7 @@ class VISP_EXPORT vpUnscentedKalman
     return mean;
   }
 private:
+  bool m_hasUpdateBeenCalled; /*!< Set to true when update is called, reset at the beginning of predict.*/
   vpColVector m_Xest; /*!< The estimated (i.e. filtered) state variables.*/
   vpMatrix m_Pest; /*!< The estimated (i.e. filtered) covariance matrix.*/
   vpMatrix m_Q; /*!< The covariance introduced by performing the prediction step.*/

diff --git a/modules/core/src/math/kalman/vpUnscentedKalman.cpp b/modules/core/src/math/kalman/vpUnscentedKalman.cpp
@@ -40,7 +40,8 @@
 
 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
 vpUnscentedKalman::vpUnscentedKalman(const vpMatrix &Q, const vpMatrix &R, std::shared_ptr<vpUKSigmaDrawerAbstract> &drawer, const vpProcessFunction &f, const vpMeasurementFunction &h)
-  : m_Q(Q)
+  : m_hasUpdateBeenCalled(false)
+  , m_Q(Q)
   , m_R(R)
   , m_f(f)
   , m_h(h)
@@ -68,6 +69,21 @@ void vpUnscentedKalman::filter(const vpColVector &z, const double &dt, const vpC
 
 void vpUnscentedKalman::predict(const double &dt, const vpColVector &u)
 {
+  vpColVector x;
+  vpMatrix P;
+
+  if (m_hasUpdateBeenCalled) {
+    // Update is the last function that has been called, starting from the filtered values.
+    x = m_Xest;
+    P = m_Pest;
+    m_hasUpdateBeenCalled = false;
+  }
+  else {
+    // Predict is the last function that has been called, starting from the predicted values.
+    x = m_mu;
+    P = m_P;
+  }
+
   // Drawing the sigma points
   m_chi = m_sigmaDrawer->drawSigmaPoints(m_Xest, m_Pest);
 
@@ -124,6 +140,7 @@ void vpUnscentedKalman::update(const vpColVector &z)
   // Updating the estimate
   m_Xest = m_stateAddFunction(m_mu, m_K * m_measResFunc(z, m_muz));
   m_Pest = m_P - m_K * m_Pz * m_K.transpose();
+  m_hasUpdateBeenCalled = true;
 }
 
 vpUnscentedKalman::vpUnscentedTransformResult vpUnscentedKalman::unscentedTransform(const std::vector<vpColVector> &sigmaPoints,

diff --git a/tutorial/kalman/tutorial-ukf.cpp b/tutorial/kalman/tutorial-ukf.cpp
@@ -424,8 +424,6 @@ int main(/*const int argc, const char *argv[]*/)
   object_pos[3] = 1.;
   //! [Init_simu]
 
-  std::cin.get();
-
   //! [Simu_loop]
   for (unsigned int i = 0; i < 200; ++i) {
     //! [Update obj pose]