37 o<<
" (#"<<t.
_uid<<
")";
78 covariance(0,0)=
_P(0,0);
79 covariance(0,1)=
_P(0,1);
81 covariance(1,0)=
_P(1,0);
82 covariance(1,1)=
_P(1,1);
86 double val=
mahalanobis(measurement,mean,covariance);
137 _id=prev_target->_id;
142 z(0)=prev_target->_xp(0);
143 z(1)=prev_target->_xp(1);
144 z(2)=prev_target->_xp(3);
173 const char COL_RESET[] =
"\x1b[0m";
174 const char RED[] =
"\x1b[31m";
175 cout << RED <<
"Error!!, Major" << COL_RESET << endl;
176 cout<<
"Bad failed target: "<<*
this<<endl;
195 _id=prev_target->_id;
215 cout<<
"Bad normal target: "<<*
this<<endl;
241 cout<<
"Bad new target: "<<*
this<<endl;
262 cout<<
"||||||||!!!!!!!!"<<endl;
263 cout<<
"_ntotal: "<<
_ntotal<<endl;
279 covariance(0,0)=
_P(0,0);
280 covariance(0,1)=
_P(0,1);
282 covariance(1,0)=
_P(1,0);
283 covariance(1,1)=
_P(1,1);
287 double val=
mahalanobis(predicted_position,mean,covariance);
293 double val=
biVariatePDF(predicted_position,mean,covariance);
306 return t1->_id<t2->_id;
311 return t1->_aux1<t2->_aux1;
316 return t1->_aux1>t2->_aux1;
bool compareTargetsByAux1Descending(TargetPtr t1, TargetPtr t2)
boost::shared_ptr< Measurement > MeasurementPtr
double mahalanobis(Vector2d &y, Vector2d &mean, Matrix2d &cov)
vector< Vector5d > _past_states
void incrementMissedAssociations(int inc=1)
ostream & operator<<(ostream &o, Target &t)
bool compareTargetsByAux1(TargetPtr t1, TargetPtr t2)
double getDistanceToPredictedPosition(TargetPtr &target, int algorithm=0)
Eigen::Matrix< double, 3, 1 > Vector3d
void cleanTargetAssociations(void)
bool compareTargetsById(TargetPtr t1, TargetPtr t2)
void initEstimator(Vector5d &x, Matrix5d &P)
vector< MeasurementPtr > _assigned_measurements
void decrementMissedAssociations(int dec=1)
Vector5d _x
Estimated state.
long _missed_associations
double getDistance(MeasurementPtr &mes, int algorithm=0)
void zeroMissedAssociations(void)
double biVariatePDF(Vector2d &x, Vector2d &m, Matrix2d &cov)
boost::shared_ptr< Target > TargetPtr
Shared pointer to the Target class.
Target class declaration.
nonholonomicEKFilter _estimator
Local estimator for process, Kalman filter with a constant velocity.
Matrix5d _P
Posteriori error covariance.
bool is_finite(const Eigen::MatrixBase< Derived > &x)
void stepEstimator(Vector3d z)