diff --git a/doc/tutorial/misc/tutorial-ukf.dox b/doc/tutorial/misc/tutorial-ukf.dox index 2b08c92a61..f8e0bb1b8e 100644 --- a/doc/tutorial/misc/tutorial-ukf.dox +++ b/doc/tutorial/misc/tutorial-ukf.dox @@ -18,7 +18,7 @@ The equations that describe the motion of the object in the world frame are the {}^W \textbf{X}_x &=& R cos(\omega t + \phi) \\ {}^W \textbf{X}_y &=& R sin(\omega t + \phi) \\ {}^W \textbf{X}_z &=& constant -\end{array} + \end{array} \f] where \f$ \omega \f$ and \f$ \phi \f$ are respectively the pulsation and the phase of the motion, while \f$ R \f$ is the @@ -44,7 +44,7 @@ The first step of the UKF is the prediction step. During this step, some particu \begin{array}{lcl} \chi &=& sigma-function(\textbf{x}, \textbf{P}) \\ \textbf{w}^m, \textbf{w}^c &=& weight-function(n, parameters) -\end{array} + \end{array} \f] There are different ways of drawing the sigma points and associated weights in the litterature, such as the one @@ -64,7 +64,7 @@ and covariance \f$ \overline{\textbf{P}} \f$ of the prior: \boldsymbol{\mu}, \overline{\textbf{P}} &=& UT({Y}, \textbf{w}^m, \textbf{w}^c, \textbf{Q}) \\ \boldsymbol{\mu} &=& \sum_{i=0}^{2n} w_i^m {Y}_i \\ \overline{\textbf{P}} &=& \sum_{i=0}^{2n} ( w_i^c ({Y}_i - \boldsymbol{\mu}) ({Y}_i - \boldsymbol{\mu})^T ) + \textbf{Q} - \end{array} + \end{array} \f] where \f$ \textbf{Q} \f$ is the covariance of the error introduced by the process function. @@ -82,7 +82,7 @@ covariance \f$ \textbf{P}_z \in {R}^{m\text{ x }m} \f$ of these points: \boldsymbol{\mu}_z, \textbf{P}_z &=& UT({Z}, \textbf{w}^m, \textbf{w}^c, \textbf{R}) \\ \boldsymbol{\mu}_z &=& \sum_{i=0}^{2n} w_i^m {Z}_i \\ \textbf{P}_z &=& \sum_{i=0}^{2n} ( w_i^c ({Z}_i - \boldsymbol{\mu}_z) ({Z}_i - \boldsymbol{\mu}_z)^T ) + \textbf{R} -\end{array} + \end{array} \f] where \f$ \textbf{R} \f$ is the measurement covariance matrix. @@ -103,9 +103,9 @@ Finally, we can compute the new state estimate \f$ \textbf{x} \f$ and the new co \f[ \begin{array}{lcl} - \textbf{x} &=& \boldsymbol{\mu} + \textbf{K} \textbf{y} \\ - \textbf{P} &=& \overline{\textbf{P}} - \textbf{K} \textbf{P}_z \textbf{K}^T -\end{array} + \textbf{x} &=& \boldsymbol{\mu} + \textbf{K} \textbf{y} \\ + \textbf{P} &=& \overline{\textbf{P}} - \textbf{K} \textbf{P}_z \textbf{K}^T + \end{array} \f] \section tuto-ukf-tutorial Explanations about the tutorial @@ -138,7 +138,7 @@ the 3D position of the object expressed in the world frame, along with the pulsa \textbf{x}[1] &=& {}^WX_y \\ \textbf{x}[2] &=& {}^WX_z \\ \textbf{x}[3] &=& \omega \Delta t -\end{array} + \end{array} \f] The measurement \f$ \textbf{z} \f$ corresponds to the perspective projection of the different markers in the image. @@ -149,7 +149,7 @@ The measurement vector can be written as: \begin{array}{lcl} \textbf{z}[2i] &=& u_i \\ \textbf{z}[2i+1] &=& v_i -\end{array} + \end{array} \f] Be \f$ \textbf{K}_{intr} \f$ the camera instrinsic parameters matrix defined by: @@ -213,7 +213,7 @@ As stated in the \ref tuto-ukf-intro, the equations of motion of the object are {}^W X_x(t) &=& R cos(\omega t + \phi) \\ {}^W X_y(t) &=& R sin(\omega t + \phi) \\ {}^W X_z(t) &=& constant -\end{array} + \end{array} \f] Thus, we have: @@ -223,7 +223,7 @@ Thus, we have: {}^WX_x( t + \Delta t) &=& R cos(\omega (t + \Delta t) + \phi) &=& R cos((\omega t + \phi) + \omega \Delta t )\\ {}^WX_y( t + \Delta t) &=& R sin(\omega (t + \Delta t) + \phi) &=& R sin((\omega t + \phi) + \omega \Delta t )\\ {}^WX_z( t + \Delta t) &=& constant -\end{array} + \end{array} \f] Which can be rewritten: @@ -232,7 +232,7 @@ Which can be rewritten: {}^WX_x( t + \Delta t) &=& R cos((\omega t + \phi) + \omega \Delta t ) &=& R cos(\omega t + \phi) cos (\omega \Delta t ) - R sin(\omega t + \phi) sin(\omega \Delta t) \\ {}^WX_y( t + \Delta t) &=& R sin((\omega t + \phi) + \omega \Delta t ) &=& R cos(\omega t + \phi) sin (\omega \Delta t ) + R sin(\omega t + \phi) cos(\omega \Delta t)\\ {}^WX_z( t + \Delta t) &=& constant -\end{array} + \end{array} \f] And can be finally written as: @@ -241,7 +241,7 @@ And can be finally written as: {}^WX_x( t + \Delta t) &=& R cos(\omega t + \phi) cos (\omega \Delta t ) - R sin(\omega t + \phi) sin(\omega \Delta t) &=& {}^W X_x( t) cos(\omega \Delta t) - {}^W X_y(t) sin(\omega \Delta t) \\ {}^WX_y( t + \Delta t) &=& R cos(\omega t + \phi) sin (\omega \Delta t ) + R sin(\omega t + \phi) cos(\omega \Delta t) &=& {}^W X_x( t) sin(\omega \Delta t) + {}^W X_y(t) cos(\omega \Delta t) \\ {}^WX_z( t + \Delta t) &=& constant -\end{array} + \end{array} \f] This motivates us to choose the following non-linear process function: @@ -252,7 +252,7 @@ This motivates us to choose the following non-linear process function: \textbf{x}[1]_{t + \Delta t} &=& {}^WX_y (t + \Delta t) &=& \textbf{x}[0]_{t} sin(\textbf{x}[3]_{t}) + \textbf{x}[1]_{t} cos(\textbf{x}[3]_{t}) \\ \textbf{x}[2]_{t + \Delta t} &=& {}^WX_z (t + \Delta t) &=& \textbf{x}[2]_{t} \\ \textbf{x}[3]_{t + \Delta t} &=& \omega \Delta t &=& \textbf{x}[3]_{t} -\end{array} + \end{array} \f] As the process function is pretty simple, a simple function called here `fx()` is enough: diff --git a/example/kalman/ukf-linear-example.cpp b/example/kalman/ukf-linear-example.cpp index 70d81cf941..664f877cad 100644 --- a/example/kalman/ukf-linear-example.cpp +++ b/example/kalman/ukf-linear-example.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * @@ -27,34 +26,32 @@ * * 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 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. + * in this case is not necessary, it is done for learning purpose only. * * The system we are interested in is a system moving on a 2D-plane. * * The state vector of the UKF is: - * \f[ - \begin{array}{lcl} - \textbf{x}[0] &=& x \\ - \textbf{x}[1] &=& \dot{x} \\ - \textbf{x}[1] &=& y \\ - \textbf{x}[2] &=& \dot{y} - \end{array} - \f] - + * \f[ + * \begin{array}{lcl} + * \textbf{x}[0] &=& x \\ + * \textbf{x}[1] &=& \dot{x} \\ + * \textbf{x}[1] &=& y \\ + * \textbf{x}[2] &=& \dot{y} + * \end{array} + * \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[ - \begin{array}{lcl} - \textbf{z}[0] &=& x \\ - \textbf{z}[1] &=& y - \end{array} - \f] - + * \begin{array}{lcl} + * \textbf{z}[0] &=& x \\ + * \textbf{z}[1] &=& y + * \end{array} + * \f] * Some noise is added to the measurement vector to simulate a sensor which is * not perfect. */ diff --git a/example/kalman/ukf-nonlinear-complex-example.cpp b/example/kalman/ukf-nonlinear-complex-example.cpp index 78db38737c..0b0904c36c 100644 --- a/example/kalman/ukf-nonlinear-complex-example.cpp +++ b/example/kalman/ukf-nonlinear-complex-example.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * 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 ukf-nonlinear-complex-example.cpp * Example of a complex non-linear use-case of the Unscented Kalman Filter (UKF). @@ -36,43 +34,43 @@ * As such, it can be modeled using a bicycle model. * * The state vector of the UKF is: - * \f[ - \begin{array}{lcl} - \textbf{x}[0] &=& x \\ - \textbf{x}[1] &=& y \\ - \textbf{x}[2] &=& \theta - \end{array} - \f] - where \f$ \theta \f$ is the heading of the robot. - - The measurement \f$ \textbf{z} \f$ corresponds to the distance and relative orientation of the - robot with different landmarks. Be \f$ p_x^i \f$ and \f$ p_y^i \f$ the position of the \f$ i^{th} \f$ landmark - along the x and y axis, the measurement vector can be written as: - \f[ - \begin{array}{lcl} - \textbf{z}[2i] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\ - \textbf{z}[2i+1] &=& \tan^{-1}{\frac{p_y^i - y}{p_x^i - x}} - \theta - \end{array} - \f] - + * \f[ + * \begin{array}{lcl} + * \textbf{x}[0] &=& x \\ + * \textbf{x}[1] &=& y \\ + * \textbf{x}[2] &=& \theta + * \end{array} + * \f] + * where \f$ \theta \f$ is the heading of the robot. + * + * The measurement \f$ \textbf{z} \f$ corresponds to the distance and relative orientation of the + * robot with different landmarks. Be \f$ p_x^i \f$ and \f$ p_y^i \f$ the position of the \f$ i^{th} \f$ landmark + * along the x and y axis, the measurement vector can be written as: + * \f[ + * \begin{array}{lcl} + * \textbf{z}[2i] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\ + * \textbf{z}[2i+1] &=& \tan^{-1}{\frac{p_y^i - y}{p_x^i - x}} - \theta + * \end{array} + * \f] + * * Some noise is added to the measurement vector to simulate measurements which are * not perfect. - - The mean of several angles must be computed in the Unscented Transform. The definition we chose to use - is the following: - - \f$ mean(\boldsymbol{\theta}) = atan2 (\frac{\sum_{i=1}^n \sin{\theta_i}}{n}, \frac{\sum_{i=1}^n \cos{\theta_i}}{n}) \f$ - - As the Unscented Transform uses a weighted mean, the actual implementation of the weighted mean - of several angles is the following: - - \f$ mean_{weighted}(\boldsymbol{\theta}) = atan2 (\sum_{i=1}^n w_m^i \sin{\theta_i}, \sum_{i=1}^n w_m^i \cos{\theta_i}) \f$ - - where \f$ w_m^i \f$ is the weight associated to the \f$ i^{th} \f$ measurements for the weighted mean. - - Additionnally, the addition and substraction of angles must be carefully done, as the result - must stay in the interval \f$[- \pi ; \pi ]\f$ or \f$[0 ; 2 \pi ]\f$ . We decided to use - the interval \f$[- \pi ; \pi ]\f$ . + * + * The mean of several angles must be computed in the Unscented Transform. The definition we chose to use + * is the following: + * + * \f$ mean(\boldsymbol{\theta}) = atan2 (\frac{\sum_{i=1}^n \sin{\theta_i}}{n}, \frac{\sum_{i=1}^n \cos{\theta_i}}{n}) \f$ + * + * As the Unscented Transform uses a weighted mean, the actual implementation of the weighted mean + * of several angles is the following: + * + * \f$ mean_{weighted}(\boldsymbol{\theta}) = atan2 (\sum_{i=1}^n w_m^i \sin{\theta_i}, \sum_{i=1}^n w_m^i \cos{\theta_i}) \f$ + * + * where \f$ w_m^i \f$ is the weight associated to the \f$ i^{th} \f$ measurements for the weighted mean. + * + * Additionally, the addition and subtraction of angles must be carefully done, as the result + * must stay in the interval \f$[- \pi ; \pi ]\f$ or \f$[0 ; 2 \pi ]\f$ . We decided to use + * the interval \f$[- \pi ; \pi ]\f$ . */ // UKF includes diff --git a/example/kalman/ukf-nonlinear-example.cpp b/example/kalman/ukf-nonlinear-example.cpp index 7305dec272..35d1307a22 100644 --- a/example/kalman/ukf-nonlinear-example.cpp +++ b/example/kalman/ukf-nonlinear-example.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * 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 ukf-nonlinear-example.cpp * Example of a simple non-linear use-case of the Unscented Kalman Filter (UKF). diff --git a/modules/core/include/visp3/core/vpUKSigmaDrawerAbstract.h b/modules/core/include/visp3/core/vpUKSigmaDrawerAbstract.h index f43dadbb69..f6313fed8d 100644 --- a/modules/core/include/visp3/core/vpUKSigmaDrawerAbstract.h +++ b/modules/core/include/visp3/core/vpUKSigmaDrawerAbstract.h @@ -31,8 +31,8 @@ * Display a point cloud using PCL library. */ -#ifndef _vpUKSigmaDrawerAbstract_h_ -#define _vpUKSigmaDrawerAbstract_h_ +#ifndef VP_UK_SIGMA_DRAWER_ABSTRACT_H +#define VP_UK_SIGMA_DRAWER_ABSTRACT_H #include diff --git a/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h b/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h index e52e0c7141..74e84042c3 100644 --- a/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h +++ b/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h @@ -31,8 +31,8 @@ * Display a point cloud using PCL library. */ -#ifndef _vpUKSigmaDrawerMerwe_h_ -#define _vpUKSigmaDrawerMerwe_h_ +#ifndef VP_UK_SIGMA_DRAWER_MERWE_H +#define VP_UK_SIGMA_DRAWER_MERWE_H #include diff --git a/modules/core/include/visp3/core/vpUnscentedKalman.h b/modules/core/include/visp3/core/vpUnscentedKalman.h index e1ade56c20..d44a43cd67 100644 --- a/modules/core/include/visp3/core/vpUnscentedKalman.h +++ b/modules/core/include/visp3/core/vpUnscentedKalman.h @@ -31,8 +31,8 @@ * Display a point cloud using PCL library. */ -#ifndef _vpUnscentedKalman_h_ -#define _vpUnscentedKalman_h_ +#ifndef VP_UNSCENTED_KALMAN_H +#define VP_UNSCENTED_KALMAN_H #include diff --git a/tutorial/kalman/tutorial-ukf.cpp b/tutorial/kalman/tutorial-ukf.cpp index 043ae8d3b7..2f552719a2 100644 --- a/tutorial/kalman/tutorial-ukf.cpp +++ b/tutorial/kalman/tutorial-ukf.cpp @@ -1,35 +1,3 @@ -/**************************************************************************** - * - * 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.cpp * Tutorial on how to use the Unscented Kalman Filter (UKF) on a complex non-linear use-case. * The system is an object, whose coordinate frame origin is the point O, on which are sticked four markers. @@ -38,25 +6,25 @@ * fixed to the ceiling. * * The state vector of the UKF is: - * \f[ - \begin{array}{lcl} - \textbf{x}[0] &=& {}^WX_x \\ - \textbf{x}[1] &=& {}^WX_y \\ - \textbf{x}[2] &=& {}^WX_z \\ - \textbf{x}[3] &=& \omega \Delta t - \end{array} - \f] - - The measurement \f$ \textbf{z} \f$ corresponds to the coordinates in pixels of the different markers. - Be \f$ u_i \f$ and \f$ v_i \f$ the horizontal and vertical pixel coordinates of the \f$ i^{th} \f$ marker. - The measurement vector can be written as: - \f[ - \begin{array}{lcl} - \textbf{z}[2i] &=& u_i \\ - \textbf{z}[2i+1] &=& v_i - \end{array} - \f] - + * \f[ + * \begin{array}{lcl} + * \textbf{x}[0] &=& {}^WX_x \\ + * \textbf{x}[1] &=& {}^WX_y \\ + * \textbf{x}[2] &=& {}^WX_z \\ + * \textbf{x}[3] &=& \omega \Delta t + * \end{array} + * \f] + * + * The measurement \f$ \textbf{z} \f$ corresponds to the coordinates in pixels of the different markers. + * Be \f$ u_i \f$ and \f$ v_i \f$ the horizontal and vertical pixel coordinates of the \f$ i^{th} \f$ marker. + * The measurement vector can be written as: + * \f[ + * \begin{array}{lcl} + * \textbf{z}[2i] &=& u_i \\ + * \textbf{z}[2i+1] &=& v_i + * \end{array} + * \f] + * * Some noise is added to the measurement vector to simulate measurements which are * not perfect. */