Unscented Kalman filter (UKF)

doc/tutorial/python/tutorial-install-python-bindings.dox

@@ -672,26 +672,50 @@ If you have an issue that looks like:
                   from /usr/include/c++/11/bits/specfun.h:45,
                   from /usr/include/c++/11/cmath:1935,
                   from /usr/include/c++/11/math.h:36,
-                  from /home/sfelton/miniconda3/envs/wrapper3.9/include/python3.9/pyport.h:205,
-                  from /home/sfelton/miniconda3/envs/wrapper3.9/include/python3.9/Python.h:50,
-                  from /home/sfelton/.local/include/pybind11/detail/common.h:266,
-                  from /home/sfelton/.local/include/pybind11/attr.h:13,
-                  from /home/sfelton/.local/include/pybind11/detail/class.h:12,
-                  from /home/sfelton/.local/include/pybind11/pybind11.h:13,
-                  from /home/sfelton/software/visp_build/modules/python/bindings/src/robot.cpp:3:
+                  from /home/user/miniconda3/envs/wrapper3.9/include/python3.9/pyport.h:205,
+                  from /home/user/miniconda3/envs/wrapper3.9/include/python3.9/Python.h:50,
+                  from /home/user/.local/include/pybind11/detail/common.h:266,
+                  from /home/user/.local/include/pybind11/attr.h:13,
+                  from /home/user/.local/include/pybind11/detail/class.h:12,
+                  from /home/user/.local/include/pybind11/pybind11.h:13,
+                  from /home/user/visp_ws/visp_build/modules/python/bindings/src/robot.cpp:3:
   /usr/include/c++/11/type_traits: **In instantiation of ‘struct std::is_move_constructible<vpImage<double> >’:**
   /usr/include/c++/11/type_traits:152:12:   required from ‘struct std::__and_<std::is_move_constructible<vpImage<double> >, std::is_move_assignable<vpImage<double> > >’
   /usr/include/c++/11/type_traits:157:12:   required from ‘struct std::__and_<std::__not_<std::__is_tuple_like<vpImage<double> > >, std::is_move_constructible<vpImage<double> >, std::is_move_assignable<vpImage<double> > >’
   /usr/include/c++/11/type_traits:2209:11:   required by substitution of ‘template<class ... _Cond> using _Require = std::__enable_if_t<std::__and_< <template-parameter-1-1> >::value> [with _Cond = {std::__not_<std::__is_tuple_like<vpImage<double> > >, std::is_move_constructible<vpImage<double> >, std::is_move_assignable<vpImage<double> >}]’
   /usr/include/c++/11/bits/move.h:196:5:   required by substitution of ‘template<class _Tp> std::_Require<std::__not_<std::__is_tuple_like<_Tp> >, std::is_move_constructible<_Tp>, std::is_move_assignable<_Tp> > std::swap(_Tp&, _Tp&) [with _Tp = vpImage<double>]’
