#include <visp3/core/vpUnscentedKalman.h>

Public Types
typedef std::function< vpColVector(const vpColVector &, const double &)>	vpCommandOnlyFunction

typedef std::function< vpColVector(const vpColVector &, const vpColVector &, const double &)>	vpCommandStateFunction

typedef std::function< vpColVector(const std::vector< vpColVector > &, const std::vector< double > &)>	vpMeanFunction

typedef std::function< vpColVector(const vpColVector &)>	vpMeasurementFunction

typedef std::function< vpColVector(const vpColVector &, const double &)>	vpProcessFunction

typedef std::function< vpColVector(const vpColVector &, const vpColVector &)>	vpAddSubFunction

Public Member Functions
	vpUnscentedKalman (const vpMatrix &Q, const vpMatrix &R, std::shared_ptr< vpUKSigmaDrawerAbstract > &drawer, const vpProcessFunction &f, const vpMeasurementFunction &h)

void	init (const vpColVector &mu0, const vpMatrix &P0)

void	setCommandOnlyFunction (const vpCommandOnlyFunction &b)

void	setCommandStateFunction (const vpCommandStateFunction &bx)

void	setMeasurementMeanFunction (const vpMeanFunction &meanFunc)

void	setMeasurementResidualFunction (const vpAddSubFunction &measResFunc)

void	setStateAddFunction (const vpAddSubFunction &stateAddFunc)

void	setStateMeanFunction (const vpMeanFunction &meanFunc)

void	setStateResidualFunction (const vpAddSubFunction &stateResFunc)

void	setProcessCovariance (const vpMatrix &Q)

void	setMeasurementCovariance (const vpMatrix &R)

void	filter (const vpColVector &z, const double &dt, const vpColVector &u=vpColVector())

void	predict (const double &dt, const vpColVector &u=vpColVector())

void	update (const vpColVector &z)

vpMatrix	getPest () const

vpMatrix	getPpred () const

vpColVector	getXest () const

vpColVector	getXpred () const

Static Public Member Functions
static vpColVector	simpleAdd (const vpColVector &a, const vpColVector &toAdd)

static vpColVector	simpleResidual (const vpColVector &a, const vpColVector &toSubtract)

static vpColVector	simpleMean (const std::vector< vpColVector > &vals, const std::vector< double > &wm)

Detailed Description

This class permits to use Unscented Kalman Filter (UKF) to tackle non-linear problems. Non-linearity can arise in the process function $f: {R}^n \rightarrow {R}^n$ , which makes evolve the internal state $\textbf{x} \in {R}^n$ of the UKF over time, or in the measurement function $h: {R}^n \rightarrow {R}^m$ , which expresses the internal state of the UKF in the measurement space of dimension .

We will briefly explain the principles of the UKF and the maths behind the wheel. We refer the interested readers to the web-book by R. Labbe, chapter 10, for more details.

The UKF is performed in two steps. First, the prediction step, during which we draw the sigma points $\chi$ and compute their corresponding weights $\textbf{w}^m \in {R}^{2n + 1}$ and $\textbf{w}^c \in {R}^{2n + 1}$ . Be $\textbf{x} \in {R}^n$ the internal state of the UKF and $\textbf{P} \in {R}^{n\text{ x }n}$ the process covariance matrix. We have:

$\begin{array}{lcl} \chi &=& sigma-function(\textbf{x}, \textbf{P}) \\ \textbf{w}^m, \textbf{w}^c &=& weight-function(n, parameters) \end{array}$

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.

Be $\textbf{u}$ the vector containing the known commands sent to the system, if any. Then, we pass each sigma point through the process function $f(\chi, \Delta t)$ , the command function $b( \textbf{u}, \Delta t )$ and the command function depending on the state $bx( \textbf{u}, \chi, \Delta t )$ to project them forward in time, forming the new prior:

${Y} = f( \chi , \Delta t ) + b( \textbf{u}, \Delta t ) + bx( \textbf{u}, \chi, \Delta t )$

Then, we apply the Unscented Transform to compute the mean $\boldsymbol{\mu}$ and covariance $\overline{\textbf{P}}$ of the prior:

$\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} \end{array}$

where $\textbf{Q}$ is the covariance of the error introduced by the process function.

The second step is the update step. It is performed in the measurement space, so we must convert the sigma points of the prior into measurements using the measurement function $h: {R}^n \rightarrow {R}^m$ :

