#include "GFKalman.h"

Public Member Functions
	GFKalman ()

	~GFKalman ()

void	operator() (GFTrack *track)
	Operator for use with STL. More...

void	operator() (std::pair< int, GFTrack * > tr)
	Operator for use with STL. More...

void	setLazy (Int_t)
	Switch lazy error handling. More...

void	setNumIterations (Int_t i)
	Set number of iterations for Kalman Filter. More...

void	processTrack (GFTrack *)
	Performs fit on a GFTrack. More...

void	fittingPass (GFTrack *, int dir)
	Performs fit on a GFTrack beginning with the current hit. More...

double	getChi2Hit (GFAbsRecoHit , GFAbsTrackRep )
	Calculates chi2 of a given hit with respect to a given track representation. More...

void	setInitialDirection (int d)
	Sets the inital direction of the track fit (1 for inner to outer, or -1 for outer to inner). The standard is 1 and is set in the ctor. More...

void	setBlowUpFactor (double f)
	Set the blowup factor (see blowUpCovs() ) More...

void	setMomLow (Double_t f)

void	setMomHigh (Double_t f)

void	setMaxUpdate (Double_t f)

void	setErrorScaleSTh (Double_t f)

void	setErrorScaleMTh (Double_t f)

Private Member Functions
void	processHit (GFTrack *, int, int, int)
	One Kalman step. More...

void	switchDirection (GFTrack *trk)
	Used to switch between forward and backward filtering. More...

TMatrixT< Double_t >	calcGain (const TMatrixT< Double_t > &cov, const TMatrixT< Double_t > &HitCov, const TMatrixT< Double_t > &H)
	Calculate Kalman Gain. More...

TMatrixT< Double_t >	calcCov7x7 (const TMatrixT< Double_t > &cov, const genf::GFDetPlane &plane)

double	chi2Increment (const TMatrixT< Double_t > &r, const TMatrixT< Double_t > &H, const TMatrixT< Double_t > &cov, const TMatrixT< Double_t > &V)
	this returns the reduced chi2 increment for a hit More...

void	blowUpCovs (GFTrack *trk)
	this is needed to blow up the covariance matrix before a fitting pass drops off-diagonal elements and blows up diagonal by blowUpFactor More...

void	blowUpCovsDiag (GFTrack *trk)

Private Attributes
int	fInitialDirection

Int_t	fNumIt

double	fBlowUpFactor

Double_t	fMomLow

Double_t	fMomHigh

Double_t	fMaxUpdate

Double_t	fErrScaleSTh

Double_t	fErrScaleMTh

bool	fGENfPRINT

Detailed Description

Definition at line 54 of file GFKalman.h.

Constructor & Destructor Documentation

genf::GFKalman::GFKalman ( )

Definition at line 41 of file GFKalman.cxx.

                       :fInitialDirection(1),fNumIt(3),fBlowUpFactor(50.),fMomLow(-100.0),fMomHigh(100.0),fMaxUpdate(1.0),fErrScaleSTh(1.0),fErrScaleMTh(1.0), fGENfPRINT(false)
 {
   art::ServiceHandle<art::TFileService> tfs;
 }

genf::GFKalman::~GFKalman ( )

Definition at line 46 of file GFKalman.cxx.

46 {;}

Member Function Documentation

void genf::GFKalman::blowUpCovs ( GFTrack * trk )

private

this is needed to blow up the covariance matrix before a fitting pass drops off-diagonal elements and blows up diagonal by blowUpFactor

Definition at line 156 of file GFKalman.cxx.

References fBlowUpFactor, genf::GFAbsTrackRep::getCov(), genf::GFTrack::getNumReps(), genf::GFAbsTrackRep::getStatusFlag(), genf::GFTrack::getTrackRep(), and genf::GFAbsTrackRep::setCov().

Referenced by processTrack(), and setErrorScaleMTh().

                                          {
   int nreps=trk->getNumReps();
   for(int irep=0; irep<nreps; ++irep){
     GFAbsTrackRep* arep=trk->getTrackRep(irep);
     //dont do it for already compromsied reps, since they wont be fitted anyway
     if(arep->getStatusFlag()==0) { 
       TMatrixT<Double_t> cov = arep->getCov();
       for(int i=0;i<cov.GetNrows();++i){
         for(int j=0;j<cov.GetNcols();++j){
           //if(i!=j){//off diagonal
           //  cov[i][j]=0.;
           //}
           //else{//diagonal
           cov[i][j] = cov[i][j] * fBlowUpFactor;
           //}
         }
       }
       arep->setCov(cov);
     }
   }  
 }

void genf::GFKalman::blowUpCovsDiag ( GFTrack * trk )

private

Definition at line 134 of file GFKalman.cxx.

References fBlowUpFactor, genf::GFAbsTrackRep::getCov(), genf::GFTrack::getNumReps(), genf::GFAbsTrackRep::getStatusFlag(), genf::GFTrack::getTrackRep(), and genf::GFAbsTrackRep::setCov().

Referenced by setErrorScaleMTh().

                                              {
   int nreps=trk->getNumReps();
   for(int irep=0; irep<nreps; ++irep){
     GFAbsTrackRep* arep=trk->getTrackRep(irep);
     //dont do it for already compromsied reps, since they wont be fitted anyway
     if(arep->getStatusFlag()==0) { 
       TMatrixT<Double_t> cov = arep->getCov();
       for(int i=0;i<cov.GetNrows();++i){
         for(int j=0;j<cov.GetNcols();++j){
           if(i!=j){//off diagonal
             cov[i][j]=0.;
           }
           else{//diagonal
             cov[i][j] = cov[i][j] * fBlowUpFactor;
             //      cov[0][0] = 0.1;
           }
         }
       }
       arep->setCov(cov);
     }
   }  
 }

