LinearKalmanFilterInstantiation¶

class LinearKalmanFilterInstantiation(self)¶

Bases: KalmanFilter

This class provides an implementation of some specific linear Kalman filters.

Default linear Kalman filter.

By default the state model is unknown and set to vpLinearKalmanFilterInstantiation::unknown .

Methods

__init__	Default linear Kalman filter.
filter	Do the filtering and prediction of the measure signal.
getStateModel	Return the current state model.
initFilter	Initialize the Kalman filter material depending on the selected state model set with setStateModel() .
initStateConstAccWithColoredNoise_MeasureVel	Modelization of a constant acceleration state model with colored noise.
initStateConstVelWithColoredNoise_MeasureVel	Modelization of a constant speed state model with colored noise.
initStateConstVel_MeasurePos	Modelization of a constant speed state model with white noise.
setStateModel	Set the Kalman state model.

Inherited Methods

H
Xpre
getIteration	Return the iteration number.
Ppre
Xest
getMeasureSize	Return the size of the measure vector \({\bf z}_{(k)}\) for one signal.
R
verbose	Sets the verbose mode.
getNumberOfSignal	Return the number of signal to filter.
init	Initialize the Kalman filter.
Pest
dt
filtering	Update the Kalman filter by applying the filtering equations and increment the filter iteration ( vpKalmanFilter::iter ).
setNumberOfSignal	Set the number of signal to filter.
prediction	Update the Kalman filter by applying the prediction equations.
Q
F
getStateSize	Return the size of the state vector \({\bf x}_{(k)}\) for one signal.

Operators

__doc__
__init__	Default linear Kalman filter.
__module__

Attributes

F
H
Pest
Ppre
Q
R
Xest
Xpre
__annotations__
dt
stateConstAccWithColoredNoise_MeasureVel
stateConstVelWithColoredNoise_MeasureVel
stateConstVel_MeasurePos
unknown

class StateModel(self, value: int)¶

Bases: pybind11_object

Selector used to set the Kalman filter state model.

Values:

• stateConstVel_MeasurePos: Consider the state as a constant velocity model with white noise. Measures available are the successive positions of the target. To know more about this state model, see initStateConstVel_MeasurePos() .

• stateConstVelWithColoredNoise_MeasureVel: Consider the state as a constant velocity model with colored noise measurements as acceleration terms. Measured available are the velocities of the target. To know more about this state model, see initStateConstVelWithColoredNoise_MeasureVel() .

• stateConstAccWithColoredNoise_MeasureVel: Consider the state as a constant acceleration model with colored noise measurements as acceleration terms. Measured available are the velocities of the target. To know more about this state model, see initStateConstAccWithColoredNoise_MeasureVel() .

• unknown: Used to indicate that the state model is not initialized.

__and__(self, other: object) → object¶

__eq__(self, other: object) → bool¶

__ge__(self, other: object) → bool¶

__getstate__(self) → int¶

__gt__(self, other: object) → bool¶

__hash__(self) → int¶

__index__(self) → int¶

__init__(self, value: int)¶

__int__(self) → int¶

__invert__(self) → object¶

__le__(self, other: object) → bool¶

__lt__(self, other: object) → bool¶

__ne__(self, other: object) → bool¶

__or__(self, other: object) → object¶

__rand__(self, other: object) → object¶

__ror__(self, other: object) → object¶

__rxor__(self, other: object) → object¶

__setstate__(self, state: int) → None¶

__xor__(self, other: object) → object¶

property name : str¶

__init__(self)¶

Default linear Kalman filter.

By default the state model is unknown and set to vpLinearKalmanFilterInstantiation::unknown .

filter(self, z: visp._visp.core.ColVector) → None¶

Do the filtering and prediction of the measure signal.

Parameters:

z: visp._visp.core.ColVector¶

Measures \({\bf z}_k\) used to initialise the filter. The dimension of this vector is equal to the number of signal to filter (given by getNumberOfSignal() ) multiplied by the size of the measure vector (given by getMeasureSize() ) .