Then, we use once again the Unscented Transform to compute the mean $\boldsymbol{\mu}_z \in {R}^m$ and the covariance $\textbf{P}_z \in {R}^{m\text{ x }m}$ of these points:

$\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} \end{array}$

where $\textbf{R}$ is the measurement covariance matrix.

Then, we compute the residual $\textbf{y}$ of the measurement $\textbf{z}$ :

$\textbf{y} = \textbf{z} - \boldsymbol{\mu}_z$

To compute the Kalman's gain, we first need to compute the cross covariance of the state and the measurements:

$\textbf{P}_{xy} = \sum_{i=0}^{2n} w_i^c ({Y}_i - \boldsymbol{\mu})({Z}_i - \boldsymbol{\mu}_z)^T$

The Kalman's gain is then defined as:

$\textbf{K} = \textbf{P}_{xz} \textbf{P}_z^{-1}$

Finally, we can compute the new state estimate $\textbf{x}$ and the new covariance $\textbf{P}$ :

$\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}$

Examples: tutorial-pf.cpp, tutorial-ukf.cpp, ukf-linear-example.cpp, ukf-nonlinear-complex-example.cpp, and ukf-nonlinear-example.cpp.

Definition at line 134 of file vpUnscentedKalman.h.

Member Typedef Documentation

◆ vpAddSubFunction

typedef std::function<vpColVector(const vpColVector &, const vpColVector &)> vpUnscentedKalman::vpAddSubFunction

Function that computes either the equivalent of an addition or the equivalent of a subtraction in the state space or in the measurement space. The first argument is the vector to which we must add/subtract something and the second argument is the thing to be added/subtracted. The return is the result of this operation.

Definition at line 183 of file vpUnscentedKalman.h.

◆ vpCommandOnlyFunction

typedef std::function<vpColVector(const vpColVector &, const double &)> vpUnscentedKalman::vpCommandOnlyFunction

Command model function, which projects effect of the command on the state. The first argument is the command(s), the second is the period and the return is the effect of the command on the state after period seconds.

Definition at line 143 of file vpUnscentedKalman.h.

◆ vpCommandStateFunction

typedef std::function<vpColVector(const vpColVector &, const vpColVector &, const double &)> vpUnscentedKalman::vpCommandStateFunction

Command model function, which projects effect of the command on the state, when the effect of the command depends on the current state. The first argument is the command(s), the second is the state and the third is the period. The return is the effect of the command on the state after period seconds.

Definition at line 151 of file vpUnscentedKalman.h.

◆ vpMeanFunction

typedef std::function<vpColVector(const std::vector<vpColVector> &, const std::vector<double> &)> vpUnscentedKalman::vpMeanFunction

Mean function, which computes the weighted mean of either the prior or the prior expressed in the measurement space. The first argument is either the prior or the prior expressed in the measurement space and the second argument is the associated vector of weights. The return is the corresponding mean.

Definition at line 160 of file vpUnscentedKalman.h.

◆ vpMeasurementFunction

typedef std::function<vpColVector(const vpColVector &)> vpUnscentedKalman::vpMeasurementFunction

Measurement function, which converts the prior points in the measurement space. The argument is a point of a prior point and the return is its projection in the measurement space.

Definition at line 167 of file vpUnscentedKalman.h.

◆ vpProcessFunction

typedef std::function<vpColVector(const vpColVector &, const double &)> vpUnscentedKalman::vpProcessFunction

Process model function, which projects the sigma points forward in time. The first argument is a sigma point, the second is the period and the return is the sigma point projected in the future (i.e. a point of the prior).

Definition at line 174 of file vpUnscentedKalman.h.

Constructor & Destructor Documentation

◆ vpUnscentedKalman()

BEGIN_VISP_NAMESPACE vpUnscentedKalman::vpUnscentedKalman	(	const vpMatrix &	Q,
		const vpMatrix &	R,
		std::shared_ptr< vpUKSigmaDrawerAbstract > &	drawer,
		const vpProcessFunction &	f,
		const vpMeasurementFunction &	h
	)

Construct a new vpUnscentedKalman object.

Parameters

[in]	Q	The covariance introduced by performing the prediction step.
[in]	R	The covariance introduced by performing the update step.
[in]	drawer	Object that permits to draw the sigma points.
[in]	f	Process model function, which projects the sigma points forward in time. The first argument is a sigma point, the second is the period and the return is the sigma point projected in the future (i.e. a point of the prior).
[in]	h	Measurement function, which converts the prior points in the measurement space. The argument is a point of a prior point and the return is its projection in the measurement space.