TMatrixT< Double_t > genf::GFKalman::calcCov7x7	(	const TMatrixT< Double_t > &	cov,
		const genf::GFDetPlane &	plane
	)

private

Definition at line 673 of file GFKalman.cxx.

References genf::GFDetPlane::getU(), genf::GFDetPlane::getV(), GFException::setFatal(), and w.

Referenced by processHit(), and setErrorScaleMTh().

 {
   // This ends up, confusingly, as: 7 columns, 5 rows!
   TMatrixT<Double_t> jac(7,5); // X,Y,Z,UX,UY,UZ,Theta in detector coords
 
   TVector3 u=plane.getU();
   TVector3 v=plane.getV();
   TVector3 w=u.Cross(v);
 
   // Below line has been done, with code change in GFKalman that has updated
   // the plane orientation by now.
   //  TVector3 pTilde = 1.0 * (w + state[1][0] * u + state[2][0] * v);
   TVector3 pTilde = w;
   double pTildeMag = pTilde.Mag();
 
   jac.Zero();
   jac[6][0] = 1.; //  Should be C as in GFSpacepointHitPolicy. 16-Feb-2013.
 
   jac[0][3] = u[0];
   jac[1][3] = u[1];
   jac[2][3] = u[2];
 
   jac[0][4] = v[0];
   jac[1][4] = v[1];
   jac[2][4] = v[2];
 
   // cnp'd from RKTrackRep.cxx, line ~496
   // da{x,y,z}/du'
   jac[3][1] = 1.0/pTildeMag*(u.X()-pTilde.X()/(pTildeMag*pTildeMag)*u*pTilde);
   jac[4][1] = 1.0/pTildeMag*(u.Y()-pTilde.Y()/(pTildeMag*pTildeMag)*u*pTilde);
   jac[5][1] = 1.0/pTildeMag*(u.Z()-pTilde.Z()/(pTildeMag*pTildeMag)*u*pTilde);
   // da{x,y,z}/dv'
   jac[3][2] = 1.0/pTildeMag*(v.X()-pTilde.X()/(pTildeMag*pTildeMag)*v*pTilde);
   jac[4][2] = 1.0/pTildeMag*(v.Y()-pTilde.Y()/(pTildeMag*pTildeMag)*v*pTilde);
   jac[5][2] = 1.0/pTildeMag*(v.Z()-pTilde.Z()/(pTildeMag*pTildeMag)*v*pTilde);
 
   // y = A.x => x = A^T.A.A^T.y
   // Thus, y's Jacobians Jac become for x (Jac^T.Jac)^(-1) Jac^T
 
   TMatrixT<Double_t> jac_t(TMatrixT<Double_t>::kTransposed,jac);
   TMatrixT<Double_t> jjInv(jac_t * jac);
   //jInv.Zero();
 
   double det(0.0);
   try {
     jjInv.Invert(&det); // this is all 1s on the diagonal, perhaps
                         // to no one's surprise.
   }
   catch (cet::exception &)
     {
       throw GFException("GFKalman: Jac.T*Jac is not invertible. But keep plowing on ... ",__LINE__,__FILE__).setFatal();
     }
   if(TMath::IsNaN(det)) {
     throw GFException("GFKalman: det of Jac.T*Jac is nan",__LINE__,__FILE__).setFatal();
   }
 
   TMatrixT<Double_t> j5x7 = jjInv*jac_t; 
   TMatrixT<Double_t> c7x7 = j5x7.T() * (cov * j5x7);
   return c7x7;
 }

TMatrixT< Double_t > genf::GFKalman::calcGain	(	const TMatrixT< Double_t > &	cov,
		const TMatrixT< Double_t > &	HitCov,
		const TMatrixT< Double_t > &	H
	)

private

Calculate Kalman Gain.

Definition at line 735 of file GFKalman.cxx.

References e, GFException::setFatal(), and GFException::setMatrices().

Referenced by processHit(), and setErrorScaleMTh().

                                                      {
 
   // calculate covsum (V + HCH^T)
 
   // Comment next 3 out for normal running.
   //cov.Print();
   //H.Print();
   //HitCov.Print();
   TMatrixT<Double_t> covsum1(cov,TMatrixT<Double_t>::kMultTranspose,H);
   //  covsum1.Print();
   TMatrixT<Double_t> covsum(H,TMatrixT<Double_t>::kMult,covsum1);
 
   //covsum.Print();
 
   covsum+=HitCov;
   //covsum = (covsum,TMatrixT<Double_t>::kPlus,HitCov);
   // covsum.Print();
 
   // invert
   double det=0;
   covsum.SetTol(1.0e-23); // to avoid inversion problem, EC, 8-Aug-2011.
   covsum.Invert(&det);
   //    std::cout << "GFKalman:: calGain(), det is  "<< det << std::endl;
   if(TMath::IsNaN(det)) {
     throw GFException("Kalman Gain: det of covsum is nan",__LINE__,__FILE__).setFatal();
   }
 
   if(det==0){
         GFException exc("cannot invert covsum in Kalman Gain - det=0",
                                                 __LINE__,__FILE__);
         exc.setFatal();
         std::vector< TMatrixT<Double_t> > matrices;
         matrices.push_back(cov);
         matrices.push_back(HitCov);
         matrices.push_back(covsum1);
         matrices.push_back(covsum);
         exc.setMatrices("cov, HitCov, covsum1 and covsum",matrices);
         throw exc;
 
   }
 
   // gain is CH^T/(V + HCH^T)
   // calculate gain
   TMatrixT<Double_t> gain1(H,TMatrixT<Double_t>::kTransposeMult,covsum);
   TMatrixT<Double_t> gain(cov,TMatrixT<Double_t>::kMult,gain1);
 
   return gain;
 }

