iDynTree::DiscreteExtendedKalmanFilterHelper class
#include <iDynTree/ExtendedKalmanFilter.h>

class implementation of discrete EKF with additive Gaussian noise

The derived class must must set the size for states, inputs and outputs using ekfSetStateSize(), ekfSetInputSize() and ekfSetOutputSize() methods. Then the derived class must call ekfInit() to resize the vectors and matrices for the EKF.

Similarly before running the estimator through a loop, it is recommended to set the initial states and variances for measurements, system dynamics and intial states. This is necessary to avoid any NaN values to be propagated.

Once, initialized properly, the filter can be run by

The internal state of the estimator can be obtained by calling ekfGetStates().

The Discrete Extended Kalman Filter equations implemented in this class are coherent with the ones described in Discrete-time predict and update equations section of this article.

The general workflow implementing/inheriting this class would be in the order,

Derived classes

class AttitudeQuaternionEKF
Quaternion based Discrete Extended Kalman Filter fusing IMU measurements, to give estimates of orientation, angular velocity and gyroscope bias.

Constructors, destructors, conversion operators

DiscreteExtendedKalmanFilterHelper()

Public functions

auto ekf_f(const iDynTree::VectorDynSize& x_k, const iDynTree::VectorDynSize& u_k, iDynTree::VectorDynSize& xhat_k_plus_one) -> bool pure virtual
Describes the state propagation for a given dynamical system If state of the system is denoted by $ x $ and the control input by $ u $ , then the system dynamics is given as $ x_{k+1} = f(x_k, u_k) $ .
auto ekf_h(const iDynTree::VectorDynSize& xhat_k_plus_one, iDynTree::VectorDynSize& zhat_k_plus_one) -> bool pure virtual
Describes the measurement model of the system, i.e., how the measurements can be described as a function of states, Given a state of the system described by $ x $ , what would be the measurement $ z $ observed from this state $ z_{k+1} = h(\hat{x}_{k+1}) $ .
auto ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F) -> bool pure virtual
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
auto ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::VectorDynSize& u, iDynTree::MatrixDynSize& F) -> bool pure virtual
Describes the system Jacobian necessary for the propagation of predicted state covariance The analytical Jacobian describing the partial derivative of the system propagation with respect to the state and the system propagation with respect to the input.
auto ekfComputeJacobianH(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& H) -> bool pure virtual
Describes the measurement Jacobian necessary for computing Kalman gain and updating the predicted state and its covariance The analytical Jacobian describing the partial derivative of the measurement model with respect to the state.
auto ekfPredict() -> bool
Implements the Discrete EKF prediction equation described by $ \hat{x}_{k+1} = f(x_k, u_k) $ is given by the ekf_f() method $ \hat{P}_{k+1} = F_k P_k F_k^T + Q $ where, $ F \mid_{x = x_k} $ is given by the ekfComputejacobianF() method.
auto ekfUpdate() -> bool
Implements the Discrete EKF update equation described by $ z_{k+1} = h(\hat{x}_{k+1}) $ is given by ekf_h() method innovation $ \tilde{y}_{k+1} = y_{k+1} - z_{k+1} $ innovation covariance $ S_{k+1} = H_{k+1} \hat{P}_{k+1} H_{k+1}^T + R $ , where $ H \mid_{x = \hat{x}_{k+1}} $ is given by ekfComputejacobianH() method Kalman gain $ K_{k+1} = \hat{P}_{k+1} H_{k+1}^T S_{k+1}^{-1} $ Updated covariance $ P_{k+1} = \hat{P}_{k+1} - (K_{k+1} H \hat{P}_{k+1}) $ Updated state estimate $ x_{k+1} = \hat{x}_{k+1} + K_{k+1} \tilde{y}_{k+1} $ .
auto ekfInit() -> bool
Initializes and resizes the internal buffers of this filter.
auto ekfInit(const size_t& state_size, const size_t& input_size, const size_t& output_size) -> bool
Initializes and resizes the internal buffers of this filter.
void ekfReset()
Resets the filter flags The filter flags check if the filter was properly initialized, if the initial state was set, if the initial state covariance was set.
auto ekfReset(const size_t& state_size, const size_t& input_size, const size_t& output_size, const iDynTree::Span<double>& x0, const iDynTree::Span<double>& P0, const iDynTree::Span<double>& Q, const iDynTree::Span<double>& R) -> bool
Resets the filter flags, initializes and resizes internal buffers of the filter, and sets initial state, initial state covariance, and system noise and measurement noise covariance matrices.
auto ekfSetMeasurementVector(const iDynTree::Span<double>& y) -> bool
Set measurement vector at every time step the measurement vector size and output size should match.
auto ekfSetInputVector(const iDynTree::Span<double>& u) -> bool
Set input vector at every time step the input vector size and input size should match.
auto ekfSetInitialState(const iDynTree::Span<double>& x0) -> bool
Set initial state the size of x0 and state size should match.
auto ekfSetStateCovariance(const iDynTree::Span<double>& P) -> bool
Set initial state covariance matrix the size of P and (state size*state size) should match.
auto ekfSetSystemNoiseCovariance(const iDynTree::Span<double>& Q) -> bool
Set system noise covariance matrix the size of Q and (state size*state size) should match.
auto ekfSetMeasurementNoiseCovariance(const iDynTree::Span<double>& R) -> bool
Set measurement noise covariance matrix the size of R and (output size*output size) should match.
void ekfSetStateSize(size_t dim_X)
Set the state dimensions.
void ekfSetInputSize(size_t dim_U)
Set the input dimensions.
void ekfSetOutputSize(size_t dim_Y)
Set the ouptut dimensions.
auto ekfGetStates(const iDynTree::Span<double>& x) const -> bool
Get current internal state of the filter the size of x and state size should match.
auto ekfGetStateCovariance(const iDynTree::Span<double>& P) const -> bool
Get state covariance matrix the size of P and (state size*state size) should match.

