#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 |
Kalman filter implementation using Eigen. Based on the following introductory paper:
http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
Definition at line 15 of file kalman.hpp.
| KalmanFilter::KalmanFilter | ( | double | dt, |
| const Eigen::MatrixXd & | A, | ||
| const Eigen::MatrixXd & | C, | ||
| const Eigen::MatrixXd & | Q, | ||
| const Eigen::MatrixXd & | R, | ||
| const Eigen::MatrixXd & | P | ||
| ) |
| KalmanFilter::KalmanFilter | ( | ) |
Create a blank estimator.
Definition at line 27 of file kalman.cpp.
| void KalmanFilter::changeStates | ( | const Eigen::VectorXd & | new_states | ) |
Definition at line 62 of file kalman.cpp.
| void KalmanFilter::init | ( | ) |
Initialize the filter with initial states as zero.
Definition at line 38 of file kalman.cpp.
| 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.
|
inline |
Return the current state and time.
Definition at line 73 of file kalman.hpp.
|
inline |
Definition at line 74 of file kalman.hpp.
| 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.
| void KalmanFilter::update | ( | const Eigen::VectorXd & | y, |
| double | dt | ||
| ) |
Definition at line 78 of file kalman.cpp.
| 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.
| 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.
| Eigen::MatrixXd KalmanFilter::C |
Definition at line 28 of file kalman.hpp.
|
private |
Definition at line 85 of file kalman.hpp.
|
private |
Definition at line 91 of file kalman.hpp.
|
private |
Definition at line 88 of file kalman.hpp.
| Eigen::MatrixXd KalmanFilter::K |
Definition at line 28 of file kalman.hpp.
|
private |
Definition at line 74 of file kalman.hpp.
|
private |
Definition at line 74 of file kalman.hpp.
| Eigen::MatrixXd KalmanFilter::P |
Definition at line 28 of file kalman.hpp.
| Eigen::MatrixXd KalmanFilter::P0 |
Definition at line 28 of file kalman.hpp.
| Eigen::MatrixXd KalmanFilter::Q |
Definition at line 28 of file kalman.hpp.
| Eigen::MatrixXd KalmanFilter::R |
Definition at line 28 of file kalman.hpp.
|
private |
Definition at line 82 of file kalman.hpp.
|
private |
Definition at line 82 of file kalman.hpp.
|
private |
Definition at line 94 of file kalman.hpp.
|
private |
Definition at line 94 of file kalman.hpp.