double genf::GFKalman::chi2Increment	(	const TMatrixT< Double_t > &	r,
		const TMatrixT< Double_t > &	H,
		const TMatrixT< Double_t > &	cov,
		const TMatrixT< Double_t > &	V
	)

private

this returns the reduced chi2 increment for a hit

Definition at line 224 of file GFKalman.cxx.

References e, R, GFException::setFatal(), GFException::setMatrices(), and GFException::setNumbers().

Referenced by getChi2Hit(), processHit(), and setErrorScaleMTh().

                                                                                        {
 
   // residuals covariances:R=(V - HCH^T)
   TMatrixT<Double_t> R(V);
   TMatrixT<Double_t> covsum1(cov,TMatrixT<Double_t>::kMultTranspose,H);
   TMatrixT<Double_t> covsum(H,TMatrixT<Double_t>::kMult,covsum1);
 
   R-=covsum;
 
   // chisq= r^TR^(-1)r
   double det=0.;
   TMatrixT<Double_t> Rsave(R);
   R.SetTol(1.0e-30); // to avoid inversion problem, EC, 8-Aug-2011. Was23, 9-Jan-2012.
 
   try
     {
       R.Invert(&det);
     }
   catch (cet::exception &)
     {
       GFException e("Kalman Chi2Increment: R is not even invertible. But keep plowing on ... ",__LINE__,__FILE__);
       //e.setFatal();
       //throw e;
     }
   if(TMath::IsNaN(det)) {
     throw GFException("Kalman Chi2Increment: det of covsum is nan",__LINE__,__FILE__).setFatal();
   }
   TMatrixT<Double_t> residTranspose(r);
   residTranspose.T();
   TMatrixT<Double_t> chisq=residTranspose*(R*r);
   assert(chisq.GetNoElements()==1);
 
   if(TMath::IsNaN(chisq[0][0])){
         GFException exc("chi2 is nan",__LINE__,__FILE__);
         exc.setFatal();
         std::vector<double> numbers;
         numbers.push_back(det);
         exc.setNumbers("det",numbers);
         std::vector< TMatrixT<Double_t> > matrices;
         matrices.push_back(r);
         matrices.push_back(V);
         matrices.push_back(Rsave);
         matrices.push_back(R);
         matrices.push_back(cov);
         exc.setMatrices("r, V, Rsave, R, cov",matrices);
     throw exc;
   }
 
   return chisq[0][0];
 }

void genf::GFKalman::fittingPass	(	GFTrack *	trk,
		int	dir
	)

Performs fit on a GFTrack beginning with the current hit.

Definition at line 179 of file GFKalman.cxx.

References genf::GFTrack::addFailedHit(), genf::GFBookkeeping::clearFailedHits(), e, genf::GFTrack::getBK(), genf::GFTrack::getNextHitToFit(), genf::GFTrack::getNumHits(), genf::GFTrack::getNumReps(), genf::GFAbsTrackRep::getStatusFlag(), genf::GFTrack::getTrackRep(), GFException::info(), GFException::isFatal(), processHit(), genf::GFAbsTrackRep::setChiSqu(), genf::GFAbsTrackRep::setNDF(), genf::GFTrack::setNextHitToFit(), genf::GFAbsTrackRep::setStatusFlag(), and GFException::what().

Referenced by processTrack(), and setNumIterations().

                                                     {
   //loop over hits
 
   unsigned int nhits=trk->getNumHits();
   unsigned int starthit=trk->getNextHitToFit();
 
   int nreps=trk->getNumReps();
   int ihit=(int)starthit;
 
   //clear chi2 sum and ndf sum in track reps
   for(int i=0;i<nreps;++i){
     GFAbsTrackRep* arep=trk->getTrackRep(i);
     arep->setChiSqu(0.);
     arep->setNDF(0);
   }
 
   //clear failedHits and outliers
   trk->getBK()->clearFailedHits();
 
   while((ihit<(int)nhits && direction==1) || (ihit>-1 && direction==-1)){
     //    GFAbsRecoHit* ahit=trk->getHit(ihit);
     // loop over reps
     for(int irep=0; irep<nreps; ++irep){
           GFAbsTrackRep* arep=trk->getTrackRep(irep);
           if(arep->getStatusFlag()==0) { 
             try {
               processHit(trk,ihit,irep,direction);
             }
             catch(GFException& e) {
               trk->addFailedHit(irep,ihit);
               std::cerr << e.what();
               e.info();
               if(e.isFatal()) {
                 arep->setStatusFlag(1);
                 continue; // go to next rep immediately
               }
             }   
           }
     }// end loop over reps
     ihit+=direction;
   }// end loop over hits
   trk->setNextHitToFit(ihit-2*direction);
   //trk->printGFBookkeeping();
 }

double genf::GFKalman::getChi2Hit	(	GFAbsRecoHit *	hit,
		GFAbsTrackRep *	rep
	)

Calculates chi2 of a given hit with respect to a given track representation.

Definition at line 278 of file GFKalman.cxx.

