iDynTree::DiscreteKalmanFilterHelper class
#include <iDynTree/KalmanFilter.h>

Discrete Kalman Filter with additive Gaussian noise.

The Kalman filter can be constructed by giving the system design matrices A, B, C and D. where A is the state transition matrix, B is the control input matrix, C is the output matrix and D the feed through matrix. These matrices can be used to describe a stochastic model for a linear dynamical system.

\[ x_{k+1} = A x_{k} + B u_{k} + w_k \]
\[ y_{k+1} = C x_{k+1} + D u_{k} + v_k \]

Once the filter is constructed, the system noise and measurement noise covariance matrices can be set. The filter can be run, after setting initial state and state covariance matrix. The filter init method is called to check if the filter is properly configured and is ready to use.

Once the filter is configured, the filter algorithm can be run in a loop by,

  • setting input vector
  • prediction step which propagates the state through the modeled state dynamics
  • setting the measurement vector
  • correct the predicted states with the incoming measurements

Constructors, destructors, conversion operators

DiscreteKalmanFilterHelper()

Public functions

auto constructKalmanFilter(const iDynTree::MatrixDynSize& A, const iDynTree::MatrixDynSize& B, const iDynTree::MatrixDynSize& C, const iDynTree::MatrixDynSize& D) -> bool
Describes the state propagation for a given dynamical system and the measurement model given the available measurements.
auto constructKalmanFilter(const iDynTree::MatrixDynSize& A, const iDynTree::MatrixDynSize& B, const iDynTree::MatrixDynSize& C) -> bool
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
auto constructKalmanFilter(const iDynTree::MatrixDynSize& A, const iDynTree::MatrixDynSize& C) -> bool
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
auto kfSetInitialState(const iDynTree::VectorDynSize& x0) -> bool
Set initial state of the Kalman filter.
auto kfSetStateCovariance(const iDynTree::MatrixDynSize& P) -> bool
Sets the state covariance matrix.
auto kfSetSystemNoiseCovariance(const iDynTree::MatrixDynSize& Q) -> bool
Sets the system noise covariance matrix.
auto kfSetMeasurementNoiseCovariance(const iDynTree::MatrixDynSize& R) -> bool
Sets the measurement noise covariance matrix.
auto kfInit() -> bool
This method checks if the filter is properly constructed and configured i.e.
auto kfSetInputVector(const iDynTree::VectorDynSize& u) -> bool
Set inputs for the Kalman filter.
auto kfPredict() -> bool
Runs one step of the Discrete Kalman Filter prediction equation described by $ \hat{x}_{k+1} = A x_{k} + B u_{k} $ $ \hat{P}_{k+1} = A_k P_k A_k^T + Q $ .
auto kfSetMeasurementVector(const iDynTree::VectorDynSize& y) -> bool
Set measurements for the Kalman filter.
auto kfUpdate() -> bool
Runs one step of the Discrete Kalman Filter update equation described by $ \tilde{y}_{k+1} = C \hat{x}_{k+1} + D u_{k} $ $ x_{k+1} = \hat{x}_{k+1} + K_{k+1}(\tilde{y}_{k+1} - z_{k+1}) $ $ P_{k+1} = (I - K_{k+1} C) \hat{P}_{k+1} $ where K is the Kalman gain.
auto kfGetStates(iDynTree::VectorDynSize& x) -> bool
Get system state.
auto kfGetStateCovariance(iDynTree::MatrixDynSize& P) -> bool
Get system state covariance.
auto kfReset() -> bool
Reset Kalman filter Resets the Kalman filter and initializes with the internally stored states and matrices initially set by the user.
auto kfReset(const iDynTree::VectorDynSize& x0, const iDynTree::MatrixDynSize& P0, const iDynTree::MatrixDynSize& Q, const iDynTree::MatrixDynSize& R) -> bool
Reset Kalman filter with the given arguments.

Function documentation

bool iDynTree::DiscreteKalmanFilterHelper::constructKalmanFilter(const iDynTree::MatrixDynSize& A, const iDynTree::MatrixDynSize& B, const iDynTree::MatrixDynSize& C, const iDynTree::MatrixDynSize& D)

Describes the state propagation for a given dynamical system and the measurement model given the available measurements.

