diff --git a/doc/tutorial/misc/tutorial-ukf.dox b/doc/tutorial/misc/tutorial-ukf.dox index 6ade91180e..f8e0bb1b8e 100644 --- a/doc/tutorial/misc/tutorial-ukf.dox +++ b/doc/tutorial/misc/tutorial-ukf.dox @@ -13,11 +13,13 @@ frame is denoted \f$ {F}_C \f$. The object is supposed plane and having four mar The equations that describe the motion of the object in the world frame are the following: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} {}^W \textbf{X}_x &=& R cos(\omega t + \phi) \\ {}^W \textbf{X}_y &=& R sin(\omega t + \phi) \\ {}^W \textbf{X}_z &=& constant -\f} + \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 radius of the revolution around the origin of the world frame. @@ -38,10 +40,12 @@ The first step of the UKF is the prediction step. During this step, some particu \f$ \chi \f$, are drawn along with associated weights \f$ \textbf{w}^m \f$ for the computation of the mean and \f$ \textbf{w}^c \f$ for the computation of the covariance: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} \chi &=& sigma-function(\textbf{x}, \textbf{P}) \\ \textbf{w}^m, \textbf{w}^c &=& weight-function(n, parameters) -\f} + \end{array} +\f] There are different ways of drawing the sigma points and associated weights in the litterature, such as the one proposed by Julier or the one proposed by E. A. Wan and R. van der Merwe in \cite Merwe00. @@ -55,11 +59,13 @@ to project them forward in time, forming the new prior: Finally, we apply the Unscented Transform to compute the mean \f$ \boldsymbol{\mu} \f$ and covariance \f$ \overline{\textbf{P}} \f$ of the prior: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} \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} - \f} + \end{array} +\f] where \f$ \textbf{Q} \f$ is the covariance of the error introduced by the process function. @@ -71,11 +77,13 @@ the prior into measurements using the measurement function \f$ h: {R}^n \righta Then, we use once again the Unscented Transform to compute the mean \f$ \boldsymbol{\mu}_z \in {R}^m \f$ and the covariance \f$ \textbf{P}_z \in {R}^{m\text{ x }m} \f$ of these points: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} \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} -\f} + \end{array} +\f] where \f$ \textbf{R} \f$ is the measurement covariance matrix. @@ -93,10 +101,12 @@ The Kalman's gain is then defined as: Finally, we can compute the new state estimate \f$ \textbf{x} \f$ and the new covariance \f$ \textbf{P} \f$: -\f{eqnarray*}{ - \textbf{x} &=& \boldsymbol{\mu} + \textbf{K} \textbf{y} \\ - \textbf{P} &=& \overline{\textbf{P}} - \textbf{K} \textbf{P}_z \textbf{K}^T -\f} +\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} +\f] \section tuto-ukf-tutorial Explanations about the tutorial @@ -122,21 +132,25 @@ Press `Return` to leave the program. For this tutorial, we use the main program tutorial-ukf.cpp . The internal state of the UKF is the 3D position of the object expressed in the world frame, along with the pulsation \f$ \omega \f$ of the motion: -\f{eqnarray*}{ +\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 -\f} + \end{array} +\f] The measurement \f$ \textbf{z} \f$ corresponds to the perspective projection of the different markers in the image. 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{eqnarray*}{ +\f[ + \begin{array}{lcl} \textbf{z}[2i] &=& u_i \\ \textbf{z}[2i+1] &=& v_i -\f} + \end{array} +\f] Be \f$ \textbf{K}_{intr} \f$ the camera instrinsic parameters matrix defined by: @@ -194,42 +208,52 @@ previous state \f$ \textbf{x}_{t} \f$. As stated in the \ref tuto-ukf-intro, the equations of motion of the object are the following: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} {}^W X_x(t) &=& R cos(\omega t + \phi) \\ {}^W X_y(t) &=& R sin(\omega t + \phi) \\ {}^W X_z(t) &=& constant -\f} + \end{array} +\f] Thus, we have: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} {}^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 -\f} + \end{array} +\f] Which can be rewritten: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} {}^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 -\f} + \end{array} +\f] And can be finally written as: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} {}^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 -\f} + \end{array} +\f] This motivates us to choose the following non-linear process function: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} \textbf{x}[0]_{t + \Delta t} &=& {}^WX_x (t + \Delta t) &=& \textbf{x}[0]_{t} cos(\textbf{x}[3]_{t}) - \textbf{x}[1]_{t} sin(\textbf{x}[3]_{t}) \\ \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} -\f} + \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 48c3bd1e99..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,30 +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{eqnarray*}{ - \textbf{x}[0] &=& x \\ - \textbf{x}[1] &=& \dot{x} \\ - \textbf{x}[1] &=& y \\ - \textbf{x}[2] &=& \dot{y} - \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{eqnarray*}{ - \textbf{z}[0] &=& x \\ - \textbf{z}[1] &=& y - \f} - + * \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 81a3bc25b8..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,39 +34,43 @@ * As such, it can be modeled using a bicycle model. * * The state vector of the UKF is: - * \f{eqnarray*}{ - \textbf{x}[0] &=& x \\ - \textbf{x}[1] &=& y \\ - \textbf{x}[2] &=& \theta - \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{eqnarray*}{ - \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 - \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 @@ -545,7 +547,7 @@ int main(/*const int argc, const char *argv[]*/) const unsigned int nbCmds = cmds.size(); // Initialize the attributes of the UKF - std::shared_ptr drawer = std::make_shared(3, 0.00001, 2., 0, stateResidual, stateAdd); + std::shared_ptr drawer = std::make_shared(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/example/kalman/ukf-nonlinear-example.cpp b/example/kalman/ukf-nonlinear-example.cpp index f362f4a8e5..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). @@ -43,20 +41,24 @@ * and the y-axis to the altitude. * * The state vector of the UKF corresponds to a constant velocity model and can be written as: - * \f{eqnarray*}{ + * \f[ + \begin{array}{lcl} \textbf{x}[0] &=& x \\ \textbf{x}[1] &=& \dot{x} \\ \textbf{x}[1] &=& y \\ \textbf{x}[2] &=& \dot{y} - \f} + \end{array} + \f] The measurement \f$ \textbf{z} \f$ corresponds to the distance and angle between the ground and the aircraft observed by the radar station. Be \f$ p_x \f$ and \f$ p_y \f$ the position of the radar station along the x and y axis, the measurement vector can be written as: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \textbf{z}[0] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\ \textbf{z}[1] &=& \tan^{-1}{\frac{y - p_y}{x - p_x}} - \f} + \end{array} + \f] * Some noise is added to the measurement vector to simulate a sensor which is * not perfect. 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 ca154cceda..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 @@ -55,12 +55,14 @@ BEGIN_VISP_NAMESPACE Be \f$ \boldsymbol{\mu} \in {R}^n \f$ the mean and \f$ \boldsymbol{\Sigma} \in {R}^{n x n} \f$ the covariance matrix of the input of the algorithm. The algorithm will draw \f$ 2n + 1 \f$ sigma points \f$ \chi_i \in {R}^n \f$ such as: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \chi_0 &=& \boldsymbol{\mu} \\ \chi_i &=& \begin{cases} \boldsymbol{\mu} + \left[ \sqrt{(n + \lambda) \boldsymbol{\Sigma}} \right]_i^T & i = 1 .. n \\ \boldsymbol{\mu} - \left[ \sqrt{(n + \lambda) \boldsymbol{\Sigma}} \right]_{i - n}^T & i = n + 1 .. 2n \end{cases} - \f} + \end{array} + \f] where the subscript \f$ i \f$ denotes that we keep the \f$ i^{th} \f$ of the matrix. @@ -90,7 +92,6 @@ BEGIN_VISP_NAMESPACE \b Additionnal \b note: the original author recommended to set \f$ \beta = 2 \f$ for Gaussian problems, \f$ \kappa = 3 - n \f$ and \f$ 0 \leq \alpha \leq 1 \f$, where a larger value for \f$ \alpha \f$ spreads the sigma points further from the mean, which can be a problem for highly non-linear problems. - */ class VISP_EXPORT vpUKSigmaDrawerMerwe : public vpUKSigmaDrawerAbstract { @@ -136,7 +137,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(m_n) + m_kappa) - static_cast(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/core/include/visp3/core/vpUnscentedKalman.h b/modules/core/include/visp3/core/vpUnscentedKalman.h index fe074dbd58..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 @@ -62,10 +62,12 @@ BEGIN_VISP_NAMESPACE Be \f$ \textbf{x} \in {R}^n \f$ the internal state of the UKF and \f$ \textbf{P} \in {R}^{n\text{ x }n} \f$ the process covariance matrix. We have: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \chi &=& sigma-function(\textbf{x}, \textbf{P}) \\ \textbf{w}^m, \textbf{w}^c &=& weight-function(n, parameters) - \f} + \end{array} + \f] There are different ways of drawing the sigma points and associated weights in the litterature, such as the one proposed by Julier or the one proposed by E. A. Wan and R. van der Merwe. @@ -80,11 +82,13 @@ BEGIN_VISP_NAMESPACE Then, we apply the Unscented Transform to compute the mean \f$ \boldsymbol{\mu} \f$ and covariance \f$ \overline{\textbf{P}} \f$ of the prior: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \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} - \f} + \end{array} + \f] where \f$ \textbf{Q} \f$ is the covariance of the error introduced by the process function. @@ -96,11 +100,13 @@ BEGIN_VISP_NAMESPACE Then, we use once again the Unscented Transform to compute the mean \f$ \boldsymbol{\mu}_z \in {R}^m \f$ and the covariance \f$ \textbf{P}_z \in {R}^{m\text{ x }m} \f$ of these points: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \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} - \f} + \end{array} + \f] where \f$ \textbf{R} \f$ is the measurement covariance matrix. @@ -118,11 +124,12 @@ BEGIN_VISP_NAMESPACE Finally, we can compute the new state estimate \f$ \textbf{x} \f$ and the new covariance \f$ \textbf{P} \f$: - \f{eqnarray*}{ + \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 - \f} - + \end{array} + \f] */ class VISP_EXPORT vpUnscentedKalman { @@ -282,6 +289,26 @@ class VISP_EXPORT vpUnscentedKalman m_stateResFunc = stateResFunc; } + /** + * \brief Permit to change the covariance introduced at each prediction step. + * + * \param[in] Q The process covariance matrix. + */ + inline void setProcessCovariance(const vpMatrix &Q) + { + m_Q = Q; + } + + /** + * \brief Permit to change the covariance introduced at each update step. + * + * \param[in] R The measurement covariance matrix. + */ + inline void setMeasurementCovariance(const vpMatrix &R) + { + m_R = R; + } + /** * \brief Perform first the prediction step and then the filtering step. * @@ -312,6 +339,26 @@ class VISP_EXPORT vpUnscentedKalman */ void update(const vpColVector &z); + /** + * \brief Get the estimated (i.e. filtered) covariance of the state. + * + * \return vpMatrix The filtered covariance matrix. + */ + inline vpMatrix getPest() const + { + return m_Pest; + } + + /** + * \brief Get the predicted covariance of the state, i.e. the covariance of the prior. + * + * \return vpMatrix The predicted covariance matrix. + */ + inline vpMatrix getPpred() const + { + return m_Ppred; + } + /** * \brief Get the estimated (i.e. filtered) state. * @@ -378,6 +425,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.*/ @@ -386,7 +434,7 @@ class VISP_EXPORT vpUnscentedKalman std::vector m_wc; /*!< The weights for the covariance computation.*/ std::vector m_Y; /*!< The projection forward in time of the sigma points according to the process model, called the prior.*/ vpColVector m_mu; /*!< The mean of the prior.*/ - vpMatrix m_P; /*!< The covariance matrix of the prior.*/ + vpMatrix m_Ppred; /*!< The covariance matrix of the prior.*/ vpMatrix m_R; /*!< The covariance introduced by performing the update step.*/ std::vector m_Z; /*!< The sigma points of the prior expressed in the measurement space, called the measurement sigma points.*/ vpColVector m_muz; /*!< The mean of the measurement sigma points.*/ diff --git a/modules/core/src/math/kalman/vpUnscentedKalman.cpp b/modules/core/src/math/kalman/vpUnscentedKalman.cpp index 9ce2c18876..1d4fe3f49b 100644 --- a/modules/core/src/math/kalman/vpUnscentedKalman.cpp +++ b/modules/core/src/math/kalman/vpUnscentedKalman.cpp @@ -41,7 +41,8 @@ #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) BEGIN_VISP_NAMESPACE vpUnscentedKalman::vpUnscentedKalman(const vpMatrix &Q, const vpMatrix &R, std::shared_ptr &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) @@ -69,8 +70,23 @@ 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_Ppred; + } + // Drawing the sigma points - m_chi = m_sigmaDrawer->drawSigmaPoints(m_Xest, m_Pest); + m_chi = m_sigmaDrawer->drawSigmaPoints(x, P); // Computation of the attached weights vpUKSigmaDrawerAbstract::vpSigmaPointsWeights weights = m_sigmaDrawer->computeWeights(); @@ -96,7 +112,7 @@ void vpUnscentedKalman::predict(const double &dt, const vpColVector &u) // Computation of the mean and covariance of the prior vpUnscentedTransformResult transformResults = unscentedTransform(m_Y, m_wm, m_wc, m_Q, m_stateResFunc, m_stateMeanFunc); m_mu = transformResults.m_mu; - m_P = transformResults.m_P; + m_Ppred = transformResults.m_P; } void vpUnscentedKalman::update(const vpColVector &z) @@ -124,7 +140,8 @@ 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_Pest = m_Ppred - m_K * m_Pz * m_K.transpose(); + m_hasUpdateBeenCalled = true; } vpUnscentedKalman::vpUnscentedTransformResult vpUnscentedKalman::unscentedTransform(const std::vector &sigmaPoints, diff --git a/modules/core/src/math/matrix/vpMatrix_cholesky.cpp b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp index d8334d9e40..3293cd86e1 100644 --- a/modules/core/src/math/matrix/vpMatrix_cholesky.cpp +++ b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp @@ -193,8 +193,11 @@ vpMatrix vpMatrix::inverseByCholeskyLapack() const 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) { diff --git a/modules/python/doc/rst/tutorials/misc/ukf-linear.rst b/modules/python/doc/rst/tutorials/misc/ukf-linear.rst new file mode 100644 index 0000000000..cf738155dc --- /dev/null +++ b/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 `_. + + + +.. literalinclude:: /examples/ukf-linear-example.py + :language: python diff --git a/modules/python/doc/rst/tutorials/misc/ukf-nonlinear-linear-complex.rst b/modules/python/doc/rst/tutorials/misc/ukf-nonlinear-linear-complex.rst new file mode 100644 index 0000000000..a2396958cb --- /dev/null +++ b/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 `_. + + + +.. literalinclude:: /examples/ukf-nonlinear-complex-example.py + :language: python diff --git a/modules/python/doc/rst/tutorials/misc/ukf-nonlinear-linear.rst b/modules/python/doc/rst/tutorials/misc/ukf-nonlinear-linear.rst new file mode 100644 index 0000000000..2b9bbd595b --- /dev/null +++ b/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 `_. + + + +.. literalinclude:: /examples/ukf-nonlinear-example.py + :language: python diff --git a/modules/python/doc/rst/tutorials/tutorials.rst b/modules/python/doc/rst/tutorials/tutorials.rst index b166c9d656..209b8747c6 100644 --- a/modules/python/doc/rst/tutorials/tutorials.rst +++ b/modules/python/doc/rst/tutorials/tutorials.rst @@ -23,3 +23,14 @@ Tracking :maxdepth: 2 tracking/* + + + +Other tools +----------------------- + +.. toctree:: + :glob: + :maxdepth: 2 + + misc/* diff --git a/modules/python/examples/ukf-linear-example.py b/modules/python/examples/ukf-linear-example.py index 1e27fde959..b27b7dd7ef 100644 --- a/modules/python/examples/ukf-linear-example.py +++ b/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 @@ -40,25 +40,29 @@ The system we are interested in is a system moving on a 2D-plane. The state vector of the UKF is: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \textbf{x}[0] &=& x \\ \textbf{x}[1] &=& \dot{x} \\ \textbf{x}[1] &=& y \\ \textbf{x}[2] &=& \dot{y} - \f} + \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{eqnarray*}{ + \f[ + \begin{array}{lcl} \textbf{z}[0] &=& x \\ \textbf{z}[1] &=& y - \f} + \end{array} + \f] Some noise is added to the measurement vector to simulate a sensor which is 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: @@ -70,12 +74,12 @@ def fx(x: ColVector, dt: float) -> ColVector: """ - @brief Process function that projects in time the internal state of the UKF. + Process function that projects in time the internal state of the UKF. - @param x The internal state of the UKF. - @param dt The sampling time: how far in the future are we projecting x. + :param x: The internal state of the UKF. + :param dt: The sampling time: how far in the future are we projecting x. - @return ColVector The updated internal state, projected in time, also known as the prior. + :return ColVector: The updated internal state, projected in time, also known as the prior. """ return ColVector([ x[0] + dt * x[1], @@ -87,11 +91,11 @@ def fx(x: ColVector, dt: float) -> ColVector: def hx(x: ColVector) -> ColVector: """ - @brief Measurement function that expresses the internal state of the UKF in the measurement space. + Measurement function that expresses the internal state of the UKF in the measurement space. - @param x The internal state of the UKF. + :param x: The internal state of the UKF. - @return ColVector The internal state, expressed in the measurement space. + :return ColVector: The internal state, expressed in the measurement space. """ return ColVector([ x[0], @@ -100,34 +104,34 @@ def hx(x: ColVector) -> ColVector: def add_state_vectors(a, b) -> ColVector: """ - @brief Method that permits to add two state vectors. + Method that permits to add two state vectors. - @param a The first state vector to which another state vector must be added. - @param b The other state vector that must be added to a. + :param a: The first state vector to which another state vector must be added. + :param b: The other state vector that must be added to a. - @return ColVector The sum a + b. + :return ColVector: The sum a + b. """ return a + b def residual_state_vectors(a, b) -> ColVector: """ - @brief Method that permits to substract a state vector to another. + Method that permits to substract a state vector to another. - @param a The first state vector to which another state vector must be substracted. - @param b The other state vector that must be substracted to a. + :param a: The first state vector to which another state vector must be substracted. + :param b: The other state vector that must be substracted to a. - @return ColVector The substraction a - b. + :return ColVector: The substraction a - b. """ return a - b def generate_Q_matrix(dt: float) -> Matrix: """ - @brief Method that generates the process covariance matrix for a process for which the + Method that generates the process covariance matrix for a process for which the state vector can be written as (x, dx/dt)^T - @param dt The sampling period. + :param dt: The sampling period. - @return Matrix The corresponding process covariance matrix. + :return Matrix: The corresponding process covariance matrix. """ return Matrix( [[dt**3/3, dt**2/2, 0, 0], @@ -142,16 +146,16 @@ def generate_Q_matrix(dt: float) -> Matrix: gt_dX = ColVector([gt_dx, gt_dy]) # The displacement vector between two timesteps gt_vx = gt_dx / dt # The velocity along the x-axis gt_vy = gt_dy / dt # The velocity along the y-axis - procVar = 0.000004 # The variance of the process function - sigmaXmeas = 0.05 # The standard deviation of the measurement noise for the x-axis measurement - sigmaYmeas = 0.05 # The standard deviation of the measurement noise for the y-axis measurement + proc_var = 0.000004 # The variance of the process function + sigma_x_meas = 0.05 # The standard deviation of the measurement noise for the x-axis measurement + sigma_y_meas = 0.05 # The standard deviation of the measurement noise for the y-axis measurement # The object that draws the sigma points used by the UKF drawer = UKSigmaDrawerMerwe(n=4, alpha=0.3, beta=2, kappa=-1, resFunc=residual_state_vectors, addFunc=add_state_vectors) P0 = Matrix(np.eye(4) * 1.) # The initial estimate of the state covariance matrix R = Matrix(np.eye(2) * 0.01) # The measurement covariance matrix - Q = generate_Q_matrix(dt) * procVar # The process covariance matrix + Q = generate_Q_matrix(dt) * proc_var # The process covariance matrix ukf = UnscentedKalman(Q, R, drawer, fx, hx) # The Unscented Kalman Filter instance # Initializing the state vector and state covariance matrix estimates @@ -184,12 +188,10 @@ 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) - y_meas = gt_X[1] + np.random.normal(0.0, sigmaYmeas) + x_meas = gt_X[0] + np.random.normal(0.0, sigma_x_meas) + y_meas = gt_X[1] + np.random.normal(0.0, sigma_y_meas) z = ColVector([x_meas, y_meas]) # Filtering using the UKF diff --git a/modules/python/examples/ukf-nonlinear-complex-example.py b/modules/python/examples/ukf-nonlinear-complex-example.py new file mode 100644 index 0000000000..29403ef912 --- /dev/null +++ b/modules/python/examples/ukf-nonlinear-complex-example.py @@ -0,0 +1,528 @@ +############################################################################# +# +# 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 unp.sing 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. +# +# Description: +# ViSP Python example to simulate an image-based visual servo on four 2D points +# +############################################################################# + +""" @example ukf-nonlinear-complex-example.py + Example of a complex non-linear use-case of the Unscented Kalman Filter (UKF). + The system we are interested in is a 4-wheel robot, moving at a low velocity. + As such, it can be modeled unp.sing 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] + + 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$ . +""" + +from visp.core import ColVector, Matrix, UnscentedKalman, UKSigmaDrawerMerwe, Math +import numpy as np +from typing import List +from math import sqrt + +# For the Graphical User Interface +try: + from visp.gui import Plot + has_gui = True +except: + has_gui = False +import numpy as np + +def normalize_angle(angle: float) -> float: + angle_0_to_2pi = Math.modulo(angle, 2. * np.pi) + angle_MinPi_Pi = angle_0_to_2pi + if angle_MinPi_Pi > np.pi: + # Substract 2 PI to be in interval [-Pi; Pi] + angle_MinPi_Pi = angle_MinPi_Pi - 2. * np.pi + return angle_MinPi_Pi + +def measurement_mean(measurements: List[ColVector], wm: List[float]) -> ColVector: + """ + Compute the weighted mean of measurement vectors. + + :param measurements: The measurement vectors, such as v[0] = dist_0 ; v[1] = bearing_0; + v[2] = dist_1 ; v[3] = bearing_1 ... + :param wm: The associated weights. + + :return ColVector: The weighted mean. + """ + nb_points = len(measurements) + size_measurement = measurements[0].size() + nb_landmarks = size_measurement // 2 + mean = np.zeros(size_measurement) + sum_cos = np.zeros(nb_landmarks) + sum_sin = np.zeros(nb_landmarks) + + for i in range(nb_points): + for j in range(nb_landmarks): + mean[2*j] += wm[i] * measurements[i][2*j] + sum_cos[j] += np.cos(measurements[i][(2*j)+1]) * wm[i] + sum_sin[j] += np.sin(measurements[i][(2*j)+1]) * wm[i] + + orientations = np.arctan2(sum_sin, sum_cos) + mean[1::2] = orientations + return ColVector(mean) + +def measurement_residual(meas: ColVector, to_substract: ColVector) -> ColVector: + """ + Compute the substraction between two vectors expressed in the measurement space, + such as v[0] = dist_0 ; v[1] = bearing_0; v[2] = dist_1 ; v[3] = bearing_1 ... + + :param meas: Measurement to which we must substract something. + :param toSubstract: The something we must substract. + + :return ColVector: \b meas - \b toSubstract . + """ + res = meas.numpy() - to_substract.numpy() + res[1::2] = [normalize_angle(angle) for angle in res[1::2]] + return ColVector(res) + +def state_add_vectors(a: ColVector, b: ColVector) -> ColVector: + """ + Compute the addition between two vectors expressed in the state space, + such as v[0] = x ; v[1] = y; v[2] = heading . + + :param a: The first state vector to which another state vector must be added. + :param b: The other state vector that must be added to a. + + :return ColVector: The sum a + b. + """ + add = a + b + return ColVector([add[0], add[1], normalize_angle(add[2])] ) + + +def state_mean_vectors(states: List[ColVector], wm: List[float]) -> ColVector: + """ + Compute the weighted mean of state vectors. + + :param states: The state vectors. + :param wm: The associated weights. + :return ColVector: The weighted mean. + """ + mean = np.zeros(3) + wm_np = np.array(wm) + weighted_x = wm_np * np.array([state[0] for state in states]) + weighted_y = wm_np * np.array([state[1] for state in states]) + mean[0] = np.array(weighted_x).sum() + mean[1] = np.array(weighted_y).sum() + sumCos = (wm_np * np.array([np.cos(state[2]) for state in states])).sum() + sumSin = (wm_np * np.array([np.sin(state[2]) for state in states])).sum() + mean[2] = np.arctan2(sumSin, sumCos) + return ColVector(mean) + +def state_residual_vectors(a, b) -> ColVector: + """ + Compute the substraction between two vectors expressed in the state space, + such as v[0] = x ; v[1] = y; v[2] = heading . + + :param a: The first state vector to which another state vector must be substracted. + :param b: The other state vector that must be substracted to a. + + :return ColVector: The substraction a - b. + """ + res = a - b + return ColVector([res[0], res[1], normalize_angle(res[2])]) + +def fx(x: ColVector, dt: float) -> ColVector: + """ + As the state model {x, y, \f$ \theta \f$} does not contain any velocity + information, it does not evolve without commands. + + :param x: The internal state of the UKF. + :param dt: The sampling time: how far in the future are we projecting x. + + :return ColVector: The updated internal state, projected in time, also known as the prior. + """ + return x + +def generate_turn_commands(v: float, angleStart: float, angleStop: float, nbSteps: int) -> List[ColVector]: + """ + Compute the commands realising a turn at constant linear velocity. + + :param v: Constant linear velocity. + :param angleStart: Starting angle (in degrees). + :param angleStop: Stop angle (in degrees). + :param nbSteps: Number of steps to perform the turn. + :return List[ColVector]: The corresponding list of commands. + """ + cmds = [] + dTheta = Math.rad(angleStop - angleStart) / float(nbSteps - 1) + for i in range(nbSteps): + theta = Math.rad(angleStart) + dTheta * float(i) + cmd = ColVector([v, theta]) + cmds.append(cmd) + return cmds + +def generate_commands() -> List[ColVector]: + """ + Generate the list of commands for the simulation. + + :return List[ColVector]: The list of commands to use in the simulation + """ + cmds = [] + # Starting by a straight line acceleration + nbSteps = 30 + dv = (1.1 - 0.001) / float(nbSteps - 1) + for i in range(nbSteps): + cmd = ColVector([0.001 + float(i) * dv, 0.]) + cmds.append(cmd) + + # Left turn + lastLinearVelocity = cmds[len(cmds) -1][0] + leftTurnCmds = generate_turn_commands(lastLinearVelocity, 0, 2, 15) + cmds.extend(leftTurnCmds) + for i in range(100): + cmds.append(cmds[len(cmds) -1]) + + # Right turn + lastLinearVelocity = cmds[len(cmds) -1][0] + rightTurnCmds = generate_turn_commands(lastLinearVelocity, 2, -2, 15); + cmds.extend(rightTurnCmds) + for i in range(200): + cmds.append(cmds[len(cmds) -1]) + + # Left turn again + lastLinearVelocity = cmds[len(cmds) -1][0] + leftTurnCmds = generate_turn_commands(lastLinearVelocity, -2, 0, 15) + cmds.extend(leftTurnCmds) + for i in range(150): + cmds.append(cmds[len(cmds) -1]) + + lastLinearVelocity = cmds[len(cmds) -1][0] + leftTurnCmds = generate_turn_commands(lastLinearVelocity, 0, 1, 25) + cmds.extend(leftTurnCmds) + for i in range(150): + cmds.append(cmds[len(cmds) -1]) + + return cmds + +class vpBicycleModel: + """ + Class that approximates a 4-wheel robot unp.sing a bicycle model. + """ + def __init__(self, w: float): + """ + Construct a new vpBicycleModel object. + + :param w:The length of the wheelbase. + """ + self._w = w # The length of the wheelbase. + + def compute_motion(self, u: ColVector, x: ColVector, dt: float) -> ColVector: + """ + Models the effect of the command on the state model. + + :param u: The commands. u[0] = velocity ; u[1] = steeringAngle . + :param x: The state model. x[0] = x ; x[1] = y ; x[2] = heading + :param dt: The period. + :return ColVector: The state model after applying the command. + """ + heading = x[2] + vel = u[0] + steeringAngle = u[1] + distance = vel * dt + + if (abs(steeringAngle) > 0.001): + # The robot is turning + beta = (distance / self._w) * np.tan(steeringAngle) + radius = self._w / np.tan(steeringAngle) + sinh = np.sin(heading) + sinhb = np.sin(heading + beta) + cosh = np.cos(heading) + coshb = np.cos(heading + beta) + motion = ColVector([ + -radius * sinh + radius * sinhb, + radius * cosh - radius * coshb, + beta + ]) + return motion + else: + # The robot is moving in straight line + motion = ColVector([ + distance * np.cos(heading), + distance * np.sin(heading), + 0. + ]) + return motion + + def move(self, u: ColVector, x: ColVector, dt: float) -> ColVector: + """ + Models the effect of the command on the state model. + + :param u: The commands. u[0] = velocity ; u[1] = steeringAngle . + :param x: The state model. x[0] = x ; x[1] = y ; x[2] = heading + :param dt: The period. + :return ColVector: The state model after applying the command. + """ + motion = self.compute_motion(u, x, dt) + newX = x + motion + normalizedAngle = normalize_angle(newX[2]) + return ColVector([newX[0], newX[1], normalizedAngle]) + +class LandmarkMeasurements: + """ + Class that permits to convert the position + heading of the 4-wheel + robot into measurements from a landmark. + """ + def __init__(self, x: float, y: float, range_std: float, rel_angle_std: float): + """ + Construct a new LandmarkMeasurements object. + + :param x: The position along the x-axis of the landmark. + :param y: The position along the y-axis of the landmark. + :param range_std: The standard deviation of the range measurements. + :param rel_angle_std: The standard deviation of the relative angle measurements. + """ + self._x = x # The position along the x-axis of the landmark + self._y = y # The position along the y-axis of the landmark + self._range_std = range_std # The standard deviation of the range measurement + self._rel_angle_std = rel_angle_std # The standard deviation of the relative angle measurement + np.random.seed(4224) + + def state_to_measurement(self, chi: ColVector) -> ColVector: + """ + Convert the prior of the UKF into the measurement space. + + :param chi: The prior. + :return ColVector: The prior expressed in the measurement space. + """ + dx = self._x - chi[0] + dy = self._y - chi[1] + meas = ColVector([sqrt(dx * dx + dy * dy), normalize_angle(np.arctan2(dy, dx) - chi[2])]) + return meas + + def measure_gt(self, pos: ColVector) -> ColVector: + """ + Perfect measurement of the range and relative orientation of the robot + located at pos. + + :param pos: The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading. + :return ColVector: [0] the range [1] the relative orientation of the robot. + """ + dx = self._x - pos[0] + dy = self._y - pos[1] + range = sqrt(dx * dx + dy * dy) + orientation = normalize_angle(np.arctan2(dy, dx) - pos[2]) + measurements = ColVector([range, orientation]) + return measurements + + def measure_with_noise(self, pos: ColVector) -> ColVector: + """ + Noisy measurement of the range and relative orientation that + correspond to pos. + + :param pos: The actual position of the robot (pos[0]: x ; pos[1] = y ; pos[2] = heading). + :return ColVector: [0] the range [1] the relative orientation. + """ + measurementsGT = self.measure_gt(pos) + measurementsNoisy = measurementsGT + range = measurementsNoisy[0] + np.random.normal(0., self._range_std) + relAngle = normalize_angle(measurementsNoisy[1] + np.random.normal(0., self._rel_angle_std)) + return ColVector([range, relAngle]) + + +class LandmarksGrid: + """ + Class that represent a grid of landmarks that measure the distance and + relative orientation of the 4-wheel robot. + """ + def __init__(self, landmarks: List[LandmarkMeasurements]): + """ + Construct a new LandmarksGrid object. + + :param landmarks: The list of landmarks forming the grid. + """ + self._landmarks = landmarks # The list of landmarks forming the grid. + + def state_to_measurement(self, chi: ColVector) -> ColVector: + """ + Convert the prior of the UKF into the measurement space. + + :param chi: The prior. + :return ColVector: The prior expressed in the measurement space. + """ + nbLandmarks = len(self._landmarks) + measurements = np.zeros(2*nbLandmarks) + for i in range (nbLandmarks): + landmarkMeas = self._landmarks[i].state_to_measurement(chi) + measurements[2*i] = landmarkMeas[0] + measurements[(2*i) + 1] = landmarkMeas[1] + return ColVector(measurements) + + def measure_gt(self, pos: ColVector) -> ColVector: + """ + Perfect measurement from each landmark of the range and relative orientation of the robot + located at pos. + + :param pos: The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading. + :return ColVector: n x ([0] the range [1] the relative orientation of the robot), where + n is the number of landmarks. + """ + nbLandmarks = len(self._landmarks) + measurements = np.zeros(2*nbLandmarks) + for i in range(nbLandmarks): + landmarkMeas = self._landmarks[i].measure_gt(pos) + measurements[2*i] = landmarkMeas[0] + measurements[(2*i) + 1] = landmarkMeas[1] + return ColVector(measurements) + + def measure_with_noise(self, pos: ColVector) -> ColVector: + """ + Noisy measurement from each landmark of the range and relative orientation that + correspond to pos. + + :param pos: The actual position of the robot (pos[0]: x ; pos[1] = y ; pos[2] = heading). + :return ColVector: n x ([0] the range [1] the relative orientation of the robot), where + n is the number of landmarks. + """ + nbLandmarks = len(self._landmarks) + measurements = np.zeros(2*nbLandmarks) + for i in range(nbLandmarks): + landmarkMeas = self._landmarks[i].measure_with_noise(pos) + measurements[2*i] = landmarkMeas[0] + measurements[(2*i) + 1] = landmarkMeas[1] + return ColVector(measurements) + +if __name__ == '__main__': + dt = 0.1 # Period of 0.1s + step = 1. # Number of update of the robot position between two UKF filtering + sigma_range = 0.3 # Standard deviation of the range measurement: 0.3m + sigma_bearing = Math.rad(0.5) # Standard deviation of the bearing angle: 0.5deg + wheelbase = 0.5 # Wheelbase of 0.5m + process_variance = 0.0001 + positions = [ (5, 10) , (10, 5), (15, 15), (20, 5), (0, 30), (50, 30), (40, 10)] # Positions of the landmarks constituing the grid + landmarks = [LandmarkMeasurements(x, y, sigma_range, sigma_bearing) for x,y in positions] # Vector of landmarks constituing the grid + nbLandmarks = len(landmarks) # Number of landmarks constituing the grid + cmds = generate_commands() + nb_cmds = len(cmds) + + # Creation of the simulated grid of landmarks and robot + grid = LandmarksGrid(landmarks) + robot = vpBicycleModel(wheelbase) + + # The object that draws the sigma points used by the UKF + drawer = UKSigmaDrawerMerwe(n=3, alpha=0.1, beta=2, kappa=0, resFunc=state_residual_vectors, addFunc=state_add_vectors) + + # The matrices require for the construction of the Unscented filter + P0 = Matrix([[0.1, 0., 0.], + [0., 0.1, 0.], + [0., 0., 0.05]]) # The initial estimate of the state covariance matrix + R1landmark = Matrix([[sigma_range * sigma_range, 0], [0, sigma_bearing*sigma_bearing]]) # The measurement covariance matrix for 1 landmark + 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) + + Q = Matrix() # The process covariance matrix + Q.eye(3) + Q = Q * process_variance + X0 = ColVector([2., 6., 0.3]) # robot_x, robot_y, robot_orientation(rad) + + # Creation of the Unscented Kalman filter + ukf = UnscentedKalman(Q, R, drawer, fx, grid.state_to_measurement) # The Unscented Kalman Filter instance + + # Initializing the state vector and state covariance matrix estimates + ukf.init(X0, P0) + ukf.setCommandStateFunction(robot.compute_motion) + ukf.setMeasurementMeanFunction(measurement_mean) + ukf.setMeasurementResidualFunction(measurement_residual) + ukf.setStateAddFunction(state_add_vectors) + ukf.setStateMeanFunction(state_mean_vectors) + ukf.setStateResidualFunction(state_residual_vectors) + + # Initializing the Graphical User Interface if the needed libraries are available + if has_gui: + num_plots = 1 + plot = Plot(num_plots) + plot.initGraph(0, 2) + plot.setTitle(0, "Position of the robot") + plot.setUnitX(0, "Position along x(m)") + plot.setUnitY(0, "Position along y (m)") + plot.setLegend(0, 0, "GT") + plot.setLegend(0, 1, "Filtered") + + robot_pos = X0 + for i in range(nb_cmds): + robot_pos = robot.move(cmds[i], robot_pos, dt / step) + + if (i % int(step) == 0): + # Perform the measurement + z = grid.measure_with_noise(robot_pos) + + # Use the UKF to filter the measurement + ukf.filter(z, dt, cmds[i]) + + if has_gui: + # Plot the filtered state + Xest = ukf.getXest() + plot.plot(0, 1, Xest[0], Xest[1]) + + # Update the GUI if available + if has_gui: + # Plot the ground truth + plot.plot(0, 0, robot_pos[0], robot_pos[1]) + + print('Finished') + input('Press enter to quit') diff --git a/modules/python/examples/ukf-nonlinear-example.py b/modules/python/examples/ukf-nonlinear-example.py index 5f10197dee..bd7062f3a8 100644 --- a/modules/python/examples/ukf-nonlinear-example.py +++ b/modules/python/examples/ukf-nonlinear-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 @@ -44,29 +44,32 @@ and the y-axis to the altitude. The state vector of the UKF is: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \textbf{x}[0] &=& x \\ \textbf{x}[1] &=& \dot{x} \\ \textbf{x}[1] &=& y \\ \textbf{x}[2] &=& \dot{y} - \f} + \end{array} + \f] The measurement \f$ \textbf{z} \f$ corresponds to the distance and angle between the ground and the aircraft observed by the radar station. Be \f$ p_x \f$ and \f$ p_y \f$ the position of the radar station along the x and y axis, the measurement vector can be written as: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \textbf{z}[0] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\ \textbf{z}[1] &=& \tan^{-1}{\frac{y - p_y}{x - p_x}} - \f} + \end{array} + \f] Some noise is added to the measurement vector to simulate a sensor which is not perfect. """ -from visp.core import ColVector, Matrix, UnscentedKalman, UKSigmaDrawerMerwe, GaussRand, Math +from visp.core import ColVector, Matrix, UnscentedKalman, UKSigmaDrawerMerwe, Math import numpy as np from typing import List -from math import cos, sin # For the Graphical User Interface try: @@ -76,80 +79,77 @@ has_gui = False import numpy as np -def normalizeAngle(angle: float) -> float: - angleIn0to2pi = Math.modulo(angle, 2. * np.pi) - angleInMinPiPi = angleIn0to2pi - if angleInMinPiPi > np.pi: +def normalize_angle(angle: float) -> float: + angle_0_to_2pi = Math.modulo(angle, 2. * np.pi) + angle_MinPi_Pi = angle_0_to_2pi + if angle_MinPi_Pi > np.pi: # Substract 2 PI to be in interval [-Pi; Pi] - angleInMinPiPi = angleInMinPiPi - 2. * np.pi - return angleInMinPiPi + angle_MinPi_Pi = angle_MinPi_Pi - 2. * np.pi + return angle_MinPi_Pi -def measurementMean(measurements: List[ColVector], wm: List[float]) -> ColVector: +def measurement_mean(measurements: List[ColVector], wm: List[float]) -> ColVector: """ - @brief Compute the weighted mean of measurement vectors. + Compute the weighted mean of measurement vectors. - @param[in] measurements The measurement vectors, such as measurements[i][0] = range and + :param measurements: The measurement vectors, such as measurements[i][0] = range and measurements[i][1] = elevation_angle. - @param[in] wm The associated weights. + :param wm: The associated weights. - @return vpColVector + :return vpColVector: The weighted mean of the measurement vectors. """ - nbPoints = len(measurements) - sumCos = 0. - sumSin = 0. - meanRange = 0. - for i in range(nbPoints): - meanRange += wm[i] * measurements[i][0] - sumCos += wm[i] * cos(measurements[i][1]) - sumSin += wm[i] * sin(measurements[i][1]) + wm_np = np.array(wm) + ranges = np.array([meas[0] for meas in measurements]) + elev_angles = np.array([meas[1] for meas in measurements]) + meanRange = (wm_np * ranges).sum() + sum_cos = (wm_np * np.cos(elev_angles)).sum() + sum_sin = (wm_np * np.sin(elev_angles)).sum() + mean_angle = np.arctan2(sum_sin, sum_cos) - meanAngle = np.arctan2(sumSin, sumCos) + return ColVector([meanRange, mean_angle]) - return ColVector([meanRange, meanAngle]) - -def measurementResidual(meas: ColVector, toSubstract: ColVector) -> ColVector: +def measurementResidual(meas: ColVector, to_substract: ColVector) -> ColVector: """ - @brief Compute the substraction between two vectors expressed in the measurement space, + Compute the substraction between two vectors expressed in the measurement space, such as v[0] = range ; v[1] = elevation_angle - @param[in] meas Measurement to which we must substract something. - @param[in] toSubstract The something we must substract. + :param meas: Measurement to which we must substract something. + :param toSubstract: The something we must substract. - @return vpColVector \b meas - \b toSubstract . + :return vpColVector: \b meas - \b toSubstract . """ - resTemp = meas - toSubstract - return ColVector([resTemp[0], normalizeAngle(resTemp[1])]) + res_temp = meas - to_substract + return ColVector([res_temp[0], normalize_angle(res_temp[1])]) -def add_state_vectors(a, b) -> ColVector: +def state_add_vectors(a, b) -> ColVector: """ - @brief Method that permits to add two state vectors. + Method that permits to add two state vectors. - @param a The first state vector to which another state vector must be added. - @param b The other state vector that must be added to a. + :param a: The first state vector to which another state vector must be added. + :param b: The other state vector that must be added to a. - @return ColVector The sum a + b. + :return ColVector: The sum a + b. """ return a + b -def residual_state_vectors(a, b) -> ColVector: +def state_residual_vectors(a, b) -> ColVector: """ - @brief Method that permits to substract a state vector to another. + Method that permits to substract a state vector to another. - @param a The first state vector to which another state vector must be substracted. - @param b The other state vector that must be substracted to a. + :param a: The first state vector to which another state vector must be substracted. + :param b: The other state vector that must be substracted to a. - @return ColVector The substraction a - b. + :return ColVector: The substraction a - b. """ return a - b def fx(x: ColVector, dt: float) -> ColVector: """ - @brief Process function that projects in time the internal state of the UKF. + Process function that projects in time the internal state of the UKF. - @param x The internal state of the UKF. - @param dt The sampling time: how far in the future are we projecting x. + :param x: The internal state of the UKF. + :param dt: The sampling time: how far in the future are we projecting x. - @return ColVector The updated internal state, projected in time, also known as the prior. + :return ColVector: The updated internal state, projected in time, also known as the prior. """ return ColVector([ x[0] + dt * x[1], @@ -160,106 +160,106 @@ def fx(x: ColVector, dt: float) -> ColVector: class vpRadarStation: """ - @brief Class that permits to convert the position of the aircraft into + Class that permits to convert the position of the aircraft into range and elevation angle measurements. """ - def __init__(self, x, y, range_std, elev_angle_std): + def __init__(self, x: float, y: float, range_std: float, elev_angle_std: float): """ - @brief Construct a new vpRadarStation + Construct a new vpRadarStation - @param x The position on the ground of the radar. - @param y The altitude of the radar. - @param range_std The standard deviation of the range measurements. - @param elev_angle_std The standard deviation of the elevation angle measurements. + :param x: The position on the ground of the radar. + :param y: The altitude of the radar. + :param range_std: The standard deviation of the range measurements. + :param elev_angle_std: The standard deviation of the elevation angle measurements. """ - self.m_x = x - self.m_y = y - self.m_stdevRange = range_std - self.m_stdevElevAngle = elev_angle_std + self._x = x + self._y = y + self._stdevRange = range_std + self._stdevElevAngle = elev_angle_std - def state_to_measurement(self, x) -> ColVector: + def state_to_measurement(self, x: ColVector) -> ColVector: """ - @brief Measurement function that expresses the internal state of the UKF in the measurement space. + Measurement function that expresses the internal state of the UKF in the measurement space. - @param x The internal state of the UKF. + :param x: The internal state of the UKF. - @return ColVector The internal state, expressed in the measurement space. + :return ColVector: The internal state, expressed in the measurement space. """ - dx = x[0] - self.m_x - dy = x[2] - self.m_y + dx = x[0] - self._x + dy = x[2] - self._y range = np.sqrt(dx * dx + dy * dy) elev_angle = np.arctan2(dy, dx) return ColVector([range, elev_angle]) - def measureGT(self, pos) -> ColVector: + def measure_gt(self, pos: ColVector) -> ColVector: """ - @brief Perfect measurement of the range and elevation angle that + Perfect measurement of the range and elevation angle that correspond to pos. - @param pos The actual position of the aircraft (pos[0]: projection of the position + :param pos: The actual position of the aircraft (pos[0]: projection of the position on the ground, pos[1]: altitude). - @return ColVector [0] the range [1] the elevation angle. + :return ColVector: [0] the range [1] the elevation angle. """ - dx = pos[0] - self.m_x - dy = pos[1] - self.m_y + dx = pos[0] - self._x + dy = pos[1] - self._y range = np.sqrt(dx * dx + dy * dy) elev_angle = np.arctan2(dy, dx) return ColVector([range, elev_angle]) - def measureWithNoise(self, pos: ColVector) -> ColVector: + def measure_with_noise(self, pos: ColVector) -> ColVector: """ - @brief Noisy measurement of the range and elevation angle that + Noisy measurement of the range and elevation angle that correspond to pos. - @param pos The actual position of the aircraft (pos[0]: projection of the position + :param pos: The actual position of the aircraft (pos[0]: projection of the position on the ground, pos[1]: altitude). - @return vpColVector [0] the range [1] the elevation angle. + :return vpColVector: [0] the range [1] the elevation angle. """ - measurementsGT = self.measureGT(pos) - measurementsNoisy = ColVector([measurementsGT[0] + np.random.normal(0., self.m_stdevRange), measurementsGT[1] + np.random.normal(0., self.m_stdevElevAngle)]) - return measurementsNoisy + measurements_GT = self.measure_gt(pos) + measurements_noisy = ColVector([measurements_GT[0] + np.random.normal(0., self._stdevRange), measurements_GT[1] + np.random.normal(0., self._stdevElevAngle)]) + return measurements_noisy class vpACSimulator: """ - @brief Class to simulate a flying aircraft. + Class to simulate a flying aircraft. """ def __init__(self, X0: ColVector, vel: ColVector, vel_std: float): """ - @brief Construct a new vpACSimulator object. + Construct a new vpACSimulator object. - @param X0 Initial position of the aircraft. - @param vel Velocity of the aircraft. - @param vel_std Standard deviation of the variation of the velocity. + :param X0: Initial position of the aircraft. + :param vel: Velocity of the aircraft. + :param vel_std: Standard deviation of the variation of the velocity. """ - self.m_pos = X0 # Position of the simulated aircraft - self.m_vel = vel # Velocity of the simulated aircraft + self._pos = X0 # Position of the simulated aircraft + self._vel = vel # Velocity of the simulated aircraft np.random.seed(4224) - self.m_stdevVel = vel_std # Standard deviation of the random generator for slight variations of the velocity of the aircraft + self._stdevVel = vel_std # Standard deviation of the random generator for slight variations of the velocity of the aircraft def update(self, dt: float) -> ColVector: """ - \brief Compute the new position of the aircraft after dt seconds have passed + Compute the new position of the aircraft after dt seconds have passed since the last update. - \param[in] dt Period since the last update. - \return vpColVector The new position of the aircraft. + :param dt: Period since the last update. + :return ColVector: The new position of the aircraft. """ - dx_temp = self.m_vel * dt - dx = ColVector([dx_temp[0] + np.random.normal(0., self.m_stdevVel) * dt, dx_temp[1] + np.random.normal(0., self.m_stdevVel) * dt]) - self.m_pos += dx - return self.m_pos + dx_temp = self._vel * dt + dx = ColVector([dx_temp[0] + np.random.normal(0., self._stdevVel) * dt, dx_temp[1] + np.random.normal(0., self._stdevVel) * dt]) + self._pos += dx + return self._pos def generate_Q_matrix(dt: float) -> Matrix: """ - @brief Method that generates the process covariance matrix for a process for which the + Method that generates the process covariance matrix for a process for which the state vector can be written as (x, dx/dt)^T - @param dt The sampling period. + :param dt: The sampling period. - @return Matrix The corresponding process covariance matrix. + :return Matrix: The corresponding process covariance matrix. """ return Matrix( [[dt**3/3, dt**2/2, 0, 0], @@ -269,7 +269,7 @@ def generate_Q_matrix(dt: float) -> Matrix: def generate_P0_matrix() -> Matrix: """ - @brief Method that generates the intial guess of the state covariance matrix. + Method that generates the intial guess of the state covariance matrix. @return Matrix The corresponding state covariance matrix. """ @@ -285,26 +285,26 @@ def generate_P0_matrix() -> Matrix: gt_Y_init = 1000. # Ground truth initial position along the Y-axis, in meters gt_vX_init = 100. # The velocity along the x-axis gt_vY_init = 5. # The velocity along the y-axis - procVar = 0.1 # The variance of the process function - sigmaRange = 5 # Standard deviation of the range measurement: 5m - sigmaElevAngle = Math.rad(0.5) # Standard deviation of the elevation angle measurent: 0.5deg - stdevAircraftVelocity = 0.2; # Standard deviation of the velocity of the simulated aircraft, + proc_var = 0.1 # The variance of the process function + sigma_range = 5 # Standard deviation of the range measurement: 5m + sigma_elev_angle = Math.rad(0.5) # Standard deviation of the elevation angle measurent: 0.5deg + stdev_aircraft_velocity = 0.2; # Standard deviation of the velocity of the simulated aircraft, # to make it deviate a bit from the constant velocity model # The object that draws the sigma points used by the UKF - drawer = UKSigmaDrawerMerwe(n=4, alpha=0.3, beta=2, kappa=-1, resFunc=residual_state_vectors, addFunc=add_state_vectors) + drawer = UKSigmaDrawerMerwe(n=4, alpha=0.3, beta=2, kappa=-1, resFunc=state_residual_vectors, addFunc=state_add_vectors) # The object that performs radar measurements - radar = vpRadarStation(0., 0., sigmaRange, sigmaElevAngle) + radar = vpRadarStation(0., 0., sigma_range, sigma_elev_angle) P0 = generate_P0_matrix() # The initial estimate of the state covariance matrix - R = Matrix([[sigmaRange * sigmaRange, 0], [0, sigmaElevAngle * sigmaElevAngle]]) # The measurement covariance matrix - Q = generate_Q_matrix(dt) * procVar # The process covariance matrix + R = Matrix([[sigma_range * sigma_range, 0], [0, sigma_elev_angle * sigma_elev_angle]]) # The measurement covariance matrix + Q = generate_Q_matrix(dt) * proc_var # The process covariance matrix ukf = UnscentedKalman(Q, R, drawer, fx, radar.state_to_measurement) # The Unscented Kalman Filter instance # Initializing the state vector and state covariance matrix estimates ukf.init(ColVector([0.9 * gt_X_init, 0.9 * gt_vX_init, 0.9 * gt_Y_init, 0.9 * gt_vY_init]), P0) - ukf.setMeasurementMeanFunction(measurementMean) + ukf.setMeasurementMeanFunction(measurement_mean) ukf.setMeasurementResidualFunction(measurementResidual) # Initializing the Graphical User Interface if the needed libraries are available @@ -332,13 +332,13 @@ def generate_P0_matrix() -> Matrix: ac_pos = ColVector([gt_X_init, gt_Y_init]) # Ground truth position ac_vel = ColVector([gt_vX_init, gt_vY_init]) # Ground truth velocity - ac = vpACSimulator(ac_pos, ac_vel, stdevAircraftVelocity) - gt_Xprec = ColVector([ac_pos[0], ac_pos[1]]) + ac = vpACSimulator(ac_pos, ac_vel, stdev_aircraft_velocity) + gt_X_prev = ColVector([ac_pos[0], ac_pos[1]]) # Previous ground truth position for i in range(500): # Creating noisy measurements gt_X = ac.update(dt) - gt_V = (gt_X - gt_Xprec) / dt - z = radar.measureWithNoise(gt_X) + gt_V = (gt_X - gt_X_prev) / dt + z = radar.measure_with_noise(gt_X) # Filtering using the UKF ukf.filter(z, dt) @@ -361,7 +361,7 @@ def generate_P0_matrix() -> Matrix: plot.plot(3, 1, i, Xest[3]) # Updating last measurement for future computation of the noisy velocity - gt_Xprec = ColVector([gt_X[0], gt_X[1]]) + gt_X_prev = ColVector([gt_X[0], gt_X[1]]) print('Finished') input('Press enter to quit') diff --git a/tutorial/kalman/tutorial-ukf.cpp b/tutorial/kalman/tutorial-ukf.cpp index 616ea05f43..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,21 +6,25 @@ * fixed to the ceiling. * * The state vector of the UKF is: - * \f{eqnarray*}{ - \textbf{x}[0] &=& {}^WX_x \\ - \textbf{x}[1] &=& {}^WX_y \\ - \textbf{x}[2] &=& {}^WX_z \\ - \textbf{x}[3] &=& \omega \Delta t - \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{eqnarray*}{ - \textbf{z}[2i] &=& u_i \\ - \textbf{z}[2i+1] &=& v_i - \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. */