References chi2Increment(), genf::GFAbsTrackRep::extrapolate(), fErrScaleMTh, genf::GFAbsRecoHit::getDetPlane(), genf::GFAbsTrackRep::getDim(), genf::GFAbsRecoHit::getHitCov(), genf::GFAbsRecoHit::getHMatrix(), and genf::GFAbsRecoHit::residualVector().

Referenced by setNumIterations().

 {
   // get prototypes for matrices
   int repDim=rep->getDim();
   TMatrixT<Double_t> state(repDim,1);
   TMatrixT<Double_t> cov(repDim,repDim);;
   GFDetPlane pl=hit->getDetPlane(rep);
   rep->extrapolate(pl,state,cov);
 
 
   TMatrixT<Double_t> H = hit->getHMatrix(rep);
 
   // get hit covariances  
   TMatrixT<Double_t> V=hit->getHitCov(pl);
   V[0][0] *= fErrScaleMTh;
   TMatrixT<Double_t> r=hit->residualVector(rep,state,pl);
   assert(r.GetNrows()>0);
 
   r[0][0] = fabs(r[0][0]);
   //this is where chi2 is calculated
   double chi2 = chi2Increment(r,H,cov,V);
 
   return chi2/r.GetNrows();
 }

void genf::GFKalman::operator() ( GFTrack * track )

inline

Operator for use with STL.

This operator allows to use the std::foreach algorithm with an STL container o GFTrack* objects.

Definition at line 69 of file GFKalman.h.

References processTrack().

69 {processTrack(track);}

genf::GFKalman::processTrack

void processTrack(GFTrack *)

Performs fit on a GFTrack.

Definition: GFKalman.cxx:48

track

Float_t track

Definition: plot.C:34

void genf::GFKalman::operator() ( std::pair< int, GFTrack * > tr )

inline

Operator for use with STL.

This operator allows to use the std::foreach algorithm with an STL container o GFTrack* objects.

Definition at line 76 of file GFKalman.h.

References processTrack().

76 {processTrack(tr.second);}

genf::GFKalman::processTrack

void processTrack(GFTrack *)

Performs fit on a GFTrack.

Definition: GFKalman.cxx:48

void genf::GFKalman::processHit	(	GFTrack *	tr,
		int	ihit,
		int	irep,
		int	direction
	)

private

One Kalman step.

Performs

Extrapolation to detector plane of the hit
Calculation of residual and Kalman Gain
Update of track representation state and chi2

Definition at line 305 of file GFKalman.cxx.

