Public Member Functions | Public Attributes | Private Attributes | List of all members
KalmanFilter Class Reference

#include <kalman.hpp>

Public Member Functions

void changeStates (const Eigen::VectorXd &new_states)
 
void init ()
 
void init (double t0, const Eigen::VectorXd &x0)
 
 KalmanFilter (double dt, const Eigen::MatrixXd &A, const Eigen::MatrixXd &C, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R, const Eigen::MatrixXd &P)
 
 KalmanFilter ()
 
Eigen::VectorXd state ()
 
double time ()
 
void update (const Eigen::VectorXd &y)
 
void update (const Eigen::VectorXd &y, double dt)
 
void update (const Eigen::VectorXd &y, double dt, const Eigen::MatrixXd A)
 

Public Attributes

Eigen::MatrixXd A
 
Eigen::MatrixXd C
 
Eigen::MatrixXd K
 
Eigen::MatrixXd P
 
Eigen::MatrixXd P0
 
Eigen::MatrixXd Q
 
Eigen::MatrixXd R
 

Private Attributes

double dt
 
Eigen::MatrixXd I
 
bool initialized
 
int m
 
int n
 
double t
 
double t0
 
Eigen::VectorXd x_hat
 
Eigen::VectorXd x_hat_new
 

Detailed Description

Kalman filter implementation using Eigen. Based on the following introductory paper:

http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
Author
: Hayk Martirosyan
Date
: 2014.11.15

Definition at line 15 of file kalman.hpp.

Constructor & Destructor Documentation

◆ KalmanFilter() [1/2]

KalmanFilter::KalmanFilter ( double  dt,
const Eigen::MatrixXd &  A,
const Eigen::MatrixXd &  C,
const Eigen::MatrixXd &  Q,
const Eigen::MatrixXd &  R,
const Eigen::MatrixXd &  P 
)

Implementation of KalmanFilter class.

Author
: Hayk Martirosyan
Date
: 2014.11.15

Definition at line 13 of file kalman.cpp.

◆ KalmanFilter() [2/2]

KalmanFilter::KalmanFilter ( )

Create a blank estimator.

Definition at line 27 of file kalman.cpp.

Member Function Documentation

◆ changeStates()

void KalmanFilter::changeStates ( const Eigen::VectorXd &  new_states)

Definition at line 62 of file kalman.cpp.

◆ init() [1/2]

void KalmanFilter::init ( )

Initialize the filter with initial states as zero.

Definition at line 38 of file kalman.cpp.

◆ init() [2/2]

void KalmanFilter::init ( double  t0,
const Eigen::VectorXd &  x0 
)

Initialize the filter with a guess for initial states.

Definition at line 29 of file kalman.cpp.

◆ state()

Eigen::VectorXd KalmanFilter::state ( )
inline

Return the current state and time.

Definition at line 73 of file kalman.hpp.

◆ time()

double KalmanFilter::time ( )
inline

Definition at line 74 of file kalman.hpp.

◆ update() [1/3]

void KalmanFilter::update ( const Eigen::VectorXd &  y)

Update the estimated state based on measured values. The time step is assumed to remain constant.

Definition at line 46 of file kalman.cpp.

◆ update() [2/3]

void KalmanFilter::update ( const Eigen::VectorXd &  y,
double  dt 
)

Definition at line 78 of file kalman.cpp.

◆ update() [3/3]

void KalmanFilter::update ( const Eigen::VectorXd &  y,
double  dt,
const Eigen::MatrixXd  A 
)

Update the estimated state based on measured values, using the given time step and dynamics matrix.

Definition at line 71 of file kalman.cpp.

Member Data Documentation

◆ A

Eigen::MatrixXd KalmanFilter::A

Create a Kalman filter with the specified matrices. A - System dynamics matrix C - Output matrix Q - Process noise covariance R - Measurement noise covariance P - Estimate error covariance

Definition at line 28 of file kalman.hpp.

◆ C

Eigen::MatrixXd KalmanFilter::C

Definition at line 28 of file kalman.hpp.

◆ dt

double KalmanFilter::dt
private

Definition at line 85 of file kalman.hpp.

◆ I

Eigen::MatrixXd KalmanFilter::I
private

Definition at line 91 of file kalman.hpp.

◆ initialized

bool KalmanFilter::initialized
private

Definition at line 88 of file kalman.hpp.

◆ K

Eigen::MatrixXd KalmanFilter::K

Definition at line 28 of file kalman.hpp.

◆ m

int KalmanFilter::m
private

Definition at line 74 of file kalman.hpp.

◆ n

int KalmanFilter::n
private

Definition at line 74 of file kalman.hpp.

◆ P

Eigen::MatrixXd KalmanFilter::P

Definition at line 28 of file kalman.hpp.

◆ P0

Eigen::MatrixXd KalmanFilter::P0

Definition at line 28 of file kalman.hpp.

◆ Q

Eigen::MatrixXd KalmanFilter::Q

Definition at line 28 of file kalman.hpp.

◆ R

Eigen::MatrixXd KalmanFilter::R

Definition at line 28 of file kalman.hpp.

◆ t

double KalmanFilter::t
private

Definition at line 82 of file kalman.hpp.

◆ t0

double KalmanFilter::t0
private

Definition at line 82 of file kalman.hpp.

◆ x_hat

Eigen::VectorXd KalmanFilter::x_hat
private

Definition at line 94 of file kalman.hpp.

◆ x_hat_new

Eigen::VectorXd KalmanFilter::x_hat_new
private

Definition at line 94 of file kalman.hpp.


The documentation for this class was generated from the following files:


datmo
Author(s): Kostas Konstantinidis
autogenerated on Tue May 2 2023 02:58:06