Unscented Kalman Filter in python

example/kalman/ukf-nonlinear-complex-example.cpp

@@ -540,7 +540,7 @@
   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;

modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h

@@ -135,7 +135,7 @@
 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,

modules/core/src/math/matrix/vpMatrix_cholesky.cpp

@@ -193,8 +193,11 @@
     vpMatrix A = *this;
     dpotrf_((char *)"L", &rowNum_, A.data, &lda, &info);
 
-    if (info != 0)
-      throw(vpException(vpException::fatalError, "Cannot inverse by Cholesky with Lapack: error in dpotrf_()"));
+    if (info != 0) {
+      std::stringstream errMsg;
+      errMsg << "Cannot inverse by Cholesky with Lapack: error "<< info << " in dpotrf_()";
+      throw(vpException(vpException::fatalError, errMsg.str()));
+    }
 
     dpotri_((char *)"L", &rowNum_, A.data, &lda, &info);
     if (info != 0) {

modules/python/doc/rst/tutorials/misc/ukf-linear.rst

@@ -0,0 +1,15 @@
+Example on a linear case study on how to use Unscented Kalman filter
+=======================================================================
+
+This example shows how to use an Unscented Kalman filter in Python.
+
+This example is a linear case study. It is not what is intended for the Unscented
+Kalman filter, but it permits to get familiar with the different classes that
+are used.
+
+The state vector and measurements are explained in the  `C++ example <https://visp-doc.inria.fr/doxygen/visp-daily/ukf-linear-example_8cpp-example.html>`_.
+
+
+
+.. literalinclude:: /examples/ukf-linear-example.py
+  :language: python

modules/python/doc/rst/tutorials/misc/ukf-nonlinear-linear-complex.rst

@@ -0,0 +1,13 @@
+Example on a complex non-linear case study on how to use Unscented Kalman filter
+=======================================================================
+
+This example shows how to use an Unscented Kalman filter in Python.
+
+This example is a complex non-linear case study.
+
+The state vector and measurements are explained in the  `C++ example <https://visp-doc.inria.fr/doxygen/visp-daily/ukf-nonlinear-complex-example_8cpp-example.html>`_.
+
+
+
+.. literalinclude:: /examples/ukf-nonlinear-complex-example.py
+  :language: python

modules/python/doc/rst/tutorials/misc/ukf-nonlinear-linear.rst

@@ -0,0 +1,13 @@
+Example on a non-linear case study on how to use Unscented Kalman filter
+=======================================================================
+
+This example shows how to use an Unscented Kalman filter in Python.
+
+This example is a non-linear case study.
+
+The state vector and measurements are explained in the  `C++ example <https://visp-doc.inria.fr/doxygen/visp-daily/ukf-nonlinear-example_8cpp-example.html>`_.
+
+
+
+.. literalinclude:: /examples/ukf-nonlinear-example.py
+  :language: python

modules/python/doc/rst/tutorials/tutorials.rst

@@ -23,3 +23,14 @@ Tracking
     :maxdepth: 2
 
     tracking/*
+
+
+
+Other tools
+-----------------------
+
+.. toctree::
+    :glob:
+    :maxdepth: 2
+
+    misc/*

modules/python/examples/ukf-linear-example.py

@@ -1,7 +1,7 @@
 #############################################################################
 #
 # ViSP, open source Visual Servoing Platform software.
-# Copyright (C) 2005 - 2023 by Inria. All rights reserved.
+# Copyright (C) 2005 - 2024 by Inria. All rights reserved.
 #
 # This software is free software; you can redistribute it and/or modify
 # it under the terms of the GNU General Public License as published by
@@ -58,7 +58,7 @@
  not perfect.
 """
 
-from visp.core import ColVector, Matrix, UnscentedKalman, UKSigmaDrawerMerwe, GaussRand
+from visp.core import ColVector, Matrix, UnscentedKalman, UKSigmaDrawerMerwe
 
 # For the Graphical User Interface
 try:
@@ -184,8 +184,6 @@ def generate_Q_matrix(dt: float) -> Matrix:
   z_prec = ColVector(2, 0.) # Previous measurement vector
   np.random.seed(4224)
 
-  rngX = GaussRand(sigmaXmeas, 0., 4224) # Gaussian noise random generator for x-axis measurement
-  rngY = GaussRand(sigmaYmeas, 0., 2112) # Gaussian noise random generator for y-axis measurement
   for i in range(100):
     # Creating noisy measurements
     x_meas = gt_X[0] + np.random.normal(0.0, sigmaXmeas)