Referenced by fittingPass(), and setErrorScaleMTh().

                                                                      {
   GFAbsRecoHit* hit = tr->getHit(ihit);
   GFAbsTrackRep* rep = tr->getTrackRep(irep);
 
   // get prototypes for matrices
   int repDim = rep->getDim();
   TMatrixT<Double_t> state(repDim,1);
   TMatrixT<Double_t> cov(repDim,repDim);
   static TMatrixT<Double_t> covFilt(cov);
   const double pi2(10.0);
   GFDetPlane pl, plPrev;
   unsigned int nhits=tr->getNumHits();
   int phit=ihit;
   static TMatrixT<Double_t> oldState(5,1);
   static std::vector<TVector3> pointsPrev;
 
   const double eps(1.0e-6);
   if (direction>0 && ihit>0)
     {
       phit = ihit - 1;
     }
   if (direction<0 && ihit<((int)nhits-1))
     {
       phit = ihit + 1;
     }
   GFAbsRecoHit* hitPrev = tr->getHit(phit);
 
   /* do an extrapolation, if the trackrep irep is not given
    * at this ihit position. This will usually be the case, but
    * not if the fit turns around
    */
   //  std::cout << "GFKalman::ProcessHit(): ihit is " << ihit << std::endl;
   //  std::cout << "GFKalman::ProcessHit(): direction is "  << direction << std::endl;
   //rep->Print();
 
 
   Double_t thetaPlanes(0.0);
   if(ihit!=tr->getRepAtHit(irep)){
     //std::cout << "not same" << std::endl;
     // get the (virtual) detector plane. This call itself calls 
     // extrapolateToPoint(), which calls Extrap() with just 2 args and 
     // default 3rd arg, which propagates track through
     // material to find the next plane. But it in fact seems 
     // to just return this pl and plPrev, as desired. Something in Extrap()
     // kicks it out... even though I can see it walking through material ...
     // A mystery.
 
     pl=hit->getDetPlane(rep);
     plPrev=hitPrev->getDetPlane(rep);
 
       // std::cout << "GFKalman::ProcessHit(): hit is ... " << ihit << std::endl;
       //hit->Print();
       //std::cout << "GFKalman::ProcessHit(): plane is ... " <<  std::endl;
       //pl.Print();
 
     //do the extrapolation. This calls Extrap(), which wraps up RKutta and 
     // GFMaterials calls. state is intialized, pl is unchanged. Latter behavior
     // I will alter below.
     try{
       rep->extrapolate(pl,state,cov);
       /*
       if ( std::isnan(cov[0][0]) )
         {
           cov = covFilt;
         }
       else
         {
           covFilt = cov; 
         }
       */
     }
     catch (cet::exception &)
       {
         throw cet::exception("GFKalman.cxx: ") << " Line " << __LINE__ << ", " << __FILE__ << " ...Rethrow. \n";
       }
 
   }
   else{
     pl = rep->getReferencePlane();
     plPrev = hitPrev->getDetPlane(rep);
     state = rep->getState();
     cov = rep->getCov();
   }
 
   // Update plane. This is a code change from Genfit out of box.
   // state has accumulated the material/magnetic field changes since last
   // step. Here, we make the *plane* at this step know about that bend.
   // We will not, in the end, save this plane, cuz Genfit knows to properly
   // calculate it later. 
   // Actually, I do *not* change the plane. EC, 11-May-2012.
   TVector3 u(pl.getU());
   TVector3 v(pl.getV());
   TVector3 wold(u.Cross(v));
   //Double_t sign(1.0);
   //if ((direction==-1) && ihit==((int)nhits-1)) sign = -1.0;
 
   TVector3 pTilde = direction * (wold + state[1][0] * u + state[2][0] * v);
   TVector3 w(pTilde.Unit());
   // Find angle/vector through which we rotate. Use it subsequently.
   TVector3 rot(wold.Cross(w));
   Double_t ang(TMath::ACos(w*wold));
   ang = TMath::Min(ang,0.95*TMath::Pi()/2.0);
   /*
     {
       u.Rotate(ang,rot);
       v.Rotate(ang,rot);
       
       pl.setNormal(w);
       pl.setU(u.Unit());
       pl.setV(v.Unit());
     }
   */
   if(cov[0][0]<1.E-50 || TMath::IsNaN(cov[0][0])){
     //std::cout<<"GFKalman::processHit() 0. Calling Exception."<<std::endl;
         GFException exc(COVEXC,__LINE__,__FILE__);
         if (fGENfPRINT) cov.Print();
         if (fGENfPRINT) std::cout<<"GFKalman::processHit() 1. No longer throw exception. Force cov[0][0] to 0.01."<<std::endl;
         // std::cout<<"GFKalman::processHit() 1. About to throw GFException."<<std::endl;
         cov = covFilt;
         //      throw exc;
         // std::cout<<"GFKalman::processHit() 2. Back from GFException."<<std::endl;
 
   }
   
   /*
   if(direction==1){
     tr->getBK(irep)->setMatrix("fPredSt",ihit,state);
     tr->getBK(irep)->setMatrix("fPredCov",ihit,cov);
     tr->getBK(irep)->setGFDetPlane("f",ihit,pl);
   }
   else{
     tr->getBK(irep)->setMatrix("bPredSt",ihit,state);
     tr->getBK(irep)->setMatrix("bPredCov",ihit,cov);
     tr->getBK(irep)->setGFDetPlane("b",ihit,pl);
   }
   */
 
   //  TMatrixT<Double_t> origcov=rep->getCov();
 
   int pdg = tr->getPDG();
   TParticlePDG * part = TDatabasePDG::Instance()->GetParticle(pdg);
   Double_t mass = part->Mass();
 
   Double_t dist = (pl.getO()-plPrev.getO()).Mag();
   Double_t mom = fabs(1.0/state[0][0]);
   Double_t beta = mom/sqrt(mass*mass+mom*mom);
   const Double_t lowerLim(0.01);
   if (std::isnan(dist) || dist<=0.0) dist=lowerLim; // don't allow 0s here.
   if (std::isnan(beta) || beta<0.01) beta=0.01;
   TMatrixT<Double_t> H=hit->getHMatrix(rep,beta,dist);
 
 
   // Can force huge error here on p and du,v/dw, and thus insensitivity to p, 
   // by setting V[0][0]->inf.
   TMatrixT<Double_t> V=hit->getHitCov(pl,plPrev,state,mass); 
   V[0][0] *= fErrScaleMTh;
   tr->setHitMeasuredCov(V);
 
   // calculate kalman gain ------------------------------
   TMatrixT<Double_t> Gain(calcGain(cov,V,H));
 
   //std::cout << "GFKalman:: processHits(), state is  " << std::endl;
   //rep->getState().Print();
 
   TMatrixT<Double_t> res=hit->residualVector(rep,state,pl,plPrev,mass);
   // calculate update -----------------------------------
 
 
   //  TVector3 pointer((pl.getO()-plPrev.getO()).Unit());
 
   TMatrixT<Double_t> rawcoord = hit->getRawHitCoord();
   TVector3 point(rawcoord[0][0],rawcoord[1][0],rawcoord[2][0]);
   TMatrixT<Double_t> prevrawcoord = hitPrev->getRawHitCoord();
   TVector3 pointPrev(prevrawcoord[0][0],prevrawcoord[1][0],prevrawcoord[2][0]);
   pointsPrev.push_back(pointPrev);
   TMatrixT<Double_t> Hnew(H);
   if ((ihit==((int)nhits-1)&&direction==-1) || (ihit==0&&direction==1)) 
     pointsPrev.clear();
 
   TVector3 pointer((point-pointPrev).Unit());
   static TVector3 pointerPrev(pointer);
   if (ihit==0&&direction==1   ) 
     {pointer[0] = 0.0;pointer[1] = 0.0;pointer[2] = 1.0;}
   if (ihit==((int)nhits-1)&&direction==-1) 
     {pointer[0] = 0.0;pointer[1] = 0.0;pointer[2] = -1.0;}
   double thetaMeas = TMath::Min(fabs(pointer.Angle(pointerPrev)),0.95*TMath::Pi()/2.0);
   // Below line introduced because it's not true we predict the angle to be
   // precisely ang. If we'd taken this quantity from our transport result
   // (with measurement errors in it) we'd expect a smear like this on ang.
   // EC, 22-Feb-2012. 
   // Error is taken on th^2
   ang = (Hnew*state)[0][0]; // H->Hnew
 
   // fErrScale is extra-fun bonus factor!
   //thetaPlanes = ang*ang; // + fErrScaleSTh*gRandom->Gaus(0.0,V[0][0]);
   thetaPlanes = gRandom->Gaus(0.0,ang*ang);
   thetaPlanes = TMath::Min(sqrt(fabs(thetaPlanes)),0.95*TMath::Pi()/2.0);
 
   Double_t dtheta =  thetaMeas - thetaPlanes; // was fabs(res[0][0]). EC, 26-Jan-2012
   if (((ihit==((int)nhits-1)||ihit==((int)nhits-2))&&direction==-1) || ((ihit==0||ihit==1)&&direction==1)) 
     {
       dtheta = 0.0; // at the bare minimum (Jan-2013). Now (Feb-2013), ....
       // Do not let these 4 points*direction influence the state or chi2.
       rep->setData(state,pl,&cov); // This is where fState,
                                    // fRefPlane and fCov are updated.
       pointerPrev = pointer;
       return;
     }
 
   oldState = state;
 
   res[0][0] = dtheta/Hnew[0][0];
   // The particle was propagated through material until its poca
   // to this current hit was found. At that point its plane is defined, 
   // in which du/dw=dv/dw=0 by construction. But, there's a deviation
   // in those two quantities measured in the *previous* plane which we want.
   TVector3 uPrev(plPrev.getU());
   TVector3 vPrev(plPrev.getV());
   TVector3 wPrev(u.Cross(v));
   // We want the newest momentum vector's d{u,v}/dw defined in the prev plane. 
   // That is the predicted du,v/dw. We will subtract from the actual.
   // But the u's and v's need to come from the State not the Planes, cuz I
   // haven't updated pl,plPrev per latest State. plFilt, the updated plPrev,
   // is what we want. w is from updated new plane pl, per latest State. 
   // e.g. plFilt.
   static GFDetPlane plFilt;//(pl);
   if (plFilt.getO().Mag()>eps) 
     {
       uPrev = plFilt.getU();
       vPrev = plFilt.getV();
       wPrev = plFilt.getNormal();
     }
   //res[1][0] = (pointer*uPrev)/(pointer*wPrev) - (w*uPrev)/(w*wPrev);
   //res[2][0] = (pointer*vPrev)/(pointer*wPrev) - (w*vPrev)/(w*wPrev);
   //  res[1][0] = 0.0;
   //  res[2][0] = 0.0;
 
   TMatrixT<Double_t> update=Gain*res;
 
   // Don't let crazy ass spacepoints which pull the fit all over the place
   // be allowed to update the state. Use __ xRMS for fMaxUpdate.
   // Use a larger limit (~0.1) when starting with too-high seed momentum.
   // Smaller, when starting with an underestimate.
   //
   // Don't allow fits above 20 GeV, or so. Else, 1/p will likely 
   // migrate across zero. Similarly, don't allow tiny momentum.
   // 
   tr->setHitUpdate(update);
   if (fabs(update[0][0])>fMaxUpdate ) 
     { // zero the gain so as to not update cov, state
       // zero the updates
       update[0][0] = 0.0;/* */
       //      update[0][0] = fMaxUpdate*update[0][0]/fabs(update[0][0]);
       // either of below leads to craziness, somehow.
       // update.Zero();
       // Gain.Zero();
     }
   state+=update; // prediction overwritten!      
 
   // Debugging purposes
   TMatrixT<Double_t> GH(Gain*Hnew);
   if (GH[0][0] > 1.)
     {
       //      std::cout << "GFKalman:: Beginnings of a problem." << std::endl;
       Hnew[0][0] = Hnew[0][0] - eps/Gain[0][0];
     }
   TMatrixT<Double_t> GHc(GH*cov);
   
   cov-=Gain*(Hnew*cov);
 
   // Below is protection required at end of contained track when
   // momentum is tiny and cov[0][0] gets huge.
   
   //  if (cov[0][0]>pi2/Hnew[0][0] || cov[0][0] <= 0.0 || TMath::IsNaN(cov[0][0])) 
   if (cov[0][0] <= 0.0 || TMath::IsNaN(cov[0][0])) 
                                                                                                                                                                                                                                                                                              {
       cov[0][0]=pi2/Hnew[0][0]; // some big value.a
       //cov = covFilt;
     }
   else  // store away a good cov matrix
     {
       covFilt = cov;
     }
   
 
   TMatrixT<double> cov7x7(calcCov7x7(cov,pl));
   tr->setHitCov(cov);
   tr->setHitCov7x7(cov7x7);
   tr->setHitState(state);
 
   if (fabs(1.0/state[0][0])<fMomLow) 
     state[0][0] = 1.0/fMomLow*fabs(state[0][0])/state[0][0];
   if (fabs(1.0/state[0][0])>fMomHigh) 
     state[0][0] = 1.0/fMomHigh*fabs(state[0][0])/state[0][0];
 
 
   // Let's also calculate the "filtered" plane, and pointer here.
   // Use those to calculate filtered chisq and pass to setHitPlane()
   // for plane-by-plane correct "filtered" track pointing that gets
   // stuffed into the Track(). 
   TVector3 uf(pl.getU());
   TVector3 vf(pl.getV());
   TVector3 wf(uf.Cross(vf));
   TVector3 Of(pl.getO());
   // direction *
   //Double_t sign2(1.0);
   //if (direction==-1 && ihit==(int)nhits-1) sign2 = -1.0;
 
   TVector3 pf = direction*(wf + state[1][0] * uf + state[2][0] * vf);
   TVector3 pposf = Of + state[3][0] * uf + state[4][0] * vf;
   Double_t angf = TMath::Min(fabs(pf.Angle(wf)),0.95*TMath::Pi()/2.0);
   TVector3 rotf(wf.Cross(pf.Unit()));
 
   uf.Rotate(angf,rotf);
   vf.Rotate(angf,rotf);
   wf = uf.Cross(vf);
   plFilt.setU(uf.Unit());
   plFilt.setV(vf.Unit());
   plFilt.setO(pposf);
   plFilt.setNormal(pf.Unit());
 
   tr->setHitPlaneXYZ(plFilt.getO());
   tr->setHitPlaneUxUyUz(plFilt.getNormal());
   tr->setHitPlaneU(plFilt.getU());
   tr->setHitPlaneV(plFilt.getV());
 
   // calculate filtered chisq from filtered residuals
   TMatrixT<Double_t> r=hit->residualVector(rep,state,plFilt,plPrev,mass);
   if (direction==-1) wold.Rotate(TMath::Pi(),wold.Orthogonal());
   dtheta = thetaMeas - TMath::Min(fabs(wold.Angle(plFilt.getNormal())),0.95*TMath::Pi()/2.);
   r[0][0] = dtheta/Hnew[0][0];
   //r[1][0] = (pointer*uPrev)/(pointer*wPrev) - (wf*uPrev)/(wf*wPrev);
   //r[2][0] = (pointer*vPrev)/(pointer*wPrev) - (wf*vPrev)/(wf*wPrev);
   //r[1][0] = 0.;
   //r[2][0] = 0.;
 
   double chi2 = chi2Increment(r,Hnew,cov,V);
   int ndf = r.GetNrows();
   if (update[0][0]==0.0) {chi2=0.0; ndf=0;};
   rep->addChiSqu( chi2 );
   rep->addNDF( ndf );
   pointerPrev = pointer;
   tr->setHitChi2(chi2);
   /*
   if(direction==1){
     tr->getBK(irep)->setNumber("fChi2",ihit,chi2/ndf);
   }
   else{
     tr->getBK(irep)->setNumber("bChi2",ihit,chi2/ndf);
   }
   */
 
   // if we survive until here: update TrackRep
   //rep->setState(state);
   //rep->setCov(cov);
   //rep->setReferencePlane(pl);
 
   // Since I've tilted the planes, d(u,v)/dw=0 by construction.
   // But, I *haven't* tilted the planes! EC, 11-May-2012.
   //state[1][0] = 0.0;
   //state[2][0] = 0.0;
   rep->setData(state,pl,&cov); // This is where fState,
                                // fRefPlane and fCov are updated.
   tr->setRepAtHit(irep,ihit);
 
 }

