36 _x = Vector5d::Zero();
37 _xp = Vector5d::Zero();
38 _z = Vector3d::Zero();
39 _P = Matrix5d::Zero();
EKFilter< double, 0, false, false, false >::Matrix kMatrix
EKfilter matrix type.
void InitFilter(Vector5d &x_init)
Init filter.
EKFilter< double, 0, false, false, false >::Vector kVector
EKfilter vector type.
Vector3d _z
Measurement vector.
Eigen::Matrix< double, 3, 1 > Vector3d
void initEstimator(Vector5d &x, Matrix5d &P)
Vector5d _x
Estimated state.
Motion model class declaration.
nonholonomicEKFilter _estimator
Local estimator for process, Kalman filter with a constant velocity.
Matrix5d _P
Posteriori error covariance.
Eigen::Matrix< double, 5, 5 > Matrix5d
Eigen::Matrix< double, 5, 1 > Vector5d
void stepEstimator(Vector3d z)