[EXAMPLE][PYTHON][FIX] Fixed the problem of Cholesky's inversion: it …

…was due to numerical instability
lagadic · May 22, 2024 · 451aa4c · 451aa4c
1 parent 5e39af0
commit 451aa4c
Show file tree

Hide file tree

Showing 3 changed files with 3 additions and 4 deletions.
diff --git a/example/kalman/ukf-nonlinear-complex-example.cpp b/example/kalman/ukf-nonlinear-complex-example.cpp
@@ -540,7 +540,7 @@ int main(/*const int argc, const char *argv[]*/)
   const unsigned int nbCmds = cmds.size();
 
   // Initialize the attributes of the UKF
-  std::shared_ptr<vpUKSigmaDrawerAbstract> drawer = std::make_shared<vpUKSigmaDrawerMerwe>(3, 0.00001, 2., 0, stateResidual, stateAdd);
+  std::shared_ptr<vpUKSigmaDrawerAbstract> drawer = std::make_shared<vpUKSigmaDrawerMerwe>(3, 0.1, 2., 0, stateResidual, stateAdd);
 
   vpMatrix R1landmark(2, 2, 0.); // The covariance of the noise introduced by the measurement with 1 landmark
   R1landmark[0][0] = sigmaRange*sigmaRange;

diff --git a/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h b/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h
@@ -135,7 +135,7 @@ class VISP_EXPORT vpUKSigmaDrawerMerwe : public vpUKSigmaDrawerAbstract
 protected:
   inline void computeLambda()
   {
-    m_lambda = m_alpha * m_alpha * (m_n + m_kappa) - m_n;
+    m_lambda = m_alpha * m_alpha * (static_cast<double>(m_n) + m_kappa) - static_cast<double>(m_n);
   }
 
   double m_alpha; /*!< A factor, which should be a real in the interval [0; 1]. The larger alpha is,

diff --git a/modules/python/examples/ukf-nonlinear-complex-example.py b/modules/python/examples/ukf-nonlinear-complex-example.py
@@ -472,7 +472,7 @@ def measureWithNoise(self, pos: ColVector) -> ColVector:
   robot = vpBicycleModel(wheelbase)
 
   # The object that draws the sigma points used by the UKF
-  drawer = UKSigmaDrawerMerwe(n=3, alpha=0.00001, beta=2, kappa=0, resFunc=residual_state_vectors, addFunc=add_state_vectors)
+  drawer = UKSigmaDrawerMerwe(n=3, alpha=0.1, beta=2, kappa=0, resFunc=residual_state_vectors, addFunc=add_state_vectors)
 
   # The matrices require for the construction of the Unscented filter
   P0 = Matrix([[0.1, 0., 0.],
@@ -482,7 +482,6 @@ def measureWithNoise(self, pos: ColVector) -> ColVector:
   R = Matrix(2*nbLandmarks, 2 * nbLandmarks) # The measurement covariance matrix for the grid of landmarks
   for i in range(nbLandmarks):
     R.insert(R1landmark, 2*i, 2*i)
-  print(R)
 
   Q = Matrix() # The process covariance matrix
   Q.eye(3)