void genf::GFKalman::processTrack ( GFTrack * trk )

Performs fit on a GFTrack.

The hits are processed in the order in which they are stored in the GFTrack object. Sorting of hits in space has to be done before!

Definition at line 48 of file GFKalman.cxx.

References blowUpCovs(), genf::GFTrack::clearRepAtHit(), fInitialDirection, fittingPass(), fNumIt, genf::GFAbsTrackRep::getCov(), genf::GFTrack::getNumHits(), genf::GFTrack::getNumReps(), genf::GFAbsTrackRep::getReferencePlane(), genf::GFAbsTrackRep::getState(), genf::GFTrack::getTrackRep(), GFException::setFatal(), genf::GFTrack::setNextHitToFit(), and switchDirection().

Referenced by operator()(), trkf::Track3DKalman::produce(), trkf::Track3DKalmanSPS::produce(), and setNumIterations().

                                            {
   int direction=fInitialDirection;
   if ((direction != 1) && (direction != -1))
     throw GFException(std::string(__func__) + ": wrong direction", __LINE__, __FILE__).setFatal();
   //  trk->clearGFBookkeeping();
   trk->clearRepAtHit();
   /*
   int nreps=trk->getNumReps();
   for(int i=0; i<nreps; ++i){ 
     trk->getBK(i)->setNhits(trk->getNumHits());
     trk->getBK(i)->bookMatrices("fPredSt");
     trk->getBK(i)->bookMatrices("fPredCov");
     trk->getBK(i)->bookMatrices("bPredSt");
     trk->getBK(i)->bookMatrices("bPredCov");
     trk->getBK(i)->bookGFDetPlanes("f");
     trk->getBK(i)->bookGFDetPlanes("b");
     trk->getBK(i)->bookNumbers("fChi2");
     trk->getBK(i)->bookNumbers("bChi2");
   }
   */
 
   /*why is there a factor of two here (in the for statement)?
     Because we consider one full iteration to be one back and
     one forth fitting pass */
   for(int ipass=0; ipass<2*fNumIt; ipass++){
     //    std::cout << "GFKalman:: numreps, numhits, ipass, direction"<< trk->getNumReps()<<", "<< trk->getNumHits()<<", "<< ipass<<", " <<direction << std::endl;
     //    if(floor(ipass/2)==fNumIt && direction==1) blowUpCovsDiag(trk);
     if(ipass>0) blowUpCovs(trk); // Back to >0, not >=0 EC, 9-11-Feb-2013
     if(direction==1){
       trk->setNextHitToFit(0);
     }
     else {
       trk->setNextHitToFit(trk->getNumHits()-1);
     }
 
 
     try
       {
         fittingPass(trk,direction);
       }
     catch(GFException &)
       {
         throw cet::exception("GFKalman.cxx: ") << " Line " << __LINE__ << ", " << __FILE__ << " ...Rethrow. \n";
       }
         
     //save first and last plane,state&cov after the fitting pass
     if(direction==1){//forward at last hit
       int nreps=trk->getNumReps();
       for(int i=0; i<nreps; ++i){
         trk->getTrackRep(i)->
           setLastPlane( trk->getTrackRep(i)->getReferencePlane() );
         trk->getTrackRep(i)->
           setLastState( trk->getTrackRep(i)->getState() );
         trk->getTrackRep(i)->
           setLastCov( trk->getTrackRep(i)->getCov() );
       }
     }
     else{//backward at first hit
       int nreps=trk->getNumReps();
       for(int i=0; i<nreps; ++i){
         trk->getTrackRep(i)->
           setFirstPlane( trk->getTrackRep(i)->getReferencePlane() );
         trk->getTrackRep(i)->
           setFirstState( trk->getTrackRep(i)->getState() );
         trk->getTrackRep(i)->
           setFirstCov( trk->getTrackRep(i)->getCov() );
       }
     }
 
     //switch direction of fitting and also inside all the reps
     if(direction==1) direction=-1;
     else direction=1;
     switchDirection(trk);
 
   }
   return;
 }

