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.

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

Construct a default Kalman filter.

The verbose mode is disabled by default

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

3. __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.

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

Construct a default Kalman filter.

The verbose mode is disabled by default

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

3. __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.