-  /home/sfelton/software/visp-sfelton/modules/core/include/visp3/core/vpImage.h:341:15:   required from ‘class vpImage<double>’
-  /home/sfelton/software/visp-sfelton/modules/core/include/visp3/core/vpImage.h:369:17:   required from here
+  /home/user/visp_ws/visp/modules/core/include/visp3/core/vpImage.h:341:15:   required from ‘class vpImage<double>’
+  /home/user/visp_ws/visp/modules/core/include/visp3/core/vpImage.h:369:17:   required from here
   /usr/include/c++/11/type_traits:1010:52: error: static assertion failed: template argument must be a complete class or an unbounded array
   1010 |       **static_assert(std::__is_complete_or_unbounded(__type_identity<_Tp>{}),**
 \endverbatim
 
 You should delete the files in `modules/python/` of the **build** directory.
 
+
+If you have an error that looks like this:
+
+\verbatim
+/home/user/miniconda3/envs/visp-python11/include/pybind11/cast.h: In instantiation of ‘pybind11::arg_v::arg_v(pybind11::arg&&, T&&, const char*) [with T = vpColVector (&)(const vpColVector&, const vpColVector&)]’:
+/home/user/miniconda3/envs/visp-python11/include/pybind11/cast.h:1452:53:   required from ‘pybind11::arg_v::arg_v(const char*, T&&, const char*) [with T = vpColVector (&)(const vpColVector&, const vpColVector&)]’
+/home/user/visp_ws/visp-build/modules/python/bindings/src/core.cpp:20491:347:   required from here
+/home/user/miniconda3/envs/visp-python11/include/pybind11/cast.h:1432:82: error: call of overloaded ‘cast(vpColVector (&)(const vpColVector&, const vpColVector&), pybind11::return_value_policy, <brace-enclosed initializer list>)’ is ambiguous
+ 1432 |         : arg(base), value(reinterpret_steal<object>(detail::make_caster<T>::cast(
+      |                                                      ~~~~~~~~~~~~~~~~~~~~~~~~~~~~^
+ 1433 |                          std::forward<T>(x), return_value_policy::automatic, {}))),
+      |                          ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+In file included from /home/user/miniconda3/envs/visp-python11/include/pybind11/cast.h:15,
+                 from /home/user/miniconda3/envs/visp-python11/include/pybind11/attr.h:14,
+                 from /home/user/miniconda3/envs/visp-python11/include/pybind11/detail/class.h:12,
+                 from /home/user/miniconda3/envs/visp-python11/include/pybind11/pybind11.h:13,
+                 from /home/user/visp_ws/visp_ukf/modules/python/bindings/src/core.cpp:3:
+\endverbatim
+
+This is due to an argument that cannot have a default value. In the example above, this argument is **vpColVector (&)(const vpColVector&, const vpColVector&)** (converted from an std::function).
+To solve this, go to `modules/python/generator/visp_python_bindgen/generator_config.py`. Then, add a regular expression in the **FORBIDDEN_DEFAULT_ARGUMENT_TYPES_REGEXS** list to match with this type.
+Here, we would add '^std::function' to exclude the std::function from being constructed with a default value.
+
+
 \subsubsection py_bindings_known_errors_import_dll ImportError: DLL load failed while importing _visp
 
 The following error may occur on Windows

doc/tutorial/tutorial-users.dox

@@ -186,6 +186,7 @@ This page introduces the user to other tools that may be useful.
 - \subpage tutorial-json <br>This tutorial explains how to read and save data in the portable JSON format. It focuses on saving the data generated by a visual servoing experiment and exporting it to Python in order to generate plots.
 - \subpage tutorial-synthetic-blenderproc <br> This tutorial shows you how to easily generate synthetic data from the 3D model of an object and obtain various modalities. This data can then be used to train a neural network for your own task.
 - \subpage tutorial-spc <br> This tutorial shows you how to monitor if a signal is "in control" using Statistical Process Control methods.
+- \subpage tutorial-ukf <br> This tutorial shows you how to use an Unscented Kalman Filter to filter data when the model and/or measurements are non-linear.
 */
 
 /*! \page tutorial_munkres Munkres Assignment Algorithm

example/CMakeLists.txt

@@ -70,6 +70,7 @@ visp_add_subdirectory(device/light             REQUIRED_DEPS visp_core visp_robo
 visp_add_subdirectory(direct-visual-servoing   REQUIRED_DEPS visp_core visp_robot visp_visual_features visp_io visp_gui)
 visp_add_subdirectory(homography               REQUIRED_DEPS visp_core visp_vision visp_io)
 visp_add_subdirectory(image                    REQUIRED_DEPS visp_core visp_io)
+visp_add_subdirectory(kalman                   REQUIRED_DEPS visp_core visp_gui)
 visp_add_subdirectory(manual                   REQUIRED_DEPS visp_core visp_sensor visp_vs visp_robot visp_ar visp_vision visp_io visp_gui)
 visp_add_subdirectory(math                     REQUIRED_DEPS visp_core visp_io)
 visp_add_subdirectory(moments/image            REQUIRED_DEPS visp_core visp_vs visp_robot visp_gui)

example/kalman/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(example-kalman)
+
+find_package(VISP REQUIRED visp_core visp_gui)
+
+set(example_cpp)
+
+list(APPEND example_cpp example-ukf-linear-example.cpp)
+list(APPEND example_cpp example-ukf-nonlinear-example.cpp)
+list(APPEND example_cpp example-ukf-nonlinear-complex-example.cpp)
+
+foreach(cpp ${example_cpp})
+  visp_add_target(${cpp})
+  if(COMMAND visp_add_dependency)
+    visp_add_dependency(${cpp} "examples")
+  endif()
+endforeach()

example/kalman/example-ukf-linear-example.cpp

@@ -0,0 +1,229 @@
+/****************************************************************************
+ *
+ * ViSP, open source Visual Servoing Platform software.
+ * 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
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See https://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp@inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+*****************************************************************************/
+
+/** \example tutorial-ukf-linear-example.cpp
+ * Example of a simple linear use-case of the Unscented Kalman Filter (UKF). Using a UKF
+ * in this case is not necessary, it is done for learning purpous only.
+ *
+ * The system we are interested in is a system moving on a 2D-plane.
+ *
+ * The state vector of the UKF is:
+ *  \f{eqnarray*}{
+        \textbf{x}[0] &=& x \\
+        \textbf{x}[1] &=& \dot{x} \\
+        \textbf{x}[1] &=& y \\
+        \textbf{x}[2] &=& \dot{y}
+   \f}
+
+ * The measurement \f$ \textbf{z} \f$ corresponds to the position along the x-axis
+ * and y-axis. The measurement vector can be written as:
+ * \f{eqnarray*}{
+        \textbf{z}[0] &=& x \\
+        \textbf{z}[1] &=& y
+   \f}
+
+ * Some noise is added to the measurement vector to simulate a sensor which is
+ * not perfect.
+*/
+
+// UKF includes
+#include <visp3/core/vpUKSigmaDrawerMerwe.h>
+#include <visp3/core/vpUnscentedKalman.h>
+
+// ViSP includes
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpGaussRand.h>
+#ifdef VISP_HAVE_DISPLAY
+#include <visp3/gui/vpPlot.h>
+#endif
+
+#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
+/**
+ * \brief The process function, that updates the prior.
+ *
+ * \param[in] chi A sigma point.
+ * \param[in] dt The period.
+ * \return vpColVector The sigma point projected in the future.
+ */
+vpColVector fx(const vpColVector &chi, const double &dt)
+{
+  vpColVector point(4);
+  point[0] = chi[1] * dt + chi[0];
+  point[1] = chi[1];
+  point[2] = chi[3] * dt + chi[2];
+  point[3] = chi[3];
+  return point;
+}
+
+/**
+ * \brief The measurement function, that project the prior in the measurement space.
+ *
+ * \param[in] chi The prior.
+ * \return vpColVector The prior projected in the measurement space.
+ */
+vpColVector hx(const vpColVector &chi)
+{
+  vpColVector point(2);
+  point[0] = chi[0];
+  point[1] = chi[2];
+  return point;
+}
+
+int main(/*const int argc, const char *argv[]*/)
+{
+  const double dt = 0.01; // Period of 1s
+  const double gt_dx = 0.01; // Ground truth displacement along x axis between two measurements
+  const double gt_dy = 0.005; // Ground truth displacement along x axis between two measurements
+  vpColVector gt_dX(2); // Ground truth displacement between two measurements
+  gt_dX[0] = gt_dx;
+  gt_dX[1] = gt_dy;
+  const double gt_vx = gt_dx / dt; // Ground truth velocity along x axis
+  const double gt_vy = gt_dy / dt; // Ground truth velocity along y axis
+  const double processVariance = 0.000004;
+  const double sigmaXmeas = 0.05; // Standard deviation of the measure along the x-axis
+  const double sigmaYmeas = 0.05; // Standard deviation of the measure along the y-axis
+
+  // Initialize the attributes of the UKF
+  std::shared_ptr<vpUKSigmaDrawerAbstract> drawer = std::make_shared<vpUKSigmaDrawerMerwe>(4, 0.3, 2., -1.);
+  vpMatrix P0(4, 4); //  The initial guess of the process covariance
+  P0.eye(4, 4);
+  P0 = P0 * 1.;
+  vpMatrix R(2, 2); // The covariance of the noise introduced by the measurement
+  R.eye(2, 2);
+  R = R * 0.01;
+  vpMatrix Q(4, 4, 0.); // The covariance of the process
+  vpMatrix Q1d(2, 2); // The covariance of a process whose states are {x, dx/dt} and for which the variance is 1
+  Q1d[0][0] = std::pow(dt, 3) / 3.;
+  Q1d[0][1] = std::pow(dt, 2)/2.;
+  Q1d[1][0] = std::pow(dt, 2)/2.;
+  Q1d[1][1] = dt;
+  Q.insert(Q1d, 0, 0);
+  Q.insert(Q1d, 2, 2);
+  Q = Q * processVariance;
+  vpUnscentedKalman::vpProcessFunction f = fx;
+  vpUnscentedKalman::vpMeasurementFunction h = hx;
+
+  // Initialize the UKF
+  vpUnscentedKalman ukf(Q, R, drawer, f, h);
+  ukf.init(vpColVector({ 0., 0.75 * gt_vx, 0., 0.75 * gt_vy }), P0);
+
+#ifdef VISP_HAVE_DISPLAY
+  // Initialize the plot
+  vpPlot plot(4);
+  plot.initGraph(0, 3);
+  plot.setTitle(0, "Position along X-axis");
+  plot.setUnitX(0, "Time (s)");
+  plot.setUnitY(0, "Position (m)");
+  plot.setLegend(0, 0, "GT");
+  plot.setLegend(0, 1, "Measure");
+  plot.setLegend(0, 2, "Filtered");
+
+  plot.initGraph(1, 3);
+  plot.setTitle(1, "Velocity along X-axis");
+  plot.setUnitX(1, "Time (s)");
+  plot.setUnitY(1, "Velocity (m/s)");
+  plot.setLegend(1, 0, "GT");
+  plot.setLegend(1, 1, "Measure");
+  plot.setLegend(1, 2, "Filtered");
+
+  plot.initGraph(2, 3);
+  plot.setTitle(2, "Position along Y-axis");
+  plot.setUnitX(2, "Time (s)");
+  plot.setUnitY(2, "Position (m)");
+  plot.setLegend(2, 0, "GT");
+  plot.setLegend(2, 1, "Measure");
+  plot.setLegend(2, 2, "Filtered");
+
+  plot.initGraph(3, 3);
+  plot.setTitle(3, "Velocity along Y-axis");
+  plot.setUnitX(3, "Time (s)");
+  plot.setUnitY(3, "Velocity (m/s)");
+  plot.setLegend(3, 0, "GT");
+  plot.setLegend(3, 1, "Measure");
+  plot.setLegend(3, 2, "Filtered");
+#endif
+
+  // Initialize measurement noise
+  vpGaussRand rngX(sigmaXmeas, 0., 4224);
+  vpGaussRand rngY(sigmaYmeas, 0., 2112);
+
+  // Main loop
+  vpColVector gt_X(2, 0.);
+  vpColVector z_prec(2, 0.);
+  for (int i = 0; i < 100; ++i) {
+    // Perform the measurement
+    double x_meas = gt_X[0] + rngX();
+    double y_meas = gt_X[1] + rngY();
+    vpColVector z(2);
+    z[0] = x_meas;
+    z[1] = y_meas;
+
+    // Use the UKF to filter the measurement
+    ukf.filter(z, dt);
+    vpColVector Xest = ukf.getXest();
+
+#ifdef VISP_HAVE_DISPLAY
+    // Plot the ground truth, measurement and filtered state
+    plot.plot(0, 0, i, gt_X[0]);
+    plot.plot(0, 1, i, x_meas);
+    plot.plot(0, 2, i, Xest[0]);
+
+    double vX_meas = (x_meas - z_prec[0]) / dt;
+    plot.plot(1, 0, i, gt_vx);
+    plot.plot(1, 1, i, vX_meas);
+    plot.plot(1, 2, i, Xest[1]);
+
+    plot.plot(2, 0, i, gt_X[1]);
+    plot.plot(2, 1, i, y_meas);
+    plot.plot(2, 2, i, Xest[2]);
+
+    double vY_meas = (y_meas - z_prec[1]) / dt;
+    plot.plot(3, 0, i, gt_vy);
+    plot.plot(3, 1, i, vY_meas);
+    plot.plot(3, 2, i, Xest[3]);
+#endif
+
+    // Update
+    gt_X += gt_dX;
+    z_prec = z;
+  }
+  std::cout << "Press Enter to quit..." << std::endl;
+  std::cin.get();
+  return 0;
+}
+#else
+int main()
+{
+  std::cout << "vpUnscentedKalman is only available if you compile ViSP in C++11 standard or higher." << std::endl;
+  return 0;
+}
+#endif