void genf::GFKalman::setBlowUpFactor ( double f )

inline

Set the blowup factor (see blowUpCovs() )

Definition at line 115 of file GFKalman.h.

References f, and fBlowUpFactor.

Referenced by trkf::Track3DKalmanSPS::produce().

115 {fBlowUpFactor=f;}

f

TFile f

Definition: plotHisto.C:6

genf::GFKalman::fBlowUpFactor

double fBlowUpFactor

Definition: GFKalman.h:158

void genf::GFKalman::setErrorScaleMTh ( Double_t f )

inline

Definition at line 120 of file GFKalman.h.

References blowUpCovs(), blowUpCovsDiag(), calcCov7x7(), calcGain(), chi2Increment(), f, fErrScaleMTh, processHit(), and switchDirection().

Referenced by trkf::Track3DKalmanSPS::produce().

120 {fErrScaleMTh=f;}

genf::GFKalman::fErrScaleMTh

Double_t fErrScaleMTh

Definition: GFKalman.h:164

f

TFile f

Definition: plotHisto.C:6

void genf::GFKalman::setErrorScaleSTh ( Double_t f )

inline

Definition at line 119 of file GFKalman.h.

References f, and fErrScaleSTh.

Referenced by trkf::Track3DKalmanSPS::produce().

119 {fErrScaleSTh=f;}

