Skip to content

Commit

Permalink
Merge pull request #1411 from rolalaro/feat_ukf_python
Browse files Browse the repository at this point in the history
Unscented Kalman Filter in python
  • Loading branch information
fspindle authored Jun 11, 2024
2 parents f217431 + 39c3954 commit 4ab566c
Show file tree
Hide file tree
Showing 17 changed files with 957 additions and 305 deletions.
76 changes: 50 additions & 26 deletions doc/tutorial/misc/tutorial-ukf.dox
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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.
Expand All @@ -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.

Expand All @@ -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.

Expand All @@ -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

Expand All @@ -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:

Expand Down Expand Up @@ -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:

Expand Down
35 changes: 18 additions & 17 deletions example/kalman/ukf-linear-example.cpp
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
/****************************************************************************
*
/*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2024 by Inria. All rights reserved.
*
Expand Down Expand Up @@ -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.
*/
Expand Down
74 changes: 38 additions & 36 deletions example/kalman/ukf-nonlinear-complex-example.cpp
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
/****************************************************************************
*
/*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2024 by Inria. All rights reserved.
*
Expand Down Expand Up @@ -27,48 +26,51 @@
*
* 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).
* 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}
* \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
Expand Down Expand Up @@ -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<vpUKSigmaDrawerAbstract> drawer = std::make_shared<vpUKSigmaDrawerMerwe>(3, 0.00001, 2., 0, stateResidual, stateAdd);
std::shared_ptr<vpUKSigmaDrawerAbstract> drawer = std::make_shared<vpUKSigmaDrawerMerwe>(3, 0.1, 2., 0, stateResidual, stateAdd);

vpMatrix R1landmark(2, 2, 0.); // The covariance of the noise introduced by the measurement with 1 landmark
R1landmark[0][0] = sigmaRange*sigmaRange;
Expand Down
18 changes: 10 additions & 8 deletions example/kalman/ukf-nonlinear-example.cpp
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
/****************************************************************************
*
/*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2024 by Inria. All rights reserved.
*
Expand Down Expand Up @@ -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).
Expand All @@ -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.
Expand Down
4 changes: 2 additions & 2 deletions modules/core/include/visp3/core/vpUKSigmaDrawerAbstract.h
Original file line number Diff line number Diff line change
Expand Up @@ -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 <vector>

Expand Down
Loading

0 comments on commit 4ab566c

Please sign in to comment.