35 #error This file should not be used, DEPRECATED
43 #include <visualization_msgs/Marker.h>
44 #include <visualization_msgs/MarkerArray.h>
46 #include <boost/shared_ptr.hpp>
47 #include <boost/format.hpp>
49 #include <Eigen/Dense>
50 #include <Eigen/Cholesky>
52 #include <kfilter/ekfilter.hpp>
54 #include <mtt/tree.hh>
55 #include <mtt/tree_util.hh>
58 #include <mtt/mht_types_shared.h>
60 #include <mtt/nonholonomic_kalman_filter.h>
62 using Eigen::Matrix4d;
63 using Eigen::Matrix2d;
64 using Eigen::Vector2d;
65 using Eigen::Vector4d;
66 using Eigen::MatrixXd;
67 using Eigen::VectorXd;
69 using namespace Kalman;
116 void SetIdentity(
kMatrix& M,
int size,
double value=1);
126 void SetZero(
kMatrix& M,
int size);
134 void InitFilter(Vector4d& x_init);
136 void InitFilter(Vector4d& x_init,Matrix4d& P_init);
157 void makeCommonProcess();
EKFilter< double, 0, false, false, false >::Matrix Matrix
EKfilter matrix type.
boost::shared_ptr< constantVelocityEKFilter > constantVelocityEKFilterPtr
EKFilter< double, 0, false, false, false >::Matrix kMatrix
EKfilter matrix type.
EKFilter< double, 0, false, false, false >::Vector kVector
EKfilter vector type.
Eigen::Matrix< double, 3, 1 > Vector3d
double dt
Time interval between measurements.
Big and complicated class that implements the MHT (Multiple Hypotheses Tracking) algorithm.
Time lt
Time of the last call to the makeCommonProcess function.
Hypotheses cluster definition.
Constant velocity Kalman filter.
EKFilter< double, 0, false, false, false >::Vector Vector
EKfilter vector type.