f

TFile f

Definition: plotHisto.C:6

genf::GFKalman::fErrScaleSTh

Double_t fErrScaleSTh

Definition: GFKalman.h:163

void genf::GFKalman::setInitialDirection ( int d )

inline

Sets the inital direction of the track fit (1 for inner to outer, or -1 for outer to inner). The standard is 1 and is set in the ctor.

Definition at line 111 of file GFKalman.h.

References d, and fInitialDirection.

Referenced by trkf::Track3DKalmanSPS::produce().

111 {fInitialDirection=d;}

genf::GFKalman::fInitialDirection

int fInitialDirection

Definition: GFKalman.h:156

d

Float_t d

Definition: plot.C:237

void genf::GFKalman::setLazy ( Int_t )

inline

Switch lazy error handling.

This is a historically left-over method and shall be deleted some time

Definition at line 84 of file GFKalman.h.

84 {std::cerr<<"Using outdates setLazy method of class GFKalman:"<<std::endl;}

void genf::GFKalman::setMaxUpdate ( Double_t f )

inline

Definition at line 118 of file GFKalman.h.

References f, and fMaxUpdate.

Referenced by trkf::Track3DKalmanSPS::produce().

118 {fMaxUpdate=f;}

f

TFile f

Definition: plotHisto.C:6

genf::GFKalman::fMaxUpdate

Double_t fMaxUpdate

Definition: GFKalman.h:162

void genf::GFKalman::setMomHigh ( Double_t f )

inline

Definition at line 117 of file GFKalman.h.

References f, and fMomHigh.

Referenced by trkf::Track3DKalmanSPS::produce().

117 {fMomHigh=f;}

f

TFile f

Definition: plotHisto.C:6

genf::GFKalman::fMomHigh

Double_t fMomHigh

Definition: GFKalman.h:161

void genf::GFKalman::setMomLow ( Double_t f )

inline

Definition at line 116 of file GFKalman.h.

References f, and fMomLow.

Referenced by trkf::Track3DKalmanSPS::produce().

116 {fMomLow=f;}

f

TFile f

Definition: plotHisto.C:6

genf::GFKalman::fMomLow

Double_t fMomLow

Definition: GFKalman.h:160

void genf::GFKalman::setNumIterations ( Int_t i )

inline

Set number of iterations for Kalman Filter.

One iteration is one forward pass plus one backward pass

Definition at line 90 of file GFKalman.h.

References dir, fittingPass(), fNumIt, getChi2Hit(), and processTrack().

Referenced by trkf::Track3DKalman::produce(), and trkf::Track3DKalmanSPS::produce().

90 {fNumIt=i;}

genf::GFKalman::fNumIt

Int_t fNumIt

Definition: GFKalman.h:157

void genf::GFKalman::switchDirection ( GFTrack * trk )

private

Used to switch between forward and backward filtering.

Definition at line 127 of file GFKalman.cxx.

References genf::GFTrack::getNumReps(), genf::GFTrack::getTrackRep(), and genf::GFAbsTrackRep::switchDirection().

Referenced by processTrack(), and setErrorScaleMTh().

                                          {
   int nreps=trk->getNumReps();
   for(int i=0; i<nreps; ++i){
     trk->getTrackRep(i)->switchDirection();
   }
 }