Protected functions

template<typename T>
void ignore(T&&)
function template to ignore unused parameters

Function documentation

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekf_f(const iDynTree::VectorDynSize& x_k, const iDynTree::VectorDynSize& u_k, iDynTree::VectorDynSize& xhat_k_plus_one) pure virtual

Describes the state propagation for a given dynamical system If state of the system is denoted by $ x $ and the control input by $ u $ , then the system dynamics is given as $ x_{k+1} = f(x_k, u_k) $ .

Parameters
x_k in state at current time instant
u_k in control input at current time instant
xhat_k_plus_one out predicted state without any correction from measurements
Returns bool true/false if successful or not

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekf_h(const iDynTree::VectorDynSize& xhat_k_plus_one, iDynTree::VectorDynSize& zhat_k_plus_one) pure virtual

Describes the measurement model of the system, i.e., how the measurements can be described as a function of states, Given a state of the system described by $ x $ , what would be the measurement $ z $ observed from this state $ z_{k+1} = h(\hat{x}_{k+1}) $ .

Parameters
xhat_k_plus_one in predicted state of next time instant
zhat_k_plus_one out predicted measurement of next time instant
Returns bool true/false if successful or not

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::VectorDynSize& u, iDynTree::MatrixDynSize& F) pure virtual

Describes the system Jacobian necessary for the propagation of predicted state covariance The analytical Jacobian describing the partial derivative of the system propagation with respect to the state and the system propagation with respect to the input.

Parameters
x in system state
u in system input
F out system Jacobian
Returns bool true/false if successful or not

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfComputeJacobianH(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& H) pure virtual

Describes the measurement Jacobian necessary for computing Kalman gain and updating the predicted state and its covariance The analytical Jacobian describing the partial derivative of the measurement model with respect to the state.

Parameters
x in system state
H out measurement Jacobian
Returns bool true/false if successful or not

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfPredict()

Implements the Discrete EKF prediction equation described by $ \hat{x}_{k+1} = f(x_k, u_k) $ is given by the ekf_f() method $ \hat{P}_{k+1} = F_k P_k F_k^T + Q $ where, $ F \mid_{x = x_k} $ is given by the ekfComputejacobianF() method.

Returns bool true/false if successful or not

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfUpdate()

