Class KalmanFilter

• Defined in File kalman_filter.hpp

Inheritance Relationships

Base Type

• public state_observer::StateObserver (Class StateObserver)

Class Documentation

class KalmanFilter : public state_observer::StateObserver

Implementation of a discrete-time Kalman Filter for linear systems.

The KalmanFilter class extends the StateObserver base class to provide a Kalman Filter, which estimates the state of a linear system by minimizing the covariance of the estimation error.

Public Functions

KalmanFilter(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &C, const Eigen::MatrixXd &D, const Eigen::VectorXd &initial_state, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R)

Constructor with full state-space matrices, initial state, and covariance matrices.

Parameters:

• A – State transition matrix (n x n).

• B – Input matrix (n x p).

• C – Output matrix (q x n).

• D – Feedthrough matrix (q x p).

• initial_state – Initial state vector (n x 1).

• Q – Process noise covariance matrix (n x n).

• R – Measurement noise covariance matrix (q x q).

Throws:

std::invalid_argument – if dimensions are inconsistent.

KalmanFilter(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &C, const Eigen::VectorXd &initial_state, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R)

Constructor without feedthrough matrix D.

Parameters:

• A – State transition matrix (n x n).

• B – Input matrix (n x p).

• C – Output matrix (q x n).

• initial_state – Initial state vector (n x 1).

• Q – Process noise covariance matrix (n x n).

• R – Measurement noise covariance matrix (q x q).

Throws:

std::invalid_argument – if dimensions are inconsistent.

KalmanFilter(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &C, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R)

Constructor without initial state and feedthrough matrix D.

Parameters:

• A – State transition matrix (n x n).

• B – Input matrix (n x p).

• C – Output matrix (q x n).

• Q – Process noise covariance matrix (n x n).

• R – Measurement noise covariance matrix (q x q).

Throws:

std::invalid_argument – if dimensions are inconsistent.

KalmanFilter(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &C, const Eigen::MatrixXd &D, const Eigen::VectorXd &initial_state, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R, const Eigen::MatrixXd &P0)

Constructor with initial error covariance matrix P0.

Parameters:

• A – State transition matrix (n x n).

• B – Input matrix (n x p).

• C – Output matrix (q x n).

• D – Feedthrough matrix (q x p).

• initial_state – Initial state vector (n x 1).

• Q – Process noise covariance matrix (n x n).

• R – Measurement noise covariance matrix (q x q).

• P0 – Initial error covariance matrix (n x n).

Throws:

std::invalid_argument – if dimensions are inconsistent.

KalmanFilter(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &C, const Eigen::VectorXd &initial_state, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R, const Eigen::MatrixXd &P0)

Constructor without feedthrough matrix D and with initial error covariance matrix P0.

Parameters:

• A – State transition matrix (n x n).

• B – Input matrix (n x p).

• C – Output matrix (q x n).

• initial_state – Initial state vector (n x 1).

• Q – Process noise covariance matrix (n x n).

• R – Measurement noise covariance matrix (q x q).

• P0 – Initial error covariance matrix (n x n).

Throws:

std::invalid_argument – if dimensions are inconsistent.

KalmanFilter(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &C, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R, const Eigen::MatrixXd &P0)

Constructor without initial state, feedthrough matrix D, and with initial error covariance matrix P0.

Parameters:

• A – State transition matrix (n x n).

• B – Input matrix (n x p).

• C – Output matrix (q x n).

• Q – Process noise covariance matrix (n x n).

• R – Measurement noise covariance matrix (q x q).

• P0 – Initial error covariance matrix (n x n).

Throws:

std::invalid_argument – if dimensions are inconsistent.

inline KalmanFilter()

Default constructor.

inline ~KalmanFilter()

Destructor.

virtual void set_parameters(const StateObserverParam::SharedPtr state_observer_params) override

Set the parameters of the Kalman Filter.

Parameters:

state_observer_params – Shared pointer to StateObserverParam.

virtual Eigen::MatrixXd update(const Eigen::VectorXd &measurement) override

Update the Kalman Filter with a measurement.

Parameters:

measurement – Measurement vector (q x 1).

Returns:

Updated state estimate.

virtual Eigen::MatrixXd update(const Eigen::VectorXd &measurement, const Eigen::VectorXd &input) override

Update the Kalman Filter with a measurement and input.

Parameters:

• measurement – Measurement vector (q x 1).

• input – Input vector (p x 1).

Returns:

Updated state estimate.

virtual Eigen::VectorXd open_loop_update() override

Perform an open-loop update of the Kalman Filter.

Returns:

Updated output vector y_.

void update_process_covariance(const Eigen::MatrixXd &new_Q)

Update the process noise covariance matrix Q.

Parameters:

new_Q – New process noise covariance matrix (n x n).

Throws:

std::invalid_argument – if dimensions are incorrect.

void update_measurement_covariance(const Eigen::MatrixXd &new_R)

Update the measurement noise covariance matrix R.

Parameters:

new_R – New measurement noise covariance matrix (q x q).

Throws:

std::invalid_argument – if dimensions are incorrect.

void update_qr(const Eigen::MatrixXd &new_Q, const Eigen::MatrixXd &new_R)

Update both the process and measurement noise covariance matrices.

Parameters:

• new_Q – New process noise covariance matrix (n x n).

• new_R – New measurement noise covariance matrix (q x q).

Throws:

std::invalid_argument – if dimensions are incorrect.

virtual Eigen::VectorXd get_state_variance() override

Get the variance of the state estimate.

Returns:

State variance vector.

inline Eigen::MatrixXd get_Q() const

Get the process noise covariance matrix Q.

Returns:

Matrix Q_.

inline Eigen::MatrixXd get_R() const

Get the measurement noise covariance matrix R.

Returns:

Matrix R_.

inline Eigen::MatrixXd get_P0() const

Get the initial error covariance matrix P0.

Returns:

Matrix P_.

Protected Attributes

Eigen::VectorXd L_

Eigen::MatrixXd P_

Eigen::MatrixXd K_

Eigen::MatrixXd Q_

Eigen::MatrixXd R_

Eigen::MatrixXd I_