filtering(self, z: visp._visp.core.ColVector) → None¶

Update the Kalman filter by applying the filtering equations and increment the filter iteration ( vpKalmanFilter::iter ).

The filtering equation is given by:

\[{\bf x}_{k \mid k} = {\bf x}_{k \mid k-1} + {\bf W}_k \left[ {\bf z}_k - {\bf H} {\bf x}_{k \mid k-1} \right] \]

where \({\bf W_k}\) is the filter gain computed using the formula:

\[{\bf W_k} = {\bf P}_{k \mid k-1} {\bf H}^T \left[ {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf R}_k \right]^{-1} \]

and where the updated covariance of the state is given by

\[{\bf P}_{k \mid k} = \left({\bf I} - {\bf W}_k {\bf H} \right) {\bf P}_{k \mid k-1} \]

or in a symmetric form

\[{\bf P}_{k \mid k} = {\bf P}_{k \mid k-1} - {\bf W}_k {\bf S}_k {\bf W}^T_k \]

with

\[{\bf S}_k = {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf R}_k \]

Parameters:

z: visp._visp.core.ColVector¶

Measure (or observation) \({\bf z}_k\) provided at iteration \(k\) .

getIteration(self) → int¶

Return the iteration number.

getMeasureSize(self) → int¶

Return the size of the measure vector \({\bf z}_{(k)}\) for one signal.

getNumberOfSignal(self) → int¶

Return the number of signal to filter.

getStateModel(self) → visp._visp.core.LinearKalmanFilterInstantiation.StateModel¶

Return the current state model.

getStateSize(self) → int¶

Return the size of the state vector \({\bf x}_{(k)}\) for one signal.

init(self, size_state: int, size_measure: int, n_signal: int) → None¶

Initialize the Kalman filter.

Parameters:

n_signal: int¶

Number of signal to filter.

initFilter(self, nsignal: int, sigma_state: visp._visp.core.ColVector, sigma_measure: visp._visp.core.ColVector, rho: float, dt: float) → None¶

Initialize the Kalman filter material depending on the selected state model set with setStateModel() . This function is provided as a wrapper over all the other initializer functions like initStateConstVel_MeasurePos() , initStateConstVelWithColoredNoise_MeasureVel() , initStateConstAccWithColoredNoise_MeasureVel() .

Warning

It is requiered to set the state model before using this method.

The example below shows how to initialize the filter for a one dimensional signal.

#include <visp3/core/vpLinearKalmanFilterInstantiation.h>

int main()
{
  vpLinearKalmanFilterInstantiation kalman;
  // Select a constant velocity state model with colored noise
  // Measures are velocities
  kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel);

  // Initialise the filter for a one dimension signal
  int signal = 1;
  vpColVector sigma_state(2);   // State vector size is 2
  vpColVector sigma_measure(1); // Measure vector size is 1
  double rho = 0.9;
  double dummy = 0; // non used parameter for the selected state model

  kalman.initFilter(signal, sigma_state, sigma_measure, rho, dummy);
}

The example below shows a more complete example to filter a two dimensional target trajectory with an estimation of the target velocities from velocity measures.

#include <visp3/core/vpLinearKalmanFilterInstantiation.h>

