diff --git a/src/libraries/TRACKING/DTrackCandidate_factory_StraightLine.cc b/src/libraries/TRACKING/DTrackCandidate_factory_StraightLine.cc index ed76273bc..d7e36fd01 100644 --- a/src/libraries/TRACKING/DTrackCandidate_factory_StraightLine.cc +++ b/src/libraries/TRACKING/DTrackCandidate_factory_StraightLine.cc @@ -551,16 +551,17 @@ DTrackCandidate_factory_StraightLine::DoFilter(double t0,double OuterZ, cand->setPID(PiPlus); auto locTrackingCovarianceMatrix = dResourcePool_TMatrixFSym->Get_SharedResource(); - locTrackingCovarianceMatrix->ResizeTo(5, 5); + locTrackingCovarianceMatrix->ResizeTo(5, 5); + locTrackingCovarianceMatrix->Zero(); for(unsigned int loc_i = 0; loc_i < 4; ++loc_i) { for(unsigned int loc_j = 0; loc_j < 4; ++loc_j) (*locTrackingCovarianceMatrix)(loc_i, loc_j) = Cbest(loc_i, loc_j); } - (*locTrackingCovarianceMatrix)(4,4)=1.; + (*locTrackingCovarianceMatrix)(4,4)=10.; cand->setTrackingErrorMatrix(locTrackingCovarianceMatrix); - + cand->setErrorMatrix(Get7x7ErrorMatrix(locTrackingCovarianceMatrix,Sbest)); // Smooth the result if (Smooth(best_trajectory,best_updates,hits,cand) == NOERROR) cand->IsSmoothed=true; @@ -1453,15 +1454,17 @@ DTrackCandidate_factory_StraightLine::DoFilter(double t0,double start_z, auto locTrackingCovarianceMatrix = dResourcePool_TMatrixFSym->Get_SharedResource(); locTrackingCovarianceMatrix->ResizeTo(5, 5); + locTrackingCovarianceMatrix->Zero(); for(unsigned int loc_i = 0; loc_i < 4; ++loc_i) { for(unsigned int loc_j = 0; loc_j < 4; ++loc_j) (*locTrackingCovarianceMatrix)(loc_i, loc_j) = Cbest(loc_i, loc_j); } - (*locTrackingCovarianceMatrix)(4,4)=1.; + (*locTrackingCovarianceMatrix)(4,4)=10.; cand->setTrackingErrorMatrix(locTrackingCovarianceMatrix); - + cand->setErrorMatrix(Get7x7ErrorMatrix(locTrackingCovarianceMatrix,Sbest)); + _data.push_back(cand); } @@ -2330,3 +2333,41 @@ DTrackCandidate_factory_StraightLine::Smooth(deque&trajectory, return NOERROR; } +shared_ptr +DTrackCandidate_factory_StraightLine::Get7x7ErrorMatrix(shared_ptrC,DMatrix4x1 &S){ + auto C7x7 = dResourcePool_TMatrixFSym->Get_SharedResource(); + C7x7->ResizeTo(7, 7); + DMatrix J(7,5); + + double p=10.; // fixed: cannot measure + double tx_=S(state_tx); + double ty_=S(state_ty); + double x_=S(state_x); + double y_=S(state_y); + double tanl=1./sqrt(tx_*tx_+ty_*ty_); + double tanl2=tanl*tanl; + double lambda=atan(tanl); + double sinl=sin(lambda); + double sinl3=sinl*sinl*sinl; + + J(state_X,state_x)=J(state_Y,state_y)=1.; + J(state_Pz,state_ty)=-p*ty_*sinl3; + J(state_Pz,state_tx)=-p*tx_*sinl3; + J(state_Px,state_ty)=J(state_Py,state_tx)=-p*tx_*ty_*sinl3; + J(state_Px,state_tx)=p*(1.+ty_*ty_)*sinl3; + J(state_Py,state_ty)=p*(1.+tx_*tx_)*sinl3; + J(state_Pz,4)=-p*p*sinl; + J(state_Px,4)=tx_*J(state_Pz,4); + J(state_Py,4)=ty_*J(state_Pz,4); + J(state_Z,state_x)=-tx_*tanl2; + J(state_Z,state_y)=-ty_*tanl2; + double diff=tx_*tx_-ty_*ty_; + double frac=tanl2*tanl2; + J(state_Z,state_tx)=(x_*diff+2.*tx_*ty_*y_)*frac; + J(state_Z,state_ty)=(2.*tx_*ty_*x_-y_*diff)*frac; + + // C'= JCJ^T + *C7x7=(*C).Similarity(J); + + return C7x7; +} diff --git a/src/libraries/TRACKING/DTrackCandidate_factory_StraightLine.h b/src/libraries/TRACKING/DTrackCandidate_factory_StraightLine.h index d940a3ba3..82dbfcc6a 100644 --- a/src/libraries/TRACKING/DTrackCandidate_factory_StraightLine.h +++ b/src/libraries/TRACKING/DTrackCandidate_factory_StraightLine.h @@ -35,6 +35,16 @@ class DTrackCandidate_factory_StraightLine:public jana::JFactory&cdc_hits, DTrackCandidate *cand); + shared_ptr Get7x7ErrorMatrix(shared_ptrC,DMatrix4x1 &S); + double CDCDriftDistance(double t); double CDCDriftDistance(double dphi, double delta,double t); double CDCDriftVariance(double t);