Back to home page

sPhenix code displayed by LXR
 
 

    

File indexing completed on 2025-08-05 08:18:24 

0001 /* Copyright 2013, Ludwig-Maximilians Universität München, Technische Universität München
0002    Authors: Tobias Schlüter & Johannes Rauch
0003 
0004    This file is part of GENFIT.
0005 
0006    GENFIT is free software: you can redistribute it and/or modify
0007    it under the terms of the GNU Lesser General Public License as published
0008    by the Free Software Foundation, either version 3 of the License, or
0009    (at your option) any later version.
0010 
0011    GENFIT is distributed in the hope that it will be useful,
0012    but WITHOUT ANY WARRANTY; without even the implied warranty of
0013    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0014    GNU Lesser General Public License for more details.
0015 
0016    You should have received a copy of the GNU Lesser General Public License
0017    along with GENFIT.  If not, see <http://www.gnu.org/licenses/>.
0018 */
0019 
0020 /* This implements the Kalman fitter with reference track.  */
0021 
0022 #include "Tools.h"
0023 #include "Track.h"
0024 #include "TrackPoint.h"
0025 #include "Exception.h"
0026 #include "IO.h"
0027 
0028 #include "KalmanFitterRefTrack.h"
0029 #include "KalmanFitterInfo.h"
0030 #include "KalmanFitStatus.h"
0031 
0032 #include <TDecompChol.h>
0033 #include <Math/ProbFunc.h>
0034 
0035 
0036 using namespace genfit;
0037 
0038 
0039 TrackPoint* KalmanFitterRefTrack::fitTrack(Track* tr, const AbsTrackRep* rep, double& chi2, double& ndf, int direction)
0040 {
0041 
0042   //if (!isTrackPrepared(tr, rep)) {
0043   //  Exception exc("KalmanFitterRefTrack::fitTrack ==> track is not properly prepared.",__LINE__,__FILE__);
0044   //  throw exc;
0045   //}
0046 
0047   unsigned int dim = rep->getDim();
0048 
0049   chi2 = 0;
0050   ndf = -1. * dim;
0051   KalmanFitterInfo* prevFi(nullptr);
0052 
0053   TrackPoint* retVal(nullptr);
0054 
0055   if (debugLvl_ > 0) {
0056     debugOut << tr->getNumPoints() << " TrackPoints with measurements in this track." << std::endl;
0057   }
0058 
0059   bool alreadyFitted(!refitAll_);
0060 
0061   p_.ResizeTo(dim);
0062   C_.ResizeTo(dim, dim);
0063 
0064   for (size_t i = 0; i < tr->getNumPointsWithMeasurement(); ++i) {
0065       TrackPoint *tp = 0;
0066       assert(direction == +1 || direction == -1);
0067       if (direction == +1)
0068         tp = tr->getPointWithMeasurement(i);
0069       else if (direction == -1)
0070         tp = tr->getPointWithMeasurement(-i-1);
0071 
0072       if (! tp->hasFitterInfo(rep)) {
0073         if (debugLvl_ > 0) {
0074           debugOut << "TrackPoint " << i << " has no fitterInfo -> continue. \n";
0075         }
0076         continue;
0077       }
0078 
0079       KalmanFitterInfo* fi = static_cast<KalmanFitterInfo*>(tp->getFitterInfo(rep));
0080 
0081       if (alreadyFitted && fi->hasUpdate(direction)) {
0082         if (debugLvl_ > 0) {
0083           debugOut << "TrackPoint " << i << " is already fitted -> continue. \n";
0084         }
0085         prevFi = fi;
0086         chi2 += fi->getUpdate(direction)->getChiSquareIncrement();
0087         ndf += fi->getUpdate(direction)->getNdf();
0088         continue;
0089       }
0090 
0091       alreadyFitted = false;
0092 
0093       if (debugLvl_ > 0) {
0094         debugOut << " process TrackPoint nr. " << i << "\n";
0095       }
0096       processTrackPoint(fi, prevFi, tp, chi2, ndf, direction);
0097       retVal = tp;
0098 
0099       prevFi = fi;
0100   }
0101 
0102   return retVal;
0103 }
0104 
0105 
0106 void KalmanFitterRefTrack::processTrackWithRep(Track* tr, const AbsTrackRep* rep, bool resortHits)
0107 {
0108   if (tr->hasFitStatus(rep) && tr->getFitStatus(rep)->isTrackPruned()) {
0109     Exception exc("KalmanFitterRefTrack::processTrack: Cannot process pruned track!", __LINE__,__FILE__);
0110     throw exc;
0111   }
0112 
0113   double oldChi2FW = 1e6;
0114   double oldPvalFW = 0.;
0115   double oldChi2BW = 1e6;
0116   double oldPvalBW = 0.;
0117   double chi2FW(0), ndfFW(0);
0118   double chi2BW(0), ndfBW(0);
0119   int nFailedHits(0);
0120 
0121   KalmanFitStatus* status = new KalmanFitStatus();
0122   tr->setFitStatus(status, rep);
0123 
0124   status->setIsFittedWithReferenceTrack(true);
0125 
0126   unsigned int nIt=0;
0127   for (;;) {
0128 
0129     try {
0130       if (debugLvl_ > 0) {
0131         debugOut << " KalmanFitterRefTrack::processTrack with rep " << rep
0132         << " (id == " << tr->getIdForRep(rep) << ")"<< ", iteration nr. " << nIt << "\n";
0133       }
0134 
0135       // prepare
0136       if (!prepareTrack(tr, rep, resortHits, nFailedHits) && !refitAll_) {
0137         if (debugLvl_ > 0) {
0138           debugOut << "KalmanFitterRefTrack::processTrack. Track preparation did not change anything!\n";
0139         }
0140 
0141         status->setIsFitted();
0142 
0143         status->setIsFitConvergedPartially();
0144         if (nFailedHits == 0)
0145           status->setIsFitConvergedFully();
0146         else
0147           status->setIsFitConvergedFully(false);
0148 
0149         status->setNFailedPoints(nFailedHits);
0150 
0151         status->setHasTrackChanged(false);
0152         status->setCharge(rep->getCharge(*static_cast<KalmanFitterInfo*>(tr->getPointWithMeasurement(0)->getFitterInfo(rep))->getBackwardUpdate()));
0153         status->setNumIterations(nIt);
0154         status->setForwardChi2(chi2FW);
0155         status->setBackwardChi2(chi2BW);
0156         status->setForwardNdf(std::max(0., ndfFW));
0157         status->setBackwardNdf(std::max(0., ndfBW));
0158         if (debugLvl_ > 0) {
0159           status->Print();
0160         }
0161         return;
0162       }
0163 
0164       if (debugLvl_ > 0) {
0165         debugOut << "KalmanFitterRefTrack::processTrack. Prepared Track:";
0166         tr->Print("C");
0167         //tr->Print();
0168       }
0169 
0170       // resort
0171       if (resortHits) {
0172         if (tr->sort()) {
0173           if (debugLvl_ > 0) {
0174             debugOut << "KalmanFitterRefTrack::processTrack. Resorted Track:";
0175             tr->Print("C");
0176           }
0177           prepareTrack(tr, rep, resortHits, nFailedHits);// re-prepare if order of hits has changed!
0178           status->setNFailedPoints(nFailedHits);
0179           if (debugLvl_ > 0) {
0180             debugOut << "KalmanFitterRefTrack::processTrack. Prepared resorted Track:";
0181             tr->Print("C");
0182           }
0183         }
0184       }
0185 
0186 
0187       // fit forward
0188       if (debugLvl_ > 0)
0189         debugOut << "KalmanFitterRefTrack::forward fit\n";
0190       TrackPoint* lastProcessedPoint = fitTrack(tr, rep, chi2FW, ndfFW, +1);
0191 
0192       // fit backward
0193       if (debugLvl_ > 0) {
0194         debugOut << "KalmanFitterRefTrack::backward fit\n";
0195       }
0196 
0197       // backward fit must not necessarily start at last hit, set prediction = forward update and blow up cov
0198       if (lastProcessedPoint != nullptr) {
0199         KalmanFitterInfo* lastInfo = static_cast<KalmanFitterInfo*>(lastProcessedPoint->getFitterInfo(rep));
0200         if (! lastInfo->hasBackwardPrediction()) {
0201           lastInfo->setBackwardPrediction(new MeasuredStateOnPlane(*(lastInfo->getForwardUpdate())));
0202           lastInfo->getBackwardPrediction()->blowUpCov(blowUpFactor_, resetOffDiagonals_, blowUpMaxVal_);  // blow up cov
0203           if (debugLvl_ > 0) {
0204             debugOut << "blow up cov for backward fit at TrackPoint " << lastProcessedPoint << "\n";
0205           }
0206         }
0207       }
0208 
0209       fitTrack(tr, rep, chi2BW, ndfBW, -1);
0210 
0211       ++nIt;
0212 
0213 
0214       double PvalBW = std::max(0.,ROOT::Math::chisquared_cdf_c(chi2BW, ndfBW));
0215       double PvalFW = (debugLvl_ > 0) ? std::max(0.,ROOT::Math::chisquared_cdf_c(chi2FW, ndfFW)) : 0; // Don't calculate if not debugging as this function potentially takes a lot of time.
0216 
0217       if (debugLvl_ > 0) {
0218         debugOut << "KalmanFitterRefTrack::Track after fit:"; tr->Print("C");
0219 
0220         debugOut << "old chi2s: " << oldChi2BW << ", " << oldChi2FW
0221             << " new chi2s: " << chi2BW << ", " << chi2FW << std::endl;
0222         debugOut << "old pVals: " << oldPvalBW << ", " << oldPvalFW
0223             << " new pVals: " << PvalBW << ", " << PvalFW << std::endl;
0224       }
0225 
0226       // See if p-value only changed little.  If the initial
0227       // parameters are very far off, initial chi^2 and the chi^2
0228       // after the first iteration will be both very close to zero, so
0229       // we need to have at least two iterations here.  Convergence
0230       // doesn't make much sense before running twice anyway.
0231       bool converged(false);
0232       bool finished(false);
0233       if (nIt >= minIterations_ && fabs(oldPvalBW - PvalBW) < deltaPval_)  {
0234         // if pVal ~ 0, check if chi2 has changed significantly
0235         if (fabs(1 - fabs(oldChi2BW / chi2BW)) > relChi2Change_) {
0236           finished = false;
0237         }
0238         else {
0239           finished = true;
0240           converged = true;
0241         }
0242 
0243         if (PvalBW == 0.)
0244           converged = false;
0245       }
0246 
0247       if (finished) {
0248         if (debugLvl_ > 0) {
0249           debugOut << "Fit is finished! ";
0250           if(converged)
0251             debugOut << "Fit is converged! ";
0252           debugOut << "\n";
0253         }
0254 
0255         if (nFailedHits == 0)
0256           status->setIsFitConvergedFully(converged);
0257         else
0258           status->setIsFitConvergedFully(false);
0259 
0260         status->setIsFitConvergedPartially(converged);
0261         status->setNFailedPoints(nFailedHits);
0262 
0263         break;
0264       }
0265       else {
0266         oldPvalBW = PvalBW;
0267         oldChi2BW = chi2BW;
0268         if (debugLvl_ > 0) {
0269           oldPvalFW = PvalFW;
0270           oldChi2FW = chi2FW;
0271         }
0272       }
0273 
0274       if (nIt >= maxIterations_) {
0275         if (debugLvl_ > 0) {
0276           debugOut << "KalmanFitterRefTrack::number of max iterations reached!\n";
0277         }
0278         break;
0279       }
0280     }
0281     catch(Exception& e) {
0282       errorOut << e.what();
0283       status->setIsFitted(false);
0284       status->setIsFitConvergedFully(false);
0285       status->setIsFitConvergedPartially(false);
0286       status->setNFailedPoints(nFailedHits);
0287       if (debugLvl_ > 0) {
0288         status->Print();
0289       }
0290       return;
0291     }
0292 
0293   }
0294 
0295 
0296   TrackPoint* tp = tr->getPointWithMeasurementAndFitterInfo(0, rep);
0297 
0298   double charge(0);
0299   if (tp != nullptr) {
0300     if (static_cast<KalmanFitterInfo*>(tp->getFitterInfo(rep))->hasBackwardUpdate())
0301       charge = static_cast<KalmanFitterInfo*>(tp->getFitterInfo(rep))->getBackwardUpdate()->getCharge();
0302   }
0303   status->setCharge(charge);
0304 
0305   if (tp != nullptr) {
0306     status->setIsFitted();
0307   }
0308   else { // none of the trackPoints has a fitterInfo
0309     status->setIsFitted(false);
0310     status->setIsFitConvergedFully(false);
0311     status->setIsFitConvergedPartially(false);
0312     status->setNFailedPoints(nFailedHits);
0313   }
0314 
0315   status->setHasTrackChanged(false);
0316   status->setNumIterations(nIt);
0317   status->setForwardChi2(chi2FW);
0318   status->setBackwardChi2(chi2BW);
0319   status->setForwardNdf(ndfFW);
0320   status->setBackwardNdf(ndfBW);
0321 
0322   if (debugLvl_ > 0) {
0323     status->Print();
0324   }
0325 }
0326 
0327 
0328 bool KalmanFitterRefTrack::prepareTrack(Track* tr, const AbsTrackRep* rep, bool setSortingParams, int& nFailedHits) {
0329 
0330   if (debugLvl_ > 0) {
0331     debugOut << "KalmanFitterRefTrack::prepareTrack \n";
0332   }
0333 
0334   int notChangedUntil, notChangedFrom;
0335 
0336   // remove outdated reference states
0337   bool changedSmthg = removeOutdated(tr, rep,  notChangedUntil, notChangedFrom);
0338 
0339 
0340   // declare matrices
0341   FTransportMatrix_.ResizeTo(rep->getDim(), rep->getDim());
0342   FTransportMatrix_.UnitMatrix();
0343   BTransportMatrix_.ResizeTo(rep->getDim(), rep->getDim());
0344 
0345   FNoiseMatrix_.ResizeTo(rep->getDim(), rep->getDim());
0346   BNoiseMatrix_.ResizeTo(rep->getDim(), rep->getDim());
0347 
0348   forwardDeltaState_.ResizeTo(rep->getDim());
0349   backwardDeltaState_.ResizeTo(rep->getDim());
0350 
0351   // declare stuff
0352   KalmanFitterInfo* prevFitterInfo(nullptr);
0353   std::unique_ptr<MeasuredStateOnPlane> firstBackwardUpdate;
0354 
0355   ReferenceStateOnPlane* referenceState(nullptr);
0356   ReferenceStateOnPlane* prevReferenceState(nullptr);
0357 
0358   const MeasuredStateOnPlane* smoothedState(nullptr);
0359   const MeasuredStateOnPlane* prevSmoothedState(nullptr);
0360 
0361   double trackLen(0);
0362 
0363   bool newRefState(false); // has the current Point a new reference state?
0364   bool prevNewRefState(false); // has the last successfull point a new reference state?
0365 
0366   unsigned int nPoints = tr->getNumPoints();
0367 
0368 
0369   unsigned int i=0;
0370   nFailedHits = 0;
0371 
0372 
0373   // loop over TrackPoints
0374   for (; i<nPoints; ++i) {
0375 
0376     try {
0377 
0378       if (debugLvl_ > 0) {
0379         debugOut << "Prepare TrackPoint " << i << "\n";
0380       }
0381 
0382       TrackPoint* trackPoint = tr->getPoint(i);
0383 
0384       // check if we have a measurement
0385       if (!trackPoint->hasRawMeasurements()) {
0386         if (debugLvl_ > 0) {
0387           debugOut << "TrackPoint has no rawMeasurements -> continue \n";
0388         }
0389         continue;
0390       }
0391 
0392       newRefState = false;
0393 
0394 
0395       // get fitterInfo
0396       KalmanFitterInfo* fitterInfo(nullptr);
0397       if (trackPoint->hasFitterInfo(rep))
0398         fitterInfo = dynamic_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(rep));
0399 
0400       // create new fitter info if none available
0401       if (fitterInfo == nullptr) {
0402         if (debugLvl_ > 0) {
0403           debugOut << "create new KalmanFitterInfo \n";
0404         }
0405         changedSmthg = true;
0406         fitterInfo = new KalmanFitterInfo(trackPoint, rep);
0407         trackPoint->setFitterInfo(fitterInfo);
0408       }
0409       else {
0410         if (debugLvl_ > 0) {
0411           debugOut << "TrackPoint " << i << " (" << trackPoint << ") already has KalmanFitterInfo \n";
0412         }
0413 
0414         if (prevFitterInfo == nullptr) {
0415           if (fitterInfo->hasBackwardUpdate())
0416             firstBackwardUpdate.reset(new MeasuredStateOnPlane(*(fitterInfo->getBackwardUpdate())));
0417         }
0418       }
0419 
0420       // get smoothedState if available
0421       if (fitterInfo->hasPredictionsAndUpdates()) {
0422         smoothedState = &(fitterInfo->getFittedState(true));
0423         if (debugLvl_ > 0) {
0424           debugOut << "got smoothed state \n";
0425           //smoothedState->Print();
0426         }
0427       }
0428       else {
0429         smoothedState = nullptr;
0430       }
0431 
0432 
0433       if (fitterInfo->hasReferenceState()) {
0434 
0435         referenceState = fitterInfo->getReferenceState();
0436 
0437 
0438         if (!prevNewRefState) {
0439           if (debugLvl_ > 0) {
0440             debugOut << "TrackPoint already has referenceState and previous referenceState has not been altered -> continue \n";
0441           }
0442           trackLen += referenceState->getForwardSegmentLength();
0443           if (setSortingParams)
0444             trackPoint->setSortingParameter(trackLen);
0445 
0446           prevNewRefState = newRefState;
0447           prevReferenceState = referenceState;
0448           prevFitterInfo = fitterInfo;
0449           prevSmoothedState = smoothedState;
0450 
0451           continue;
0452         }
0453 
0454 
0455         if (prevReferenceState == nullptr) {
0456           if (debugLvl_ > 0) {
0457             debugOut << "TrackPoint already has referenceState but previous referenceState is nullptr -> reset forward info of current reference state and continue \n";
0458           }
0459 
0460           referenceState->resetForward();
0461 
0462           if (setSortingParams)
0463             trackPoint->setSortingParameter(trackLen);
0464 
0465           prevNewRefState = newRefState;
0466           prevReferenceState = referenceState;
0467           prevFitterInfo = fitterInfo;
0468           prevSmoothedState = smoothedState;
0469 
0470           continue;
0471         }
0472 
0473         // previous refState has been altered ->need to update transport matrices
0474         if (debugLvl_ > 0) {
0475           debugOut << "TrackPoint already has referenceState but previous referenceState has been altered -> update transport matrices and continue \n";
0476         }
0477         StateOnPlane stateToExtrapolate(*prevReferenceState);
0478 
0479         // make sure track is consistent if extrapolation fails
0480         prevReferenceState->resetBackward();
0481         referenceState->resetForward();
0482 
0483         double segmentLen = rep->extrapolateToPlane(stateToExtrapolate, fitterInfo->getReferenceState()->getPlane(), false, true);
0484         if (debugLvl_ > 0) {
0485           debugOut << "extrapolated stateToExtrapolate (prevReferenceState) by " << segmentLen << " cm.\n";
0486         }
0487         trackLen += segmentLen;
0488 
0489         if (segmentLen == 0) {
0490           FTransportMatrix_.UnitMatrix();
0491           FNoiseMatrix_.Zero();
0492           forwardDeltaState_.Zero();
0493           BTransportMatrix_.UnitMatrix();
0494           BNoiseMatrix_.Zero();
0495           backwardDeltaState_.Zero();
0496         }
0497         else {
0498           rep->getForwardJacobianAndNoise(FTransportMatrix_, FNoiseMatrix_, forwardDeltaState_);
0499           rep->getBackwardJacobianAndNoise(BTransportMatrix_, BNoiseMatrix_, backwardDeltaState_);
0500         }
0501 
0502         prevReferenceState->setBackwardSegmentLength(-segmentLen);
0503         prevReferenceState->setBackwardTransportMatrix(BTransportMatrix_);
0504         prevReferenceState->setBackwardNoiseMatrix(BNoiseMatrix_);
0505         prevReferenceState->setBackwardDeltaState(backwardDeltaState_);
0506 
0507         referenceState->setForwardSegmentLength(segmentLen);
0508         referenceState->setForwardTransportMatrix(FTransportMatrix_);
0509         referenceState->setForwardNoiseMatrix(FNoiseMatrix_);
0510         referenceState->setForwardDeltaState(forwardDeltaState_);
0511 
0512         newRefState = true;
0513 
0514         if (setSortingParams)
0515           trackPoint->setSortingParameter(trackLen);
0516 
0517         prevNewRefState = newRefState;
0518         prevReferenceState = referenceState;
0519         prevFitterInfo = fitterInfo;
0520         prevSmoothedState = smoothedState;
0521 
0522         continue;
0523       }
0524 
0525 
0526       // Construct plane
0527       SharedPlanePtr plane;
0528       if (smoothedState != nullptr) {
0529         if (debugLvl_ > 0)
0530           debugOut << "construct plane with smoothedState \n";
0531         plane = trackPoint->getRawMeasurement(0)->constructPlane(*smoothedState);
0532       }
0533       else if (prevSmoothedState != nullptr) {
0534         if (debugLvl_ > 0) {
0535           debugOut << "construct plane with prevSmoothedState \n";
0536           //prevSmoothedState->Print();
0537         }
0538         plane = trackPoint->getRawMeasurement(0)->constructPlane(*prevSmoothedState);
0539       }
0540       else if (prevReferenceState != nullptr) {
0541         if (debugLvl_ > 0) {
0542           debugOut << "construct plane with prevReferenceState \n";
0543         }
0544         plane = trackPoint->getRawMeasurement(0)->constructPlane(*prevReferenceState);
0545       }
0546       else if (rep != tr->getCardinalRep() &&
0547                 trackPoint->hasFitterInfo(tr->getCardinalRep()) &&
0548                 dynamic_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep())) != nullptr &&
0549                 static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->hasPredictionsAndUpdates() ) {
0550         if (debugLvl_ > 0) {
0551           debugOut << "construct plane with smoothed state of cardinal rep fit \n";
0552         }
0553         TVector3 pos, mom;
0554         const MeasuredStateOnPlane& fittedState = static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->getFittedState(true);
0555         tr->getCardinalRep()->getPosMom(fittedState, pos, mom);
0556         StateOnPlane cardinalState(rep);
0557         rep->setPosMom(cardinalState, pos, mom);
0558         rep->setQop(cardinalState, tr->getCardinalRep()->getQop(fittedState));
0559         plane = trackPoint->getRawMeasurement(0)->constructPlane(cardinalState);
0560       }
0561       else {
0562         if (debugLvl_ > 0) {
0563           debugOut << "construct plane with state from track \n";
0564         }
0565         StateOnPlane seedFromTrack(rep);
0566         rep->setPosMom(seedFromTrack, tr->getStateSeed()); // also fills auxInfo
0567         plane = trackPoint->getRawMeasurement(0)->constructPlane(seedFromTrack);
0568       }
0569 
0570       if (plane.get() == nullptr) {
0571         Exception exc("KalmanFitterRefTrack::prepareTrack ==> construced plane is nullptr!",__LINE__,__FILE__);
0572         exc.setFatal();
0573         throw exc;
0574       }
0575 
0576 
0577 
0578       // do extrapolation and set reference state infos
0579       std::unique_ptr<StateOnPlane> stateToExtrapolate(nullptr);
0580       if (prevFitterInfo == nullptr) { // first measurement
0581         if (debugLvl_ > 0) {
0582           debugOut << "prevFitterInfo == nullptr \n";
0583         }
0584         if (smoothedState != nullptr) {
0585           if (debugLvl_ > 0) {
0586             debugOut << "extrapolate smoothedState to plane\n";
0587           }
0588           stateToExtrapolate.reset(new StateOnPlane(*smoothedState));
0589         }
0590         else if (referenceState != nullptr) {
0591           if (debugLvl_ > 0) {
0592             debugOut << "extrapolate referenceState to plane\n";
0593           }
0594           stateToExtrapolate.reset(new StateOnPlane(*referenceState));
0595         }
0596         else if (rep != tr->getCardinalRep() &&
0597                   trackPoint->hasFitterInfo(tr->getCardinalRep()) &&
0598                   dynamic_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep())) != nullptr &&
0599                   static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->hasPredictionsAndUpdates() ) {
0600           if (debugLvl_ > 0) {
0601             debugOut << "extrapolate smoothed state of cardinal rep fit to plane\n";
0602           }
0603           TVector3 pos, mom;
0604           const MeasuredStateOnPlane& fittedState = static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->getFittedState(true);
0605           stateToExtrapolate.reset(new StateOnPlane(fittedState));
0606           stateToExtrapolate->setRep(rep);
0607         }
0608         else {
0609           if (debugLvl_ > 0) {
0610             debugOut << "extrapolate seed from track to plane\n";
0611           }
0612           stateToExtrapolate.reset(new StateOnPlane(rep));
0613           rep->setPosMom(*stateToExtrapolate, tr->getStateSeed());
0614           rep->setTime(*stateToExtrapolate, tr->getTimeSeed());
0615         }
0616       } // end if (prevFitterInfo == nullptr)
0617       else {
0618         if (prevSmoothedState != nullptr) {
0619           if (debugLvl_ > 0) {
0620             debugOut << "extrapolate prevSmoothedState to plane \n";
0621           }
0622           stateToExtrapolate.reset(new StateOnPlane(*prevSmoothedState));
0623         }
0624         else {
0625           assert (prevReferenceState != nullptr);
0626           if (debugLvl_ > 0) {
0627             debugOut << "extrapolate prevReferenceState to plane \n";
0628           }
0629           stateToExtrapolate.reset(new StateOnPlane(*prevReferenceState));
0630         }
0631       }
0632 
0633       // make sure track is consistent if extrapolation fails
0634       if (prevReferenceState != nullptr)
0635         prevReferenceState->resetBackward();
0636       fitterInfo->deleteReferenceInfo();
0637 
0638       if (prevFitterInfo != nullptr) {
0639         rep->extrapolateToPlane(*stateToExtrapolate, prevFitterInfo->getPlane());
0640         if (debugLvl_ > 0) {
0641           debugOut << "extrapolated stateToExtrapolate to plane of prevFitterInfo (plane could have changed!) \n";
0642         }
0643       }
0644 
0645       double segmentLen = rep->extrapolateToPlane(*stateToExtrapolate, plane, false, true);
0646       trackLen += segmentLen;
0647       if (debugLvl_ > 0) {
0648         debugOut << "extrapolated stateToExtrapolate by " << segmentLen << " cm.\t";
0649         debugOut << "charge of stateToExtrapolate: " << rep->getCharge(*stateToExtrapolate) << " \n";
0650       }
0651 
0652       // get jacobians and noise matrices
0653       if (segmentLen == 0) {
0654         FTransportMatrix_.UnitMatrix();
0655         FNoiseMatrix_.Zero();
0656         forwardDeltaState_.Zero();
0657         BTransportMatrix_.UnitMatrix();
0658         BNoiseMatrix_.Zero();
0659         backwardDeltaState_.Zero();
0660       }
0661       else {
0662         if (i>0)
0663           rep->getForwardJacobianAndNoise(FTransportMatrix_, FNoiseMatrix_, forwardDeltaState_);
0664         rep->getBackwardJacobianAndNoise(BTransportMatrix_, BNoiseMatrix_, backwardDeltaState_);
0665       }
0666 
0667 
0668       if (i==0) {
0669         // if we are at first measurement and seed state is defined somewhere else
0670         segmentLen = 0;
0671         trackLen = 0;
0672       }
0673       if (setSortingParams)
0674         trackPoint->setSortingParameter(trackLen);
0675 
0676 
0677       // set backward matrices for previous reference state
0678       if (prevReferenceState != nullptr) {
0679         prevReferenceState->setBackwardSegmentLength(-segmentLen);
0680         prevReferenceState->setBackwardTransportMatrix(BTransportMatrix_);
0681         prevReferenceState->setBackwardNoiseMatrix(BNoiseMatrix_);
0682         prevReferenceState->setBackwardDeltaState(backwardDeltaState_);
0683       }
0684 
0685 
0686       // create new reference state
0687       newRefState = true;
0688       changedSmthg = true;
0689       referenceState = new ReferenceStateOnPlane(stateToExtrapolate->getState(),
0690              stateToExtrapolate->getPlane(),
0691              stateToExtrapolate->getRep(),
0692              stateToExtrapolate->getAuxInfo());
0693       referenceState->setForwardSegmentLength(segmentLen);
0694       referenceState->setForwardTransportMatrix(FTransportMatrix_);
0695       referenceState->setForwardNoiseMatrix(FNoiseMatrix_);
0696       referenceState->setForwardDeltaState(forwardDeltaState_);
0697 
0698       referenceState->resetBackward();
0699 
0700       fitterInfo->setReferenceState(referenceState);
0701 
0702 
0703       // get MeasurementsOnPlane
0704       std::vector<double> oldWeights = fitterInfo->getWeights();
0705       bool oldWeightsFixed = fitterInfo->areWeightsFixed();
0706       fitterInfo->deleteMeasurementInfo();
0707       const std::vector<AbsMeasurement*>& rawMeasurements = trackPoint->getRawMeasurements();
0708       for ( std::vector< genfit::AbsMeasurement* >::const_iterator measurement = rawMeasurements.begin(), lastMeasurement = rawMeasurements.end(); measurement != lastMeasurement; ++measurement) {
0709         assert((*measurement) != nullptr);
0710         fitterInfo->addMeasurementsOnPlane((*measurement)->constructMeasurementsOnPlane(*referenceState));
0711       }
0712       if (oldWeights.size() == fitterInfo->getNumMeasurements()) {
0713         fitterInfo->setWeights(oldWeights);
0714         fitterInfo->fixWeights(oldWeightsFixed);
0715       }
0716 
0717 
0718       // if we made it here, no Exceptions were thrown and the TrackPoint could successfully be processed
0719       prevNewRefState = newRefState;
0720       prevReferenceState = referenceState;
0721       prevFitterInfo = fitterInfo;
0722       prevSmoothedState = smoothedState;
0723 
0724     }
0725     catch (Exception& e) {
0726 
0727       if (debugLvl_ > 0) {
0728         errorOut << "exception at hit " << i << "\n";
0729         debugOut << e.what();
0730       }
0731 
0732 
0733       ++nFailedHits;
0734       if (maxFailedHits_<0 || nFailedHits <= maxFailedHits_) {
0735         prevNewRefState = true;
0736         referenceState = nullptr;
0737         smoothedState = nullptr;
0738         tr->getPoint(i)->deleteFitterInfo(rep);
0739 
0740         if (setSortingParams)
0741           tr->getPoint(i)->setSortingParameter(trackLen);
0742 
0743         if (debugLvl_ > 0) {
0744           debugOut << "There was an exception, try to continue with next TrackPoint " << i+1 << " \n";
0745         }
0746 
0747         continue;
0748       }
0749 
0750 
0751       // clean up
0752       removeForwardBackwardInfo(tr, rep, notChangedUntil, notChangedFrom);
0753 
0754       // set sorting parameters of rest of TrackPoints and remove FitterInfos
0755       for (; i<nPoints; ++i) {
0756         TrackPoint* trackPoint = tr->getPoint(i);
0757 
0758         if (setSortingParams)
0759           trackPoint->setSortingParameter(trackLen);
0760 
0761         trackPoint->deleteFitterInfo(rep);
0762       }
0763       return true;
0764 
0765     }
0766 
0767   } // end loop over TrackPoints
0768 
0769 
0770 
0771 
0772   removeForwardBackwardInfo(tr, rep, notChangedUntil, notChangedFrom);
0773 
0774   if (firstBackwardUpdate && tr->getPointWithMeasurementAndFitterInfo(0, rep)) {
0775     KalmanFitterInfo* fi = static_cast<KalmanFitterInfo*>(tr->getPointWithMeasurementAndFitterInfo(0, rep)->getFitterInfo(rep));
0776     if (fi && ! fi->hasForwardPrediction()) {
0777       if (debugLvl_ > 0) {
0778         debugOut << "set backwards update of first point as forward prediction (with blown up cov) \n";
0779       }
0780       if (fi->getPlane() != firstBackwardUpdate->getPlane()) {
0781         rep->extrapolateToPlane(*firstBackwardUpdate, fi->getPlane());
0782       }
0783       firstBackwardUpdate->blowUpCov(blowUpFactor_, resetOffDiagonals_, blowUpMaxVal_);
0784       fi->setForwardPrediction(new MeasuredStateOnPlane(*firstBackwardUpdate));
0785     }
0786   }
0787 
0788   KalmanFitStatus* fitStatus = dynamic_cast<KalmanFitStatus*>(tr->getFitStatus(rep));
0789   if (fitStatus != nullptr)
0790     fitStatus->setTrackLen(trackLen);
0791 
0792   if (debugLvl_ > 0) {
0793     debugOut << "trackLen of reference track = " << trackLen << "\n";
0794   }
0795 
0796   // self check
0797   //assert(tr->checkConsistency());
0798   assert(isTrackPrepared(tr, rep));
0799 
0800   return changedSmthg;
0801 }
0802 
0803 
0804 bool
0805 KalmanFitterRefTrack::removeOutdated(Track* tr, const AbsTrackRep* rep, int& notChangedUntil, int& notChangedFrom) {
0806 
0807   if (debugLvl_ > 0) {
0808     debugOut << "KalmanFitterRefTrack::removeOutdated \n";
0809   }
0810 
0811   bool changedSmthg(false);
0812 
0813   unsigned int nPoints = tr->getNumPoints();
0814   notChangedUntil = nPoints-1;
0815   notChangedFrom = 0;
0816 
0817   // loop over TrackPoints
0818   for (unsigned int i=0; i<nPoints; ++i) {
0819 
0820     TrackPoint* trackPoint = tr->getPoint(i);
0821 
0822     // check if we have a measurement
0823     if (!trackPoint->hasRawMeasurements()) {
0824       if (debugLvl_ > 0) {
0825         debugOut << "TrackPoint has no rawMeasurements -> continue \n";
0826       }
0827       continue;
0828     }
0829 
0830     // get fitterInfo
0831     KalmanFitterInfo* fitterInfo(nullptr);
0832     if (trackPoint->hasFitterInfo(rep))
0833       fitterInfo = dynamic_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(rep));
0834 
0835     if (fitterInfo == nullptr)
0836       continue;
0837 
0838 
0839     // check if we need to calculate or update reference state
0840     if (fitterInfo->hasReferenceState()) {
0841 
0842       if (! fitterInfo->hasPredictionsAndUpdates()) {
0843         if (debugLvl_ > 0) {
0844           debugOut << "reference state but not all predictions & updates -> do not touch reference state. \n";
0845         }
0846         continue;
0847       }
0848 
0849 
0850       const MeasuredStateOnPlane& smoothedState = fitterInfo->getFittedState(true);
0851       resM_.ResizeTo(smoothedState.getState());
0852       resM_ = smoothedState.getState();
0853       resM_ -= fitterInfo->getReferenceState()->getState();
0854       double chi2(0);
0855 
0856       // calculate chi2, ignore off diagonals
0857       double* resArray = resM_.GetMatrixArray();
0858       for (int j=0; j<smoothedState.getCov().GetNcols(); ++j)
0859         chi2 += resArray[j]*resArray[j] / smoothedState.getCov()(j,j);
0860 
0861       if (chi2 < deltaChi2Ref_) {
0862         // reference state is near smoothed state ->  do not update reference state
0863         if (debugLvl_ > 0) {
0864           debugOut << "reference state is near smoothed state ->  do not update reference state, chi2 = " << chi2 << "\n";
0865         }
0866         continue;
0867       } else {
0868         if (debugLvl_ > 0) {
0869           debugOut << "reference state is not close to smoothed state, chi2 = " << chi2 << "\n";
0870         }
0871       }
0872     }
0873 
0874     if (debugLvl_ > 0) {
0875       debugOut << "remove reference info \n";
0876     }
0877 
0878     fitterInfo->deleteReferenceInfo();
0879     changedSmthg = true;
0880 
0881     // decided to update reference state -> set notChangedUntil (only once)
0882     if (notChangedUntil == (int)nPoints-1)
0883       notChangedUntil = i-1;
0884 
0885     notChangedFrom = i+1;
0886 
0887   } // end loop over TrackPoints
0888 
0889 
0890   if (debugLvl_ > 0) {
0891     tr->Print("C");
0892   }
0893 
0894   return changedSmthg;
0895 }
0896 
0897 
0898 void
0899 KalmanFitterRefTrack::removeForwardBackwardInfo(Track* tr, const AbsTrackRep* rep, int notChangedUntil, int notChangedFrom) const {
0900 
0901   unsigned int nPoints = tr->getNumPoints();
0902 
0903   if (refitAll_) {
0904     tr->deleteForwardInfo(0, -1, rep);
0905     tr->deleteBackwardInfo(0, -1, rep);
0906     return;
0907   }
0908 
0909   // delete forward/backward info from/to points where reference states have changed
0910   if (notChangedUntil != (int)nPoints-1) {
0911     tr->deleteForwardInfo(notChangedUntil+1, -1, rep);
0912   }
0913   if (notChangedFrom != 0)
0914     tr->deleteBackwardInfo(0, notChangedFrom-1, rep);
0915 
0916 }
0917 
0918 
0919 void
0920 KalmanFitterRefTrack::processTrackPoint(KalmanFitterInfo* fi, const KalmanFitterInfo* prevFi, const TrackPoint* tp, double& chi2, double& ndf, int direction)
0921 {
0922   if(squareRootFormalism_) {
0923     processTrackPointSqrt(fi, prevFi, tp, chi2, ndf, direction);
0924     return;
0925   }
0926 
0927   if (debugLvl_ > 0) {
0928     debugOut << " KalmanFitterRefTrack::processTrackPoint " << fi->getTrackPoint() << "\n";
0929   }
0930 
0931   unsigned int dim = fi->getRep()->getDim();
0932 
0933   p_.Zero(); // p_{k|k-1}
0934   C_.Zero(); // C_{k|k-1}
0935 
0936   // predict
0937   if (prevFi != nullptr) {
0938     const TMatrixD& F = fi->getReferenceState()->getTransportMatrix(direction); // Transport matrix
0939     assert(F.GetNcols() == (int)dim);
0940     const TMatrixDSym& N = fi->getReferenceState()->getNoiseMatrix(direction); // Noise matrix
0941     //p_ = ( F * prevFi->getUpdate(direction)->getState() ) + fi->getReferenceState()->getDeltaState(direction);
0942     p_ = prevFi->getUpdate(direction)->getState();
0943     p_ *= F;
0944     p_ += fi->getReferenceState()->getDeltaState(direction);
0945 
0946     C_ = prevFi->getUpdate(direction)->getCov();
0947     C_.Similarity(F);
0948     C_ += N;
0949     fi->setPrediction(new MeasuredStateOnPlane(p_, C_, fi->getReferenceState()->getPlane(), fi->getReferenceState()->getRep(), fi->getReferenceState()->getAuxInfo()), direction);
0950     if (debugLvl_ > 1) {
0951       debugOut << "\033[31m";
0952       debugOut << "F (Transport Matrix) "; F.Print();
0953       debugOut << "p_{k,r} (reference state) "; fi->getReferenceState()->getState().Print();
0954       debugOut << "c (delta state) "; fi->getReferenceState()->getDeltaState(direction).Print();
0955       debugOut << "F*p_{k-1,r} + c "; (F *prevFi->getReferenceState()->getState() + fi->getReferenceState()->getDeltaState(direction)).Print();
0956     }
0957   }
0958   else {
0959     if (fi->hasPrediction(direction)) {
0960       if (debugLvl_ > 0) {
0961         debugOut << "  Use prediction as start \n";
0962       }
0963       p_ = fi->getPrediction(direction)->getState();
0964       C_ = fi->getPrediction(direction)->getCov();
0965     }
0966     else {
0967       if (debugLvl_ > 0) {
0968         debugOut << "  Use reference state and seed cov as start \n";
0969       }
0970       const AbsTrackRep *rep = fi->getReferenceState()->getRep();
0971       p_ = fi->getReferenceState()->getState();
0972 
0973       // Convert from 6D covariance of the seed to whatever the trackRep wants.
0974       TMatrixDSym dummy(p_.GetNrows());
0975       MeasuredStateOnPlane mop(p_, dummy, fi->getReferenceState()->getPlane(), rep, fi->getReferenceState()->getAuxInfo());
0976       TVector3 pos, mom;
0977       rep->getPosMom(mop, pos, mom);
0978       rep->setPosMomCov(mop, pos, mom, fi->getTrackPoint()->getTrack()->getCovSeed());
0979       // Blow up, set.
0980       mop.blowUpCov(blowUpFactor_, resetOffDiagonals_, blowUpMaxVal_);
0981       fi->setPrediction(new MeasuredStateOnPlane(mop), direction);
0982       C_ = mop.getCov();
0983     }
0984     if (debugLvl_ > 1) {
0985       debugOut << "\033[31m";
0986       debugOut << "p_{k,r} (reference state)"; fi->getReferenceState()->getState().Print();
0987     }
0988   }
0989 
0990   if (debugLvl_ > 1) {
0991     debugOut << " p_{k|k-1} (state prediction)"; p_.Print();
0992     debugOut << " C_{k|k-1} (covariance prediction)"; C_.Print();
0993     debugOut << "\033[0m";
0994   }
0995 
0996   // update(s)
0997   double chi2inc = 0;
0998   double ndfInc = 0;
0999   const std::vector<MeasurementOnPlane *> measurements = getMeasurements(fi, tp, direction);
1000   for (std::vector<MeasurementOnPlane *>::const_iterator it = measurements.begin(); it != measurements.end(); ++it) {
1001     const MeasurementOnPlane& m = **it;
1002 
1003     if (!canIgnoreWeights() && m.getWeight() <= 1.01E-10) {
1004       if (debugLvl_ > 1) {
1005         debugOut << "Weight of measurement is almost 0, continue ... /n";
1006       }
1007       continue;
1008     }
1009 
1010     const AbsHMatrix* H(m.getHMatrix());
1011     // (weighted) cov
1012     const TMatrixDSym& V((!canIgnoreWeights() && m.getWeight() < 0.99999) ?
1013                           1./m.getWeight() * m.getCov() :
1014                           m.getCov());
1015 
1016     covSumInv_.ResizeTo(C_);
1017     covSumInv_ = C_; // (V_k + H_k C_{k|k-1} H_k^T)^(-1)
1018     H->HMHt(covSumInv_);
1019     covSumInv_ += V;
1020 
1021     tools::invertMatrix(covSumInv_);
1022 
1023     const TMatrixD& CHt(H->MHt(C_));
1024 
1025     res_.ResizeTo(m.getState());
1026     res_ = m.getState();
1027     res_ -= H->Hv(p_); // residual
1028     if (debugLvl_ > 1) {
1029       debugOut << "\033[34m";
1030       debugOut << "m (measurement) "; m.getState().Print();
1031       debugOut << "V ((weighted) measurement covariance) "; (1./m.getWeight() * m.getCov()).Print();
1032       debugOut << "residual        "; res_.Print();
1033       debugOut << "\033[0m";
1034     }
1035     p_ +=  TMatrixD(CHt, TMatrixD::kMult, covSumInv_) * res_; // updated state
1036     if (debugLvl_ > 1) {
1037       debugOut << "\033[32m";
1038       debugOut << " update"; (TMatrixD(CHt, TMatrixD::kMult, covSumInv_) * res_).Print();
1039       debugOut << "\033[0m";
1040     }
1041 
1042     covSumInv_.Similarity(CHt); // with (C H^T)^T = H C^T = H C  (C is symmetric)
1043     C_ -= covSumInv_; // updated Cov
1044 
1045     if (debugLvl_ > 1) {
1046       //debugOut << " C update "; covSumInv_.Print();
1047       debugOut << "\033[32m";
1048       debugOut << " p_{k|k} (updated state)"; p_.Print();
1049       debugOut << " C_{k|k} (updated covariance)"; C_.Print();
1050       debugOut << "\033[0m";
1051     }
1052 
1053     // Calculate chi² increment.  At the first point chi2inc == 0 and
1054     // the matrix will not be invertible.
1055     res_ = m.getState();
1056     res_ -= H->Hv(p_); // new residual
1057     if (debugLvl_ > 1) {
1058       debugOut << " resNew ";
1059       res_.Print();
1060     }
1061 
1062     // only calculate chi2inc if res != nullptr.
1063     // If matrix inversion fails, chi2inc = 0
1064     if (res_ != 0) {
1065       Rinv_.ResizeTo(C_);
1066       Rinv_ = C_;
1067       H->HMHt(Rinv_);
1068       Rinv_ -= V;
1069       Rinv_ *= -1;
1070 
1071       bool couldInvert(true);
1072       try {
1073         tools::invertMatrix(Rinv_);
1074       }
1075       catch (Exception& e) {
1076         if (debugLvl_ > 1) {
1077           debugOut << e.what();
1078         }
1079         couldInvert = false;
1080       }
1081 
1082       if (couldInvert) {
1083         if (debugLvl_ > 1) {
1084           debugOut << " Rinv ";
1085           Rinv_.Print();
1086         }
1087         chi2inc += Rinv_.Similarity(res_);
1088       }
1089     }
1090 
1091 
1092     if (!canIgnoreWeights()) {
1093       ndfInc += m.getWeight() * m.getState().GetNrows();
1094     }
1095     else
1096       ndfInc += m.getState().GetNrows();
1097 
1098 
1099   } // end loop over measurements
1100 
1101   chi2 += chi2inc;
1102   ndf += ndfInc;
1103 
1104 
1105   KalmanFittedStateOnPlane* upState = new KalmanFittedStateOnPlane(p_, C_, fi->getReferenceState()->getPlane(), fi->getReferenceState()->getRep(), fi->getReferenceState()->getAuxInfo(), chi2inc, ndfInc);
1106   upState->setAuxInfo(fi->getReferenceState()->getAuxInfo());
1107   fi->setUpdate(upState, direction);
1108 
1109 
1110   if (debugLvl_ > 0) {
1111     debugOut << " chi² inc " << chi2inc << "\t";
1112     debugOut << " ndf inc  " << ndfInc << "\t";
1113     debugOut << " charge of update  " << fi->getRep()->getCharge(*upState) << "\n";
1114   }
1115 
1116   // check
1117   if (not fi->checkConsistency()) {
1118     throw genfit::Exception("Consistency check failed ", __LINE__, __FILE__);
1119   }
1120 
1121 }
1122 
1123 
1124 
1125 
1126 void
1127 KalmanFitterRefTrack::processTrackPointSqrt(KalmanFitterInfo* fi, const KalmanFitterInfo* prevFi,
1128                         const TrackPoint* tp, double& chi2, double& ndf, int direction)
1129 {
1130   if (debugLvl_ > 0) {
1131     debugOut << " KalmanFitterRefTrack::processTrackPointSqrt " << fi->getTrackPoint() << "\n";
1132   }
1133 
1134   unsigned int dim = fi->getRep()->getDim();
1135 
1136   p_.Zero(); // p_{k|k-1}
1137   C_.Zero(); // C_{k|k-1}
1138 
1139   TMatrixD S(dim, dim); // sqrt(C_);
1140 
1141   // predict
1142   if (prevFi != nullptr) {
1143     const TMatrixD& F = fi->getReferenceState()->getTransportMatrix(direction); // Transport matrix
1144     assert(F.GetNcols() == (int)dim);
1145     const TMatrixDSym& N = fi->getReferenceState()->getNoiseMatrix(direction); // Noise matrix
1146     //N = 0;
1147 
1148     //p_ = ( F * prevFi->getUpdate(direction)->getState() ) + fi->getReferenceState()->getDeltaState(direction);
1149     p_ = prevFi->getUpdate(direction)->getState();
1150     p_ *= F;
1151     p_ += fi->getReferenceState()->getDeltaState(direction);
1152 
1153 
1154     TDecompChol decompS(prevFi->getUpdate(direction)->getCov());
1155     decompS.Decompose();
1156     TMatrixD Q;
1157     tools::noiseMatrixSqrt(N, Q);
1158     tools::kalmanPredictionCovSqrt(decompS.GetU(), F, Q, S);
1159 
1160     fi->setPrediction(new MeasuredStateOnPlane(p_, TMatrixDSym(TMatrixDSym::kAtA, S), fi->getReferenceState()->getPlane(), fi->getReferenceState()->getRep(), fi->getReferenceState()->getAuxInfo()), direction);
1161     if (debugLvl_ > 1) {
1162       debugOut << "\033[31m";
1163       debugOut << "F (Transport Matrix) "; F.Print();
1164       debugOut << "p_{k,r} (reference state) "; fi->getReferenceState()->getState().Print();
1165       debugOut << "c (delta state) "; fi->getReferenceState()->getDeltaState(direction).Print();
1166       debugOut << "F*p_{k-1,r} + c "; (F *prevFi->getReferenceState()->getState() + fi->getReferenceState()->getDeltaState(direction)).Print();
1167     }
1168   }
1169   else {
1170     if (fi->hasPrediction(direction)) {
1171       if (debugLvl_ > 0) {
1172         debugOut << "  Use prediction as start \n";
1173       }
1174       p_ = fi->getPrediction(direction)->getState();
1175       TDecompChol decompS(fi->getPrediction(direction)->getCov());
1176       decompS.Decompose();
1177       S = decompS.GetU();
1178     }
1179     else {
1180       if (debugLvl_ > 0) {
1181         debugOut << "  Use reference state and seed cov as start \n";
1182       }
1183       const AbsTrackRep *rep = fi->getReferenceState()->getRep();
1184       p_ = fi->getReferenceState()->getState();
1185 
1186       // Convert from 6D covariance of the seed to whatever the trackRep wants.
1187       TMatrixDSym dummy(p_.GetNrows());
1188       MeasuredStateOnPlane mop(p_, dummy, fi->getReferenceState()->getPlane(), rep, fi->getReferenceState()->getAuxInfo());
1189       TVector3 pos, mom;
1190       rep->getPosMom(mop, pos, mom);
1191       rep->setPosMomCov(mop, pos, mom, fi->getTrackPoint()->getTrack()->getCovSeed());
1192       // Blow up, set.
1193       mop.blowUpCov(blowUpFactor_, resetOffDiagonals_, blowUpMaxVal_);
1194       fi->setPrediction(new MeasuredStateOnPlane(mop), direction);
1195       TDecompChol decompS(mop.getCov());
1196       decompS.Decompose();
1197       S = decompS.GetU();
1198     }
1199     if (debugLvl_ > 1) {
1200       debugOut << "\033[31m";
1201       debugOut << "p_{k,r} (reference state)"; fi->getReferenceState()->getState().Print();
1202     }
1203   }
1204 
1205   if (debugLvl_ > 1) {
1206     debugOut << " p_{k|k-1} (state prediction)"; p_.Print();
1207     debugOut << " C_{k|k-1} (covariance prediction)"; C_.Print();
1208     debugOut << "\033[0m";
1209   }
1210 
1211   // update(s)
1212   double chi2inc = 0;
1213   double ndfInc = 0;
1214 
1215   const std::vector<MeasurementOnPlane *> measurements = getMeasurements(fi, tp, direction);
1216   for (std::vector<MeasurementOnPlane *>::const_iterator it = measurements.begin(); it != measurements.end(); ++it) {
1217     const MeasurementOnPlane& m = **it;
1218 
1219     if (!canIgnoreWeights() && m.getWeight() <= 1.01E-10) {
1220       if (debugLvl_ > 1) {
1221         debugOut << "Weight of measurement is almost 0, continue ... /n";
1222       }
1223       continue;
1224     }
1225 
1226     const AbsHMatrix* H(m.getHMatrix());
1227     // (weighted) cov
1228     const TMatrixDSym& V((!canIgnoreWeights() && m.getWeight() < 0.99999) ?
1229                           1./m.getWeight() * m.getCov() :
1230                           m.getCov());
1231     TDecompChol decompR(V);
1232     decompR.Decompose();
1233     const TMatrixD& R(decompR.GetU());
1234 
1235     res_.ResizeTo(m.getState());
1236     res_ = m.getState();
1237     res_ -= H->Hv(p_); // residual
1238 
1239     TVectorD update;
1240     tools::kalmanUpdateSqrt(S, res_, R, H,
1241                 update, S);
1242 
1243     if (debugLvl_ > 1) {
1244       debugOut << "\033[34m";
1245       debugOut << "m (measurement) "; m.getState().Print();
1246       debugOut << "V ((weighted) measurement covariance) "; (1./m.getWeight() * m.getCov()).Print();
1247       debugOut << "residual        "; res_.Print();
1248       debugOut << "\033[0m";
1249     }
1250 
1251     p_ += update;
1252     if (debugLvl_ > 1) {
1253       debugOut << "\033[32m";
1254       debugOut << " update"; update.Print();
1255       debugOut << "\033[0m";
1256     }
1257 
1258     if (debugLvl_ > 1) {
1259       //debugOut << " C update "; covSumInv_.Print();
1260       debugOut << "\033[32m";
1261       debugOut << " p_{k|k} (updated state)"; p_.Print();
1262       debugOut << " C_{k|k} (updated covariance)"; C_.Print();
1263       debugOut << "\033[0m";
1264     }
1265 
1266     // Calculate chi² increment.  At the first point chi2inc == 0 and
1267     // the matrix will not be invertible.
1268     res_ -= H->Hv(update); // new residual
1269     if (debugLvl_ > 1) {
1270       debugOut << " resNew ";
1271       res_.Print();
1272     }
1273 
1274     // only calculate chi2inc if res != 0.
1275     // If matrix inversion fails, chi2inc = 0
1276     if (res_ != 0) {
1277       Rinv_.ResizeTo(V);
1278       Rinv_ = V - TMatrixDSym(TMatrixDSym::kAtA, H->MHt(S));
1279 
1280       bool couldInvert(true);
1281       try {
1282         tools::invertMatrix(Rinv_);
1283       }
1284       catch (Exception& e) {
1285         if (debugLvl_ > 1) {
1286           debugOut << e.what();
1287         }
1288         couldInvert = false;
1289       }
1290 
1291       if (couldInvert) {
1292         if (debugLvl_ > 1) {
1293           debugOut << " Rinv ";
1294           Rinv_.Print();
1295         }
1296         chi2inc += Rinv_.Similarity(res_);
1297       }
1298     }
1299 
1300     if (!canIgnoreWeights()) {
1301       ndfInc += m.getWeight() * m.getState().GetNrows();
1302     }
1303     else
1304       ndfInc += m.getState().GetNrows();
1305 
1306 
1307   } // end loop over measurements
1308 
1309   C_ = TMatrixDSym(TMatrixDSym::kAtA, S);
1310 
1311   chi2 += chi2inc;
1312   ndf += ndfInc;
1313 
1314 
1315   KalmanFittedStateOnPlane* upState = new KalmanFittedStateOnPlane(p_, C_, fi->getReferenceState()->getPlane(), fi->getReferenceState()->getRep(), fi->getReferenceState()->getAuxInfo(), chi2inc, ndfInc);
1316   upState->setAuxInfo(fi->getReferenceState()->getAuxInfo());
1317   fi->setUpdate(upState, direction);
1318 
1319 
1320   if (debugLvl_ > 0) {
1321     debugOut << " chi² inc " << chi2inc << "\t";
1322     debugOut << " ndf inc  " << ndfInc << "\t";
1323     debugOut << " charge of update  " << fi->getRep()->getCharge(*upState) << "\n";
1324   }
1325 
1326   // check
1327   if (not fi->checkConsistency()) {
1328     throw genfit::Exception("Consistency check failed ", __LINE__, __FILE__);
1329   }
1330 
1331 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.3.7 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!