Implements the Discrete EKF update equation described by $ z_{k+1} = h(\hat{x}_{k+1}) $ is given by ekf_h() method innovation $ \tilde{y}_{k+1} = y_{k+1} - z_{k+1} $ innovation covariance $ S_{k+1} = H_{k+1} \hat{P}_{k+1} H_{k+1}^T + R $ , where $ H \mid_{x = \hat{x}_{k+1}} $ is given by ekfComputejacobianH() method Kalman gain $ K_{k+1} = \hat{P}_{k+1} H_{k+1}^T S_{k+1}^{-1} $ Updated covariance $ P_{k+1} = \hat{P}_{k+1} - (K_{k+1} H \hat{P}_{k+1}) $ Updated state estimate $ x_{k+1} = \hat{x}_{k+1} + K_{k+1} \tilde{y}_{k+1} $ .

Returns bool true/false if successful or not

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfInit()

Initializes and resizes the internal buffers of this filter.

Returns bool true/false if successful or not

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfInit(const size_t& state_size, const size_t& input_size, const size_t& output_size)

Initializes and resizes the internal buffers of this filter.

Parameters
state_size in state size
input_size in input size
output_size in output size
Returns bool true/false if successful or not

void iDynTree::DiscreteExtendedKalmanFilterHelper::ekfReset()

Resets the filter flags The filter flags check if the filter was properly initialized, if the initial state was set, if the initial state covariance was set.

These three flags are crucial for proper setting up of the filter. The other flags include the checks on whether the input and measurement vectors were updated at every prediction/update step

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfReset(const size_t& state_size, const size_t& input_size, const size_t& output_size, const iDynTree::Span<double>& x0, const iDynTree::Span<double>& P0, const iDynTree::Span<double>& Q, const iDynTree::Span<double>& R)

Resets the filter flags, initializes and resizes internal buffers of the filter, and sets initial state, initial state covariance, and system noise and measurement noise covariance matrices.

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfSetMeasurementVector(const iDynTree::Span<double>& y)

Set measurement vector at every time step the measurement vector size and output size should match.

Parameters
y in iDynTree::Span object to access the measurement vector
Returns bool true/false if successful or not

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfSetInputVector(const iDynTree::Span<double>& u)

Set input vector at every time step the input vector size and input size should match.

Parameters
u in iDynTree::Span object to access the input vector
Returns bool true/false if successful or not

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfSetInitialState(const iDynTree::Span<double>& x0)

Set initial state the size of x0 and state size should match.

Parameters
x0 in iDynTree::Span object to access the state vector
Returns bool true/false if successful or not

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfSetStateCovariance(const iDynTree::Span<double>& P)

Set initial state covariance matrix the size of P and (state size*state size) should match.

Parameters
P in iDynTree::Span object to access the state covariance matrix
Returns bool true/false if successful or not

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfSetSystemNoiseCovariance(const iDynTree::Span<double>& Q)

Set system noise covariance matrix the size of Q and (state size*state size) should match.

Parameters
Q in iDynTree::Span object to access the system noise covariance matrix
Returns bool true/false if successful or not

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfSetMeasurementNoiseCovariance(const iDynTree::Span<double>& R)

Set measurement noise covariance matrix the size of R and (output size*output size) should match.

Parameters
R in iDynTree::Span object to access the measurement noise covariance matrix
Returns bool true/false if successful or not

void iDynTree::DiscreteExtendedKalmanFilterHelper::ekfSetStateSize(size_t dim_X)

Set the state dimensions.

Parameters
dim_X in state size
Returns bool true/false if successful or not

void iDynTree::DiscreteExtendedKalmanFilterHelper::ekfSetInputSize(size_t dim_U)

Set the input dimensions.

Parameters
dim_U in input size
Returns bool true/false if successful or not

void iDynTree::DiscreteExtendedKalmanFilterHelper::ekfSetOutputSize(size_t dim_Y)

Set the ouptut dimensions.

Parameters
dim_Y in output size
Returns bool true/false if successful or not

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfGetStates(const iDynTree::Span<double>& x) const

Get current internal state of the filter the size of x and state size should match.

Parameters
x out iDynTree::Span object to copy the internal state vector into
Returns bool true/false if successful or not

bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfGetStateCovariance(const iDynTree::Span<double>& P) const

Get state covariance matrix the size of P and (state size*state size) should match.

Parameters
P out iDynTree::Span object to copy the internal state covariance matrix onto
Returns bool true/false if successful or not