Parameters
A in state transition matrix
B in control input matrix mapping inputs to states
C in output matrix mapping states to measurements
D in feed through matrix mapping inputs to measurements
Returns bool true/false if filter was constructed successfully or not

If state of the system is denoted by $ x $ , the control input by $ u $ and the measurements by $ y $ , then the system dynamics is given as $ x_{k+1} = A x_{k} + B u_{k} + w_k $ and the measurement model is given by $ y_{k+1} = C x_{k+1} + D u_{k} + v_k $

bool iDynTree::DiscreteKalmanFilterHelper::kfSetInitialState(const iDynTree::VectorDynSize& x0)

Set initial state of the Kalman filter.

Parameters
x0 in initial state of dimension $ dim_x \times 1 $
Returns bool true/false successful or not

bool iDynTree::DiscreteKalmanFilterHelper::kfSetStateCovariance(const iDynTree::MatrixDynSize& P)

Sets the state covariance matrix.

Parameters
P in state covariance matrix of dimensions $ dim_x \times dim_x $
Returns bool true/false successful or not

bool iDynTree::DiscreteKalmanFilterHelper::kfSetSystemNoiseCovariance(const iDynTree::MatrixDynSize& Q)

Sets the system noise covariance matrix.

Parameters
Q in system noise covariance matrix of dimensions $ dim_x \times dim_x $
Returns bool true/false successful or not

bool iDynTree::DiscreteKalmanFilterHelper::kfSetMeasurementNoiseCovariance(const iDynTree::MatrixDynSize& R)

Sets the measurement noise covariance matrix.

Parameters
R in measurement covariance matrix of dimensions $ dim_y \times dim_y $
Returns bool true/false successful or not

bool iDynTree::DiscreteKalmanFilterHelper::kfInit()

This method checks if the filter is properly constructed and configured i.e.

Returns bool true/false successful or not

if initial states and covariance matrices are set.

bool iDynTree::DiscreteKalmanFilterHelper::kfSetInputVector(const iDynTree::VectorDynSize& u)

Set inputs for the Kalman filter.

Parameters
u in input vector of dimension $ dim_u \times 1 $
Returns bool true/false successful or not

bool iDynTree::DiscreteKalmanFilterHelper::kfPredict()

Runs one step of the Discrete Kalman Filter prediction equation described by $ \hat{x}_{k+1} = A x_{k} + B u_{k} $ $ \hat{P}_{k+1} = A_k P_k A_k^T + Q $ .

Returns bool true/false if successful or not

bool iDynTree::DiscreteKalmanFilterHelper::kfSetMeasurementVector(const iDynTree::VectorDynSize& y)

Set measurements for the Kalman filter.

Parameters
y in input vector of dimension $ dim_y \times 1 $
Returns bool true/false successful or not

bool iDynTree::DiscreteKalmanFilterHelper::kfUpdate()

Runs one step of the Discrete Kalman Filter update equation described by $ \tilde{y}_{k+1} = C \hat{x}_{k+1} + D u_{k} $ $ x_{k+1} = \hat{x}_{k+1} + K_{k+1}(\tilde{y}_{k+1} - z_{k+1}) $ $ P_{k+1} = (I - K_{k+1} C) \hat{P}_{k+1} $ where K is the Kalman gain.

Returns bool true/false if successful or not

bool iDynTree::DiscreteKalmanFilterHelper::kfGetStates(iDynTree::VectorDynSize& x)

Get system state.

Parameters
x out system state of dimension $ dim_x \times 1 $
Returns bool true/false successful or not

bool iDynTree::DiscreteKalmanFilterHelper::kfGetStateCovariance(iDynTree::MatrixDynSize& P)

Get system state covariance.

Parameters
P out system state covariance matrix of dimension $ dim_x \times dim_x $
Returns bool true/false successful or not

bool iDynTree::DiscreteKalmanFilterHelper::kfReset(const iDynTree::VectorDynSize& x0, const iDynTree::MatrixDynSize& P0, const iDynTree::MatrixDynSize& Q, const iDynTree::MatrixDynSize& R)

Reset Kalman filter with the given arguments.

Parameters
x0 in initial state vector of dimensions $ dim_x \times 1 $
P0
Q in system noise covariance matrix of dimensions $ dim_x \times dim_x $
R in measurement noise covariance matrix of dimensions $ dim_y \times dim_y $