Definition at line 43 of file vpUnscentedKalman.cpp.

Member Function Documentation

◆ filter()

void vpUnscentedKalman::filter	(	const vpColVector &	z,
		const double &	dt,
		const vpColVector &	u = `vpColVector()`
	)

Perform first the prediction step and then the filtering step.

Parameters

[in]	z	The new measurement.
[in]	dt	The time in the future we must predict.
[in]	u	The command(s) given to the system, if the impact of the system is known.

Warning: To use the commands, the method vpUnscentedKalman::setCommandOnlyFunction or vpUnscentedKalman::setCommandStateFunction must be called beforehand.

Definition at line 73 of file vpUnscentedKalman.cpp.

References predict(), and update().

◆ getPest()

vpMatrix vpUnscentedKalman::getPest ( ) const

inline

Get the estimated (i.e. filtered) covariance of the state.

Returns: vpMatrix The filtered covariance matrix.

Definition at line 338 of file vpUnscentedKalman.h.

◆ getPpred()

vpMatrix vpUnscentedKalman::getPpred ( ) const

inline

Get the predicted covariance of the state, i.e. the covariance of the prior.

Returns: vpMatrix The predicted covariance matrix.

Definition at line 348 of file vpUnscentedKalman.h.

◆ getXest()

vpColVector vpUnscentedKalman::getXest ( ) const

inline

Get the estimated (i.e. filtered) state.

Returns: vpColVector The estimated state.

Definition at line 358 of file vpUnscentedKalman.h.

◆ getXpred()

vpColVector vpUnscentedKalman::getXpred ( ) const

inline

Get the predicted state (i.e. the prior).

Returns: vpColVector The predicted state.

Definition at line 368 of file vpUnscentedKalman.h.

◆ init()

void vpUnscentedKalman::init	(	const vpColVector &	mu0,
		const vpMatrix &	P0
	)

Set the guess of the initial state and covariance.

Parameters

[in]	mu0	Guess of the initial state.
[in]	P0	Guess of the initial covariance.

Definition at line 59 of file vpUnscentedKalman.cpp.

References vpException::dimensionError, vpArray2D< Type >::getCols(), and vpArray2D< Type >::getRows().

◆ predict()

void vpUnscentedKalman::predict	(	const double &	dt,
		const vpColVector &	u = `vpColVector()`
	)

Predict the new state based on the last state and how far in time we want to predict.

Parameters

[in]	dt	The time in the future we must predict.
[in]	u	The command(s) given to the system, if the impact of the system is known.

Warning: To use the commands, the method vpUnscentedKalman::setCommandOnlyFunction or vpUnscentedKalman::setCommandStateFunction must be called beforehand.

Definition at line 79 of file vpUnscentedKalman.cpp.

References vpUKSigmaDrawerAbstract::vpSigmaPointsWeights::m_wc, and vpUKSigmaDrawerAbstract::vpSigmaPointsWeights::m_wm.

Referenced by filter().

◆ setCommandOnlyFunction()

void vpUnscentedKalman::setCommandOnlyFunction ( const vpCommandOnlyFunction & b )

inline

Set the command function to use when computing the prior.

Parameters

b	The command function to use.

Definition at line 213 of file vpUnscentedKalman.h.

◆ setCommandStateFunction()

void vpUnscentedKalman::setCommandStateFunction ( const vpCommandStateFunction & bx )

inline

Set the command function to use when computing the prior.

Parameters

bx	The command function to use.

Definition at line 223 of file vpUnscentedKalman.h.

◆ setMeasurementCovariance()

void vpUnscentedKalman::setMeasurementCovariance ( const vpMatrix & R )

inline

Permit to change the covariance introduced at each update step.

Parameters

[in] R The measurement covariance matrix.

Definition at line 298 of file vpUnscentedKalman.h.

◆ setMeasurementMeanFunction()

void vpUnscentedKalman::setMeasurementMeanFunction ( const vpMeanFunction & meanFunc )

inline

Set the measurement mean function to use when computing a mean in the measurement space.

Parameters

meanFunc The mean function to use.

Definition at line 234 of file vpUnscentedKalman.h.

◆ setMeasurementResidualFunction()

void vpUnscentedKalman::setMeasurementResidualFunction ( const vpAddSubFunction & measResFunc )

inline

Set the measurement residual function to use when computing a subtraction in the measurement space.

