Merge pull request #1033 from JeffersonLab/StraightLine7x7ErrorMatrix

Add 7x7 error matrix to track candidate for straight line tracks
JeffersonLab · Jan 16, 2018 · d000727 · d000727
2 parents 9c60804 + 4bcc6b4
commit d000727
Show file tree

Hide file tree

Showing 2 changed files with 58 additions and 5 deletions.
diff --git 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_t>&trajectory,
    return NOERROR;
 }
 
+shared_ptr<TMatrixFSym> 
+DTrackCandidate_factory_StraightLine::Get7x7ErrorMatrix(shared_ptr<TMatrixFSym>C,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
@@ -35,6 +35,16 @@ class DTrackCandidate_factory_StraightLine:public jana::JFactory<DTrackCandidate
          state_tx,
          state_ty,
       };
+      enum state_cartesian{
+	state_Px,
+	state_Py,
+	state_Pz,
+	state_X,
+	state_Y,
+	state_Z,
+	state_T
+      };
+
 
       class trajectory_t{
          public:
@@ -110,6 +120,8 @@ class DTrackCandidate_factory_StraightLine:public jana::JFactory<DTrackCandidate
             vector<const DCDCTrackHit *>&cdc_hits,
             DTrackCandidate *cand); 
 
+      shared_ptr<TMatrixFSym> Get7x7ErrorMatrix(shared_ptr<TMatrixFSym>C,DMatrix4x1 &S);
+
       double CDCDriftDistance(double t);
       double CDCDriftDistance(double dphi, double delta,double t);
       double CDCDriftVariance(double t);