#include <ekfIdentifier.h>
Public Types | |
typedef ublas::matrix< double > | matrix |
typedef ublas::vector< double > | vector |
Public Member Functions | |
EkfIdentifier (boost::shared_ptr< DynamicsModel > dynamics, const double &eps=1.0e-9) | |
EkfIdentifier (boost::shared_ptr< DynamicsModel > dynamics, matrix Px, matrix Pp) | |
boost::shared_ptr< DynamicsModel > | getModel () const |
Returns the identified dynamics model. | |
void | predict (vector u, const double &dt, matrix Qx, matrix Qp) |
Predicts the next state. | |
void | correct (ExtendedKalmanFilter< double >::ObserveFun h, vector z, matrix R) |
Corrects for errors between the observation and current state estimate. |
EkfIdentifier::EkfIdentifier | ( | boost::shared_ptr< DynamicsModel > | dynamics, | |
const double & | eps = 1.0e-9 | |||
) |
dynamics | model to identify | |
eps | small value used to calculate Jacobians |
EkfIdentifier::EkfIdentifier | ( | boost::shared_ptr< DynamicsModel > | dynamics, | |
matrix | Px, | |||
matrix | Pp | |||
) |
dynamics | model to identify | |
Px | state uncertainty matrix | |
Pp | parameter uncertainty matrix |
void EkfIdentifier::predict | ( | vector | u, | |
const double & | dt, | |||
matrix | Qx, | |||
matrix | Qp | |||
) |
Predicts the next state.
u | control input | |
dt | update time | |
Qx | state noise covariance matrix | |
Qp | parameter noise covariance matrix |
References getModel().
void EkfIdentifier::correct | ( | ExtendedKalmanFilter< double >::ObserveFun | h, | |
vector | z, | |||
matrix | R | |||
) |
Corrects for errors between the observation and current state estimate.
h | observation function | |
z | measurement | |
R | measuement noise covariance matrix |
References navlib::ExtendedKalmanFilter< T >::correct(), getModel(), and navlib::ExtendedKalmanFilter< T >::getState().