Parameters

measResFunc The residual function to use.

Definition at line 245 of file vpUnscentedKalman.h.

◆ setProcessCovariance()

void vpUnscentedKalman::setProcessCovariance ( const vpMatrix & Q )

inline

Permit to change the covariance introduced at each prediction step.

Parameters

[in] Q The process covariance matrix.

Definition at line 288 of file vpUnscentedKalman.h.

◆ setStateAddFunction()

void vpUnscentedKalman::setStateAddFunction ( const vpAddSubFunction & stateAddFunc )

inline

Set the state addition function to use when computing a addition in the state space.

Parameters

stateAddFunc The addition function to use.

Definition at line 256 of file vpUnscentedKalman.h.

◆ setStateMeanFunction()

void vpUnscentedKalman::setStateMeanFunction ( const vpMeanFunction & meanFunc )

inline

Set the state mean function to use when computing a mean in the state space.

Parameters

meanFunc The mean function to use.

Definition at line 267 of file vpUnscentedKalman.h.

◆ setStateResidualFunction()

void vpUnscentedKalman::setStateResidualFunction ( const vpAddSubFunction & stateResFunc )

inline

Set the state residual function to use when computing a subtraction in the state space.

Parameters

stateResFunc The residual function to use.

Definition at line 278 of file vpUnscentedKalman.h.

◆ simpleAdd()

static vpColVector vpUnscentedKalman::simpleAdd	(	const vpColVector &	a,
		const vpColVector &	toAdd
	)

inlinestatic

Simple function to compute an addition, which just does $\textbf{res} = \textbf{a} + \textbf{toAdd}$ .

Parameters

[in]	a	Vector to which we must add something.
[in]	toAdd	The something we must add to a .

Returns: vpColVector $\textbf{res} = \textbf{a} + \textbf{toAdd}$

Definition at line 380 of file vpUnscentedKalman.h.

◆ simpleMean()

static vpColVector vpUnscentedKalman::simpleMean	(	const std::vector< vpColVector > &	vals,
		const std::vector< double > &	wm
	)

inlinestatic

Simple function to compute a mean, which just does $\boldsymbol{\mu} = \sum_{i} wm_i \textbf{vals}_i$ .

Parameters

[in]	vals	Vector containing all the vectors we must compute the mean.
[in]	wm	The correspond list of weights.

Returns: vpColVector $\boldsymbol{\mu} = \sum_{i} wm_i \textbf{vals}_i$

Definition at line 406 of file vpUnscentedKalman.h.

References vpException::dimensionError, and vpArray2D< Type >::size().

◆ simpleResidual()

static vpColVector vpUnscentedKalman::simpleResidual	(	const vpColVector &	a,
		const vpColVector &	toSubtract
	)

inlinestatic

Simple function to compute a residual, which just does $\textbf{res} = \textbf{a} - \textbf{toSubtract}$ .

Parameters

[in]	a	Vector to which we must subtract something.
[in]	toSubtract	The something we must subtract to a .

Returns: vpColVector $\textbf{res} = \textbf{a} - \textbf{toSubtract}$

Definition at line 393 of file vpUnscentedKalman.h.

◆ update()

void vpUnscentedKalman::update ( const vpColVector & z )

Update the estimate of the state based on a new measurement.

Parameters

[in] z The measurements at the current timestep.

Definition at line 126 of file vpUnscentedKalman.cpp.

References vpMatrix::inverseByCholesky(), and vpMatrix::transpose().

Referenced by filter().

Public Types

Public Member Functions

Static Public Member Functions

Detailed Description

Member Typedef Documentation

◆ vpAddSubFunction

◆ vpCommandOnlyFunction

◆ vpCommandStateFunction

◆ vpMeanFunction

◆ vpMeasurementFunction

◆ vpProcessFunction

Constructor & Destructor Documentation

◆ vpUnscentedKalman()

Member Function Documentation

◆ filter()

◆ getPest()

◆ getPpred()

◆ getXest()

◆ getXpred()

◆ init()

◆ predict()

◆ setCommandOnlyFunction()

◆ setCommandStateFunction()

◆ setMeasurementCovariance()

◆ setMeasurementMeanFunction()

◆ setMeasurementResidualFunction()

◆ setProcessCovariance()

◆ setStateAddFunction()

◆ setStateMeanFunction()

◆ setStateResidualFunction()

◆ simpleAdd()

◆ simpleMean()

◆ simpleResidual()

◆ update()