int main()
{
  vpLinearKalmanFilterInstantiation kalman;

  // Set the constant velocity state model used for the filtering
  vpLinearKalmanFilterInstantiation::vpStateModel model;
  model = vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel;
  kalman.setStateModel(model);

  // We are now able to retrieve the size of the state vector and measure vector
  int state_size = kalman.getStateSize();

  // Filter the x and y velocities of a target (2 signals are to consider)
  int nsignal = 2;

  // Initialize the filter parameters:
  // - Firstly, the state variance
  int size = state_size*nsignal;
  vpColVector sigma_state(size);
  sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x)
  sigma_state[3] = 0.002; // Variance on the acceleration for the 2nd signal (y)
  // - Secondly, the measures variance
  vpColVector sigma_measure(nsignal); // 2 velocity measures available
  sigma_measure[0] = 0.03; // Variance on the x velocity measure
  sigma_measure[1] = 0.06; // Variance on the y velocity measure
  // - Thirdly, the correlation between successive accelerations
  double rho = 0.9;

  double dummy = 0; // non used parameter for the selected state model

  // Initialize the filter
  // The state model was set before
  kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);

  // Does the filtering
  vpColVector vm(2); // Measured velocities
  for ( ; ; ) {
    // Get the two dimensional velocity measures
    // vm[0] = ...;
    // vm[1] = ...;

    // Compute the filtering and the prediction
    kalman.filter(vm);
    // Print the estimation of the velocities (1st value of the state vector)
    std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl;
    std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]"
              << std::endl;
    // The one step prediction is available in kalman.Xpre variable
  }
}

Parameters:

sigma_state: visp._visp.core.ColVector¶

Vector that contains the variance of the state noise. The dimension of this vector is equal to the state vector \({\bf x}\) size multiplied by the number of signal to filter. Values are used to initialize the \({\bf Q}\) state covariance matrix.

sigma_measure: visp._visp.core.ColVector¶

Vector that contains the variance of the measurement noise. The dimension of this vector is equal to the measure vector \({\bf x}\) size multiplied by the number of signal to filter. Values are used to initialize the \({\bf R}\) measure covariance matrix.

rho: float¶

Degree of correlation between successive accelerations. Values are in [0:1[.

initStateConstAccWithColoredNoise_MeasureVel(self, nsignal: int, sigma_state: visp._visp.core.ColVector, sigma_measure: visp._visp.core.ColVector, rho: float, dt: float) → None¶

Modelization of a constant acceleration state model with colored noise. The measure is assumed to be the velocity of the target.

This state model assume that there is some memory associated with noise measurements as acceleration terms. They can be represented as remaining correlated (or colored) over successive time intervals, leading to the following state model:

\[\begin{split}\left\{ \begin{array}{rll} x_{(k+1)} & = x_{(k)} + \Delta t \; \dot{x}_{(k)} + \nu_{(k)} &\\\nu_{(k+1)}& = \rho \nu_{(k)} &+w_{1(k)} \\\dot{x}_{(k+1)} & = \dot{x}_{(k)} &+w_{2(k)}\\\end{array} \right. \end{split}\]

The terms \(w_{1(k)}\) and \(w_{2(k)}\) account for deviations from the assumed constant acceleration trajectory. They are assumed zero-mean, white, mutually uncorrelated, stationary random variable with variance \(\sigma^2_{Q_1}\) and \(\sigma^2_{Q_2}\) . The term \(\rho\) is the degree of correlation between successive accelerations. Values can range from 0 to 1.

We recall that the recursive state evolution equation is given by

\[\begin{split}{\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\\end{split}\]

From this state model, the transition matrix \({\bf F}\) and the state covariance matrix \({\bf Q}\) are given by:

\[\begin{split}{\bf F} = \left[ \begin{array}{ccc} 1 & 1 & \Delta t\\0 & \rho & 0 \\0 & 0 & 1 \end{array} \right] \end{split}\]

and

\[\begin{split}{\bf Q} = \left[ \begin{array}{ccc} 0 & 0 & 0\\0 & \sigma^2_{Q_1} & 0\\0 & 0& \sigma^2_{Q_2} \\\end{array} \right] \end{split}\]

The measurement model is given by:

\[z_{(k)} = {\bf H} {\bf x}_{(k)} + r_{(k)} \]

where \({\bf H} = [1 \; 0 \; 0]\) , \(z_{(k)}\) is the measure of the velocity and \(r_{(k)}\) is the measurement noise, assumed zero-mean, white mutually uncorrelated stationary random variables with variance \(\sigma^2_R\) , giving the covariance matrix:

\[{\bf R} = \left[\sigma^2_R\right] \]

The initial value of the state vector is set to:

\[\begin{split}{\bf x_{(0)}} = \left[ \begin{array}{c} z_{(0)}\\0 \\0 \end{array} \right] \end{split}\]

The initial value \(P_{(0|0)}\) of the prediction covariance matrix is given by:

\[\begin{split}{\bf P_{(0|0)}} = \left[ \begin{array}{ccc} \sigma^2_R & 0 & \sigma^2_R / \Delta t\\0 & \sigma^2_{Q_1}/(1-\rho^2) & -\rho \sigma^2_{Q_1} / (1-\rho^2)\Delta t \\\sigma^2_R / \Delta t & -\rho \sigma^2_{Q_1} / (1-\rho^2)\Delta t & (2\sigma^2_R +\sigma^2_{Q_1}/(1-\rho^2) )/\Delta t^2 \end{array} \right] \end{split}\]

The example below shows how to filter a two dimensional target trajectory with an estimation of the target velocity from velocity measures.

#include <visp3/core/vpLinearKalmanFilterInstantiation.h>

int main()
{
  vpLinearKalmanFilterInstantiation kalman;
  // Filter the x and y velocities of a target (2 signals are to consider)
  int nsignal = 2;

  // Initialize the filter parameters:
  // - Firstly, the state variance
  vpColVector sigma_state(6); // 6 = 3 for the state size x 2 signal
  sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x)
  sigma_state[2] = 0.001; // Variance on the acceleration for the 1st signal (x)
  sigma_state[4] = 0.002; // Variance on the acceleration for the 2nd signal (y)
  sigma_state[5] = 0.002; // Variance on the acceleration for the 2nd signal (y)
  // - Secondly, the measures variance
  vpColVector sigma_measure(nsignal); // 2 velocity measures available
  sigma_measure[0] = 0.03; // Variance on the x velocity measure
  sigma_measure[1] = 0.06; // Variance on the y velocity measure
  // - Thirdly, the correlation between successive accelerations
  double rho = 0.9;
  // - Lastly, the sampling time
  double dt = 0.020; // 20 ms

  // Initialize the filter
  kalman.initStateConstAccWithColoredNoise_MeasureVel(nsignal, sigma_state, sigma_measure, rho, dt);

  // Does the filtering
  vpColVector vm(2); // Measured velocities
  for ( ; ; ) {
    // Get the two dimensional velocity measures
    // vm[0] = ...;
    // vm[1] = ...;

    // Compute the filtering and the prediction
    kalman.filter(vm);
    // Print the estimation of the velocities (1st value of the state vector)
    std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl;
    std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]"
              << std::endl;
    // The one step prediction is available in kalman.Xpre variable
  }
}

Parameters:

sigma_state: visp._visp.core.ColVector¶

Vector that fix the variance of the state covariance matrix \([0 \; \sigma^2_{Q_1} \; \sigma^2_{Q_2}]^T\) . The dimension of this vector is 3 multiplied by the number of signal to filter.

sigma_measure: visp._visp.core.ColVector¶

Variance \(\sigma^2_R\) of the measurement noise. The dimension of this vector is equal to the number of signal to filter.

rho: float¶

Degree of correlation between successive accelerations. Values are in [0:1[.

initStateConstVelWithColoredNoise_MeasureVel(self, nsignal: int, sigma_state: visp._visp.core.ColVector, sigma_measure: visp._visp.core.ColVector, rho: float) → None¶

Modelization of a constant speed state model with colored noise. The measure is assumed to be the velocity of the target.

This state model assume that there is some memory associated with noise measurements as acceleration terms. They can be represented as remaining correlated (or colored) over successive time intervals, leading to the following state model:

\[\begin{split}\left\{ \begin{array}{rll} x_{(k+1)} & = x_{(k)} + \nu_{(k)} &\\\nu_{(k+1)}& = \rho \nu_{(k)} &+w_{(k)} \end{array} \right. \end{split}\]

The term \(w_{(k)}\) account for deviations from the assumed constant velocity trajectory. It is assumed zero-mean, white, mutually uncorrelated, stationary random variable with variance \(\sigma^2_Q\) . The term \(\rho\) is the degree of correlation between successive accelerations. Values can range from 0 to 1.

We recall that the recursive state evolution equation is given by

\[\begin{split}{\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\\end{split}\]

From this state model, the transition matrix \({\bf F}\) and the state covariance matrix \({\bf Q}\) are given by:

\[\begin{split}{\bf F} = \left[ \begin{array}{cc} 1 & 1\\0 & \rho \end{array} \right] \end{split}\]

and

\[\begin{split}{\bf Q} = \left[ \begin{array}{cc} 0 & 0\\0 & \sigma^2_Q \end{array} \right] \end{split}\]

The measurement model is given by:

\[z_{(k)} = {\bf H} {\bf x}_{(k)} + r_{(k)} \]

where \({\bf H} = [1 \; 0 \; 0]\) , \(z_{(k)}\) is the measure of the velocity and \(r_{(k)}\) is the measurement noise, assumed zero-mean, white mutually uncorrelated stationary random variables with variance \(\sigma^2_R\) , giving the covariance matrix:

\[{\bf R} = \left[\sigma^2_R\right] \]

The initial value of the state vector is set to:

\[\begin{split}{\bf x_{(0)}} = \left[ \begin{array}{c} z_{(0)}\\0 \end{array} \right] \end{split}\]

The initial value \(P_{(0|0)}\) of the prediction covariance matrix is given by:

\[\begin{split}{\bf P_{(0|0)}} = \left[ \begin{array}{cc} \sigma^2_R & 0\\0 & \sigma^2_Q/(1-\rho^2) \end{array} \right] \end{split}\]

The example below shows how to filter a two dimensional target trajectory with an estimation of the target velocity from velocity measures.

#include <visp3/core/vpLinearKalmanFilterInstantiation.h>

int main()
{
  vpLinearKalmanFilterInstantiation kalman;
  // Filter the x and y velocities of a target (2 signals are to consider)
  int nsignal = 2;

  // Initialize the filter parameters:
  // - Firstly, the state variance
  vpColVector sigma_state(4); // 4 = 2 for the state size x 2 signal
  sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x)
  sigma_state[3] = 0.002; // Variance on the acceleration for the 2nd signal (y)
  // - Secondly, the measures variance
  vpColVector sigma_measure(nsignal); // 2 velocity measures available
  sigma_measure[0] = 0.03; // Variance on the x velocity measure
  sigma_measure[1] = 0.06; // Variance on the y velocity measure
  // - Thirdly, the correlation between successive accelerations
  double rho = 0.9;

  // Initialize the filter
  kalman.initStateConstVelWithColoredNoise_MeasureVel(nsignal, sigma_state, sigma_measure, rho);

  // Does the filtering
  vpColVector vm(2); // Measured velocities
  for ( ; ; ) {
    // Get the two dimensional velocity measures
    // vm[0] = ...;
    // vm[1] = ...;

    // Compute the filtering and the prediction
    kalman.filter(vm);
    // Print the estimation of the velocities (1st value of the state vector)
    std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl;
    std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]"
              << std::endl;
    // The one step prediction is available in kalman.Xpre variable
  }
}

Parameters:

sigma_state: visp._visp.core.ColVector¶

Vector that fix the variance of the state covariance matrix \([0 \; \sigma^2_Q]^T\) . The dimension of this vector is 2 multiplied by the number of signal to filter.

sigma_measure: visp._visp.core.ColVector¶

Variance \(\sigma^2_R\) of the measurement noise. The dimension of this vector is equal to the number of signal to filter.

rho: float¶

Degree of correlation between successive accelerations. Values are in [0:1[.

initStateConstVel_MeasurePos(self, nsignal: int, sigma_state: visp._visp.core.ColVector, sigma_measure: visp._visp.core.ColVector, dt: float) → None¶

Modelization of a constant speed state model with white noise. The measure is assumed to be the position of the target.

The considered state model is the following

\[\begin{split}\left\{ \begin{array}{rlrl} x_{(k+1)} & = x_{(k)} & + \Delta t \; {\dot{x}}_{(k)} & + {w_1}_{(k)} \\{\dot{x}}_{(k+1)} & = & {\dot{x}}_{(k)} &+{w_2}_{(k)} \end{array} \right. \end{split}\]

The terms \({w_1}_{(k)}\) and \({w_2}_{(k)}\) account for deviations from the assumed constant velocity trajectory. They are assumed zero-mean, white, mutually uncorrelated, stationary random variable with variance \(\sigma^2_{Q_1}\) and \(\sigma^2_{Q_2}\) .

We recall that the recursive state evolution equation is given by

\[\begin{split}{\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\\end{split}\]

From this state model, the transition matrix \({\bf F}\) and the state covariance matrix \({\bf Q}\) are given by:

\[\begin{split}{\bf F} = \left[ \begin{array}{cc} 1 & \Delta t\\0 & 1 \end{array} \right] \end{split}\]

and

\[\begin{split}{\bf Q} = \sigma^2_Q \left[ \begin{array}{cc} \frac{1}{3}\Delta t^3 & \frac{1}{2}\Delta t^2\\\frac{1}{2}\Delta t^2 & \Delta t \end{array} \right] \end{split}\]

The initial value of the state vector at iteration 0 is set to:

\[\begin{split}{\bf x_{(0)}} = \left[ \begin{array}{c} z_{(0)}\\0 \end{array} \right] \end{split}\]

The value at iteration 1 is set to:

\[\begin{split}{\bf x_{(1)}} = \left[ \begin{array}{c} z_{(1)}\\(z_{(1)} - z_{(0)})/ \Delta t \end{array} \right] \end{split}\]

The initial value \(P_{(0|0)}\) of the prediction covariance matrix is given by:

\[\begin{split}{\bf P_{(0|0)}} = \left[ \begin{array}{cc} \sigma^2_R & \frac{\sigma^2_R}{2 \Delta t}\\\frac{\sigma^2_R}{2 \Delta t} & \frac{2}{3}\sigma^2_Q \Delta t + \frac{\sigma^2_R}{2 \Delta t^2} \end{array} \right] \end{split}\]

Parameters:

sigma_state: visp._visp.core.ColVector¶

Vector that fix the variance of the state covariance matrix \([\sigma^2_Q \; 0]^T\) . The dimension of this vector is 2 multiplied by the number of signal to filter.

sigma_measure: visp._visp.core.ColVector¶

Variance \(\sigma^2_R\) of the measurement noise. The dimension of this vector is equal to the number of signal to filter.

prediction(self) → None¶

Update the Kalman filter by applying the prediction equations.

The predicted state is given by

\[{{\bf x}}_{k|k-1} = {\bf F}_{k-1} {\bf x}_{k-1\mid k-1} \]

and the state prediction covariance by

\[{\bf P}_{k \mid k-1} = {\bf F}_{k-1} {\bf P}_{k-1 \mid k-1} {\bf F}^T_{k-1} + {\bf Q}_k \]

setNumberOfSignal(self, n_signal: int) → None¶

Set the number of signal to filter.

setStateModel(self, model: visp._visp.core.LinearKalmanFilterInstantiation.StateModel) → None¶

Set the Kalman state model. Depending on the state model, we set the state vector size and the measure vector size.

The example below shows how to use this method and then to get the size of the state and measure vectors.

#include <visp3/core/vpLinearKalmanFilterInstantiation.h>

int main()
{
  vpLinearKalmanFilterInstantiation kalman;

  kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel);
  // Value is 2
  std::cout << "State vector size: " << kalman.getStateSize() << std::endl;
  // Value is 1
  std::cout << "Measure vector size: " << kalman.getMeasureSize() << std::endl;
}

verbose(self, on: bool) → None¶

Sets the verbose mode.

Parameters:

on: bool¶

If true, activates the verbose mode which consists in printing the Kalman filter internal values.