From d09e08bb2e577eb5d7ebe3d42397c5dc54191489 Mon Sep 17 00:00:00 2001 From: rlagneau Date: Thu, 25 Apr 2024 10:08:48 +0200 Subject: [PATCH] [TUTO][DOC] Improvement of the documentation of the tutorials/examples --- .../kalman/tutorial-ukf-linear-example.cpp | 25 ++++++++++- ...tutorial-ukf-nonlinear-complex-example.cpp | 41 ++++++++++++++++++- .../kalman/tutorial-ukf-nonlinear-example.cpp | 28 ++++++++++++- 3 files changed, 91 insertions(+), 3 deletions(-) diff --git a/tutorial/kalman/tutorial-ukf-linear-example.cpp b/tutorial/kalman/tutorial-ukf-linear-example.cpp index 21038da084..0358fae511 100644 --- a/tutorial/kalman/tutorial-ukf-linear-example.cpp +++ b/tutorial/kalman/tutorial-ukf-linear-example.cpp @@ -30,7 +30,30 @@ * *****************************************************************************/ -//! \example tutorial-ukf-linear-example.cpp +/** \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 diff --git a/tutorial/kalman/tutorial-ukf-nonlinear-complex-example.cpp b/tutorial/kalman/tutorial-ukf-nonlinear-complex-example.cpp index 513db68cd5..db8f9a8973 100644 --- a/tutorial/kalman/tutorial-ukf-nonlinear-complex-example.cpp +++ b/tutorial/kalman/tutorial-ukf-nonlinear-complex-example.cpp @@ -30,7 +30,46 @@ * *****************************************************************************/ -//! \example tutorial-ukf-nonlinear-complex-example.cpp +/** \example tutorial-ukf-nonlinear-complex-example.cpp + * 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 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} + + * 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$ . +*/ // UKF includes #include diff --git a/tutorial/kalman/tutorial-ukf-nonlinear-example.cpp b/tutorial/kalman/tutorial-ukf-nonlinear-example.cpp index cc77dc83a7..6d50f9e672 100644 --- a/tutorial/kalman/tutorial-ukf-nonlinear-example.cpp +++ b/tutorial/kalman/tutorial-ukf-nonlinear-example.cpp @@ -30,7 +30,33 @@ * *****************************************************************************/ -//! \example tutorial-ukf-nonlinear-example.cpp +/** \example tutorial-ukf-nonlinear-example.cpp + * Example of a simple non-linear use-case of the Unscented Kalman Filter (UKF). + * The system we are interested in is an aircraft flying in the sky and + * observed by a radar station. + * We consider the plan perpendicular to the ground and passing by both the radar + * station and the aircraft. The x-axis corresponds to the position on the ground + * and the y-axis to the altitude. + * + * 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 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*}{ + \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} + + * Some noise is added to the measurement vector to simulate a sensor which is + * not perfect. +*/ // UKF includes #include