KalmanFilter¶

class KalmanFilter(*args, **kwargs)¶

Bases: pybind11_object

This class provides a generic Kalman filtering algorithm along with some specific state model (constant velocity, constant acceleration) which are implemented in the vpLinearKalmanFilterInstantiation class.

The 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}\]

where \({\bf x}_{k}\) is the unknown state at iteration \(k\) .

The measurement equation is given by:

\[{\bf z}_k = {\bf H} {\bf x}_k + {\bf r}_k \]

where \({\bf z}_{k}\) is the measure (also named observation) at iteration \(k\) .

The predicted state is obtained by:

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

\[{\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 \]

where

\({\bf x}_{k|k-1}\) is the prediction of the state,
\({\bf P}_{k \mid k-1}\) is the state prediction covariance matrix.

Filtering equation are:

\[{\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} \]

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

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

where \({\bf W}_k\) is the filter gain.

Notice that there is a recursion for the inverse covariance

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

where \({\bf P}_{k \mid k}^{-1}\) is the inverse of the covariance matrix.

ViSP provides different state evolution models implemented in the vpLinearKalmanFilterInstantiation class.

Overloaded function.

__init__(self: visp._visp.core.KalmanFilter) -> None

Construct a default Kalman filter.

The verbose mode is disabled by default

__init__(self: visp._visp.core.KalmanFilter, n_signal: int) -> None

Construct a default Kalman filter by setting the number of signal to filter.

The verbose mode is disabled by default

Parameters:

n_signal: Number of signal to filter.

__init__(self: visp._visp.core.KalmanFilter, size_state: int, size_measure: int, n_signal: int) -> None

Construct a Kalman filter.

The verbose mode is disabled by default

Parameters:

n_signal: Number of signal to filter.

Methods

`__init__`	Overloaded function.
`filtering`	Update the Kalman filter by applying the filtering equations and increment the filter iteration ( vpKalmanFilter::iter ).
`getIteration`	Return the iteration number.
`getMeasureSize`	Return the size of the measure vector \({\bf z}_{(k)}\) for one signal.
`getNumberOfSignal`	Return the number of signal to filter.
`getStateSize`	Return the size of the state vector \({\bf x}_{(k)}\) for one signal.
`init`	Initialize the Kalman filter.
`prediction`	Update the Kalman filter by applying the prediction equations.
`setNumberOfSignal`	Set the number of signal to filter.
`verbose`	Sets the verbose mode.

Inherited Methods

Operators

`__doc__`
`__init__`	Overloaded function.
`__module__`

Attributes

`F`
`H`
`Pest`
`Ppre`
`Q`
`R`
`Xest`
`Xpre`
`__annotations__`
`dt`

__init__(*args, **kwargs)¶

Overloaded function.

__init__(self: visp._visp.core.KalmanFilter) -> None

Construct a default Kalman filter.

The verbose mode is disabled by default

__init__(self: visp._visp.core.KalmanFilter, n_signal: int) -> None

Construct a default Kalman filter by setting the number of signal to filter.

The verbose mode is disabled by default

Parameters:

n_signal: Number of signal to filter.

__init__(self: visp._visp.core.KalmanFilter, size_state: int, size_measure: int, n_signal: int) -> None

Construct a Kalman filter.

The verbose mode is disabled by default

Parameters:

n_signal: Number of signal to filter.

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.

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.

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.

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.