fitters/src/KalmanFitter.cc

0001 /* Copyright 2013, Ludwig-Maximilians 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 simple Kalman fitter with no reference track
0021    that uses the stateSeed only until it forgets about it after the
0022    first few hits.  */
0023
0024 #include "KalmanFitter.h"
0025
0026 #include "Exception.h"
0027 #include "KalmanFitterInfo.h"
0028 #include "KalmanFitStatus.h"
0029 #include "Track.h"
0030 #include "TrackPoint.h"
0031 #include "Tools.h"
0032 #include "IO.h"
0033
0034 #include <Math/ProbFunc.h>
0035 #include <TBuffer.h>
0036 #include <TDecompChol.h>
0037 #include <TMatrixDSymEigen.h>
0038 #include <algorithm>
0039
0040 using namespace genfit;
0041
0042
0043 bool KalmanFitter::fitTrack(Track* tr, const AbsTrackRep* rep,
0044     double& chi2, double& ndf,
0045     int startId, int endId, int& nFailedHits)
0046 {
0047
0048   if (multipleMeasurementHandling_ == unweightedClosestToReference ||
0049       multipleMeasurementHandling_ == weightedClosestToReference ||
0050       multipleMeasurementHandling_ == unweightedClosestToReferenceWire ||
0051       multipleMeasurementHandling_ == weightedClosestToReferenceWire) {
0052     Exception exc("KalmanFitter::fitTrack ==> cannot use (un)weightedClosestToReference(Wire) as multiple measurement handling.",__LINE__,__FILE__);
0053     exc.setFatal();
0054     throw exc;
0055   }
0056
0057   if (startId < 0)
0058     startId += tr->getNumPointsWithMeasurement();
0059   if (endId < 0)
0060     endId += tr->getNumPointsWithMeasurement();
0061
0062   int direction(1);
0063   if (endId < startId)
0064     direction = -1;
0065
0066   chi2 = 0;
0067   ndf = -1. * rep->getDim();
0068
0069   nFailedHits = 0;
0070
0071   if (debugLvl_ > 0) {
0072     debugOut << tr->getNumPointsWithMeasurement() << " TrackPoints w/ measurement in this track." << std::endl;
0073   }
0074   for (int i = startId; ; i+=direction) {
0075     TrackPoint *tp = tr->getPointWithMeasurement(i);
0076     assert(direction == +1 || direction == -1);
0077
0078     if (debugLvl_ > 0) {
0079       debugOut << " process TrackPoint nr. " << i << " (" << tp << ")\n";
0080     }
0081
0082     try {
0083       processTrackPoint(tp, rep, chi2, ndf, direction);
0084     }
0085     catch (Exception& e) {
0086       errorOut << e.what();
0087
0088       ++nFailedHits;
0089       if (maxFailedHits_<0 || nFailedHits <= maxFailedHits_) {
0090         tr->getPoint(i)->deleteFitterInfo(rep);
0091
0092         if (i == endId)
0093           break;
0094
0095         if (debugLvl_ > 0)
0096           debugOut << "There was an exception, try to continue with next TrackPoint " << i+direction << " \n";
0097
0098         continue;
0099       }
0100
0101       return false;
0102     }
0103
0104     if (i == endId)
0105       break;
0106   }
0107
0108   return true;
0109 }
0110
0111
0112 void KalmanFitter::processTrackWithRep(Track* tr, const AbsTrackRep* rep, bool)
0113 {
0114
0115   if (tr->hasFitStatus(rep) and tr->getFitStatus(rep)->isTrackPruned()) {
0116     Exception exc("KalmanFitter::processTrack: Cannot process pruned track!", __LINE__,__FILE__);
0117     throw exc;
0118   }
0119
0120   TrackPoint* trackPoint = tr->getPointWithMeasurement(0);
0121
0122   if (trackPoint->hasFitterInfo(rep) &&
0123       dynamic_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(rep)) != nullptr &&
0124       static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(rep))->hasUpdate(-1)) {
0125     MeasuredStateOnPlane* update = static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(rep))->getUpdate(-1);
0126     currentState_.reset(new MeasuredStateOnPlane(*update));
0127     if (debugLvl_ > 0)
0128       debugOut << "take backward update of previous iteration as seed \n";
0129   }
0130   if (rep != tr->getCardinalRep() &&
0131       trackPoint->hasFitterInfo(tr->getCardinalRep()) &&
0132       dynamic_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep())) != nullptr &&
0133       static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->hasPredictionsAndUpdates() ) {
0134     if (debugLvl_ > 0)
0135       debugOut << "take smoothed state of cardinal rep fit as seed \n";
0136     TVector3 pos, mom;
0137     TMatrixDSym cov;
0138     const MeasuredStateOnPlane& fittedState = static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->getFittedState(true);
0139     tr->getCardinalRep()->getPosMomCov(fittedState, pos, mom, cov);
0140     currentState_.reset(new MeasuredStateOnPlane(rep));
0141     rep->setPosMomCov(*currentState_, pos, mom, cov);
0142     rep->setQop(*currentState_, tr->getCardinalRep()->getQop(fittedState));
0143   }
0144   else {
0145     currentState_.reset(new MeasuredStateOnPlane(rep));
0146     rep->setTime(*currentState_, tr->getTimeSeed());
0147     rep->setPosMomCov(*currentState_, tr->getStateSeed(), tr->getCovSeed());
0148     if (debugLvl_ > 0)
0149       debugOut << "take seed state of track as seed \n";
0150   }
0151
0152   // Only after we have linearly propagated the error into the TrackRep can we
0153   // blow up the error in good faith.
0154   currentState_->blowUpCov(blowUpFactor_, resetOffDiagonals_, blowUpMaxVal_);
0155
0156   double oldChi2FW(1.e6);
0157   double oldChi2BW(1.e6);
0158   double oldPvalFW(0.);
0159
0160   double oldPvalBW = 0.;
0161   double chi2FW(0), ndfFW(0);
0162   double chi2BW(0), ndfBW(0);
0163
0164   int nFailedHitsForward(0), nFailedHitsBackward(0);
0165
0166
0167   KalmanFitStatus* status = new KalmanFitStatus();
0168   tr->setFitStatus(status, rep);
0169
0170   unsigned int nIt = 0;
0171   for(;;) {
0172     try {
0173       if (debugLvl_ > 0) {
0174         debugOut << "\033[1;21mstate pre" << std::endl;
0175         currentState_->Print();
0176         debugOut << "\033[0mfitting" << std::endl;
0177       }
0178
0179       if (!fitTrack(tr, rep, chi2FW, ndfFW, 0, -1, nFailedHitsForward)) {
0180         status->setIsFitted(false);
0181         status->setIsFitConvergedFully(false);
0182         status->setIsFitConvergedPartially(false);
0183         status->setNFailedPoints(nFailedHitsForward);
0184         return;
0185       }
0186
0187       if (debugLvl_ > 0) {
0188         debugOut << "\033[1;21mstate post forward" << std::endl;
0189         currentState_->Print();
0190         debugOut << "\033[0m";
0191       }
0192
0193       // Backwards iteration:
0194       currentState_->blowUpCov(blowUpFactor_, resetOffDiagonals_, blowUpMaxVal_);  // blow up cov
0195
0196       if (!fitTrack(tr, rep, chi2BW, ndfBW, -1, 0, nFailedHitsBackward)) {
0197         status->setIsFitted(false);
0198         status->setIsFitConvergedFully(false);
0199         status->setIsFitConvergedPartially(false);
0200         status->setNFailedPoints(nFailedHitsBackward);
0201         return;
0202       }
0203
0204       if (debugLvl_ > 0) {
0205         debugOut << "\033[1;21mstate post backward" << std::endl;
0206         currentState_->Print();
0207         debugOut << "\033[0m";
0208
0209         debugOut << "old chi2s: " << oldChi2BW << ", " << oldChi2FW
0210             << " new chi2s: " << chi2BW << ", " << chi2FW
0211             << " oldPvals " << oldPvalFW << ", " << oldPvalBW << std::endl;
0212       }
0213       ++nIt;
0214
0215       double PvalBW = std::max(0.,ROOT::Math::chisquared_cdf_c(chi2BW, ndfBW));
0216       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.
0217       // See if p-value only changed little.  If the initial
0218       // parameters are very far off, initial chi^2 and the chi^2
0219       // after the first iteration will be both very close to zero, so
0220       // we need to force at least two iterations here.  Convergence
0221       // doesn't make much sense before running twice anyway.
0222       bool converged(false);
0223       bool finished(false);
0224       if (nIt >= minIterations_ && fabs(oldPvalBW - PvalBW) < deltaPval_)  {
0225         // if pVal ~ 0, check if chi2 has changed significantly
0226         if (chi2BW == 0) {
0227           finished = false;
0228         }
0229         else if (fabs(1 - fabs(oldChi2BW / chi2BW)) > relChi2Change_) {
0230           finished = false;
0231         }
0232         else {
0233           finished = true;
0234           converged = true;
0235         }
0236
0237         if (PvalBW == 0.)
0238           converged = false;
0239       }
0240
0241       if (finished) {
0242         if (debugLvl_ > 0) {
0243           debugOut << "Fit is finished! ";
0244           if(converged)
0245             debugOut << "Fit is converged! ";
0246           debugOut << "\n";
0247         }
0248
0249         if (nFailedHitsForward == 0 && nFailedHitsBackward == 0)
0250           status->setIsFitConvergedFully(converged);
0251         else
0252           status->setIsFitConvergedFully(false);
0253
0254         status->setIsFitConvergedPartially(converged);
0255
0256         status->setNFailedPoints(std::max(nFailedHitsForward, nFailedHitsBackward));
0257
0258         break;
0259       }
0260       else {
0261         oldPvalBW = PvalBW;
0262         oldChi2BW = chi2BW;
0263         if (debugLvl_ > 0) {
0264           oldPvalFW = PvalFW;
0265           oldChi2FW = chi2FW;
0266         }
0267         currentState_->blowUpCov(blowUpFactor_, resetOffDiagonals_, blowUpMaxVal_);  // blow up cov
0268       }
0269
0270       if (nIt >= maxIterations_) {
0271         if (debugLvl_ > 0)
0272           debugOut << "KalmanFitter::number of max iterations reached!\n";
0273         break;
0274       }
0275     }
0276     catch(Exception& e) { // should not happen, but I leave it in for safety
0277       errorOut << e.what();
0278       status->setIsFitted(false);
0279       status->setIsFitConvergedFully(false);
0280       status->setIsFitConvergedPartially(false);
0281       status->setNFailedPoints(std::max(nFailedHitsForward, nFailedHitsBackward));
0282       return;
0283     }
0284   }
0285
0286   status->setIsFitted();
0287   double charge(0);
0288   TrackPoint* tp = tr->getPointWithMeasurementAndFitterInfo(0, rep);
0289   if (tp != nullptr) {
0290     if (static_cast<KalmanFitterInfo*>(tp->getFitterInfo(rep))->hasBackwardUpdate())
0291       charge = static_cast<KalmanFitterInfo*>(tp->getFitterInfo(rep))->getBackwardUpdate()->getCharge();
0292   }
0293   status->setNFailedPoints(std::max(nFailedHitsForward, nFailedHitsBackward));
0294   status->setCharge(charge);
0295   status->setNumIterations(nIt);
0296   status->setForwardChi2(chi2FW);
0297   status->setBackwardChi2(chi2BW);
0298   status->setForwardNdf(std::max(0., ndfFW));
0299   status->setBackwardNdf(std::max(0., ndfBW));
0300 }
0301
0302
0303 void
0304 KalmanFitter::processTrackPartially(Track* tr, const AbsTrackRep* rep, int startId, int endId) {
0305
0306   if (tr->getFitStatus(rep) != nullptr && tr->getFitStatus(rep)->isTrackPruned()) {
0307     Exception exc("KalmanFitter::processTrack: Cannot process pruned track!", __LINE__,__FILE__);
0308     throw exc;
0309   }
0310
0311   if (startId < 0)
0312     startId += tr->getNumPointsWithMeasurement();
0313   if (endId < 0)
0314     endId += tr->getNumPointsWithMeasurement();
0315
0316   int direction(1);
0317   if (endId < startId)
0318     direction = -1;
0319
0320
0321   TrackPoint* trackPoint = tr->getPointWithMeasurement(startId);
0322   TrackPoint* prevTrackPoint(nullptr);
0323
0324
0325   if (direction == 1 && startId > 0)
0326     prevTrackPoint = tr->getPointWithMeasurement(startId - 1);
0327   else if (direction == -1 && startId < (int)tr->getNumPointsWithMeasurement() - 1)
0328     prevTrackPoint = tr->getPointWithMeasurement(startId + 1);
0329
0330
0331   if (prevTrackPoint != nullptr &&
0332       prevTrackPoint->hasFitterInfo(rep) &&
0333       dynamic_cast<KalmanFitterInfo*>(prevTrackPoint->getFitterInfo(rep)) != nullptr &&
0334       static_cast<KalmanFitterInfo*>(prevTrackPoint->getFitterInfo(rep))->hasUpdate(direction)) {
0335     currentState_.reset(new MeasuredStateOnPlane(*(static_cast<KalmanFitterInfo*>(prevTrackPoint->getFitterInfo(rep))->getUpdate(direction))));
0336     if (debugLvl_ > 0)
0337       debugOut << "take update of previous fitter info as seed \n";
0338   }
0339   else if (rep != tr->getCardinalRep() &&
0340       trackPoint->hasFitterInfo(tr->getCardinalRep()) &&
0341       dynamic_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep())) != nullptr &&
0342       static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->hasPredictionsAndUpdates() ) {
0343     if (debugLvl_ > 0)
0344       debugOut << "take smoothed state of cardinal rep fit as seed \n";
0345     TVector3 pos, mom;
0346     TMatrixDSym cov;
0347     const MeasuredStateOnPlane& fittedState = static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->getFittedState(true);
0348     tr->getCardinalRep()->getPosMomCov(fittedState, pos, mom, cov);
0349     currentState_.reset(new MeasuredStateOnPlane(rep));
0350     rep->setPosMomCov(*currentState_, pos, mom, cov);
0351     rep->setQop(*currentState_, tr->getCardinalRep()->getQop(fittedState));
0352   }
0353   else {
0354     currentState_.reset(new MeasuredStateOnPlane(rep));
0355     rep->setTime(*currentState_, tr->getTimeSeed());
0356     rep->setPosMomCov(*currentState_, tr->getStateSeed(), tr->getCovSeed());
0357     if (debugLvl_ > 0)
0358       debugOut << "take seed of track as seed \n";
0359   }
0360
0361   // if at first or last hit, blow up
0362   if (startId == 0 || startId == (int)tr->getNumPointsWithMeasurement() - 1) {
0363     currentState_->blowUpCov(blowUpFactor_, resetOffDiagonals_, blowUpMaxVal_);
0364     if (debugLvl_ > 0)
0365       debugOut << "blow up seed \n";
0366   }
0367
0368   if (debugLvl_ > 0) {
0369     debugOut << "\033[1;21mstate pre" << std::endl;
0370     currentState_->Print();
0371     debugOut << "\033[0mfitting" << std::endl;
0372   }
0373
0374   double chi2, ndf;
0375   int nFailedHits;
0376   fitTrack(tr, rep, chi2, ndf, startId, endId, nFailedHits); // return value has no consequences here
0377
0378 }
0379
0380
0381 void
0382 KalmanFitter::processTrackPoint(TrackPoint* tp,
0383     const AbsTrackRep* rep, double& chi2, double& ndf, int direction)
0384 {
0385   assert(direction == -1 || direction == +1);
0386
0387   if (!tp->hasRawMeasurements())
0388     return;
0389
0390   bool newFi(!tp->hasFitterInfo(rep));
0391
0392   KalmanFitterInfo* fi;
0393   if (newFi) {
0394     fi = new KalmanFitterInfo(tp, rep);
0395     tp->setFitterInfo(fi);
0396   }
0397   else
0398     fi = static_cast<KalmanFitterInfo*>(tp->getFitterInfo(rep));
0399
0400   SharedPlanePtr plane;
0401   bool oldWeightsFixed(false);
0402   std::vector<double> oldWeights;
0403
0404   // construct measurementsOnPlane if forward fit
0405   if (newFi) {
0406     // remember old weights
0407     oldWeights = fi->getWeights();
0408     oldWeightsFixed = fi->areWeightsFixed();
0409
0410     // delete outdated stuff
0411     fi->deleteForwardInfo();
0412     fi->deleteBackwardInfo();
0413     fi->deleteMeasurementInfo();
0414
0415     // construct plane with first measurement
0416     const std::vector< genfit::AbsMeasurement* >& rawMeasurements =  tp->getRawMeasurements();
0417     plane = rawMeasurements[0]->constructPlane(*currentState_);
0418   }
0419   else
0420     plane = fi->getPlane();
0421
0422   double extLen = rep->extrapolateToPlane(*currentState_, plane);
0423   if (debugLvl_ > 0) {
0424     debugOut << "extrapolated by " << extLen << std::endl;
0425   }
0426   fi->setPrediction(currentState_->clone(), direction);
0427   MeasuredStateOnPlane *state = fi->getPrediction(direction);
0428
0429   // construct new MeasurementsOnPlane
0430   if (newFi) {
0431     const std::vector< genfit::AbsMeasurement* >& rawMeasurements =  tp->getRawMeasurements();
0432     for (std::vector< genfit::AbsMeasurement* >::const_iterator it = rawMeasurements.begin(); it != rawMeasurements.end(); ++it) {
0433       fi->addMeasurementsOnPlane((*it)->constructMeasurementsOnPlane(*state));
0434     }
0435     if (oldWeights.size() == fi->getNumMeasurements()) {
0436       fi->setWeights(oldWeights);
0437       fi->fixWeights(oldWeightsFixed);
0438       if (debugLvl_ > 0) {
0439         debugOut << "set old weights \n";
0440       }
0441     }
0442     assert(fi->getPlane() == plane);
0443     assert(fi->checkConsistency());
0444   }
0445
0446   if (debugLvl_ > 0) {
0447     debugOut << "its plane is at R = " << plane->getO().Perp()
0448               << " with normal pointing along (" << plane->getNormal().X() << ", " << plane->getNormal().Y() << ", " << plane->getNormal().Z() << ")" << std::endl;
0449   }
0450
0451   TVectorD stateVector(state->getState());
0452   TMatrixDSym cov(state->getCov());
0453   double chi2inc = 0;
0454   double ndfInc = 0;
0455
0456   if (!squareRootFormalism_) {
0457     // update(s)
0458     const std::vector<MeasurementOnPlane *>& measurements = getMeasurements(fi, tp, direction);
0459     for (std::vector<MeasurementOnPlane *>::const_iterator it = measurements.begin(); it != measurements.end(); ++it) {
0460       const MeasurementOnPlane& mOnPlane = **it;
0461       const double weight = mOnPlane.getWeight();
0462
0463       if (debugLvl_ > 0) {
0464         debugOut << "Weight of measurement: " << weight << "\n";
0465       }
0466
0467       if (!canIgnoreWeights() && weight <= 1.01E-10) {
0468         if (debugLvl_ > 0) {
0469           debugOut << "Weight of measurement is almost 0, continue ... \n";
0470         }
0471         continue;
0472       }
0473
0474       const TVectorD& measurement(mOnPlane.getState());
0475       const AbsHMatrix* H(mOnPlane.getHMatrix());
0476       // (weighted) cov
0477       const TMatrixDSym& V((!canIgnoreWeights() && weight < 0.99999) ?
0478           1./weight * mOnPlane.getCov() :
0479           mOnPlane.getCov());
0480       if (debugLvl_ > 1) {
0481         debugOut << "\033[31m";
0482         debugOut << "State prediction: "; stateVector.Print();
0483         debugOut << "Cov prediction: "; state->getCov().Print();
0484         debugOut << "\033[0m";
0485         debugOut << "\033[34m";
0486         debugOut << "measurement: "; measurement.Print();
0487         debugOut << "measurement covariance V: "; V.Print();
0488         //cov.Print();
0489         //measurement.Print();
0490       }
0491
0492       TVectorD res(measurement - H->Hv(stateVector));
0493       if (debugLvl_ > 1) {
0494         debugOut << "Residual = (" << res(0);
0495         if (res.GetNrows() > 1)
0496           debugOut << ", " << res(1);
0497         debugOut << ")" << std::endl;
0498         debugOut << "\033[0m";
0499       }
0500       // If hit, do Kalman algebra.
0501
0502       {
0503         // calculate kalman gain ------------------------------
0504         // calculate covsum (V + HCH^T) and invert
0505         TMatrixDSym covSumInv(cov);
0506         H->HMHt(covSumInv);
0507         covSumInv += V;
0508         tools::invertMatrix(covSumInv);
0509
0510         TMatrixD CHt(H->MHt(cov));
0511         TVectorD update(TMatrixD(CHt, TMatrixD::kMult, covSumInv) * res);
0512         //TMatrixD(CHt, TMatrixD::kMult, covSumInv).Print();
0513
0514         if (debugLvl_ > 1) {
0515           //debugOut << "STATUS:" << std::endl;
0516           //stateVector.Print();
0517           debugOut << "\033[32m";
0518           debugOut << "Update: "; update.Print();
0519           debugOut << "\033[0m";
0520           //cov.Print();
0521         }
0522
0523         stateVector += update;
0524         covSumInv.Similarity(CHt); // with (C H^T)^T = H C^T = H C  (C is symmetric)
0525         cov -= covSumInv;
0526       }
0527
0528       if (debugLvl_ > 1) {
0529         debugOut << "\033[32m";
0530         debugOut << "updated state: "; stateVector.Print();
0531         debugOut << "updated cov: "; cov.Print();
0532       }
0533
0534       TVectorD resNew(measurement - H->Hv(stateVector));
0535       if (debugLvl_ > 1) {
0536         debugOut << "Residual New = (" << resNew(0);
0537
0538         if (resNew.GetNrows() > 1)
0539           debugOut << ", " << resNew(1);
0540         debugOut << ")" << std::endl;
0541         debugOut << "\033[0m";
0542       }
0543
0544       // Calculate chi²
0545       TMatrixDSym HCHt(cov);
0546       H->HMHt(HCHt);
0547       HCHt -= V;
0548       HCHt *= -1;
0549
0550       tools::invertMatrix(HCHt);
0551
0552       chi2inc += HCHt.Similarity(resNew);
0553
0554       if (!canIgnoreWeights()) {
0555         ndfInc += weight * measurement.GetNrows();
0556       }
0557       else
0558         ndfInc += measurement.GetNrows();
0559
0560       if (debugLvl_ > 0) {
0561         debugOut << "chi² increment = " << chi2inc << std::endl;
0562       }
0563     } // end loop over measurements
0564   } else {
0565     // The square-root formalism is applied only to the updates, not
0566     // the prediction even though the addition of the noise covariance
0567     // (which in implicit in the extrapolation) is probably the most
0568     // fragile part of the numerical procedure.  This would require
0569     // calculating the transport matrices also here, which would be
0570     // possible but is not done, as this is not the preferred form of
0571     // the Kalman Fitter, anyway.
0572
0573     TDecompChol decompCov(cov);
0574     decompCov.Decompose();
0575     TMatrixD S(decompCov.GetU());
0576
0577     const std::vector<MeasurementOnPlane *>& measurements = getMeasurements(fi, tp, direction);
0578     for (std::vector<MeasurementOnPlane *>::const_iterator it = measurements.begin(); it != measurements.end(); ++it) {
0579       const MeasurementOnPlane& mOnPlane = **it;
0580       const double weight = mOnPlane.getWeight();
0581
0582       if (debugLvl_ > 0) {
0583         debugOut << "Weight of measurement: " << weight << "\n";
0584       }
0585
0586       if (!canIgnoreWeights() && weight <= 1.01E-10) {
0587         if (debugLvl_ > 0) {
0588           debugOut << "Weight of measurement is almost 0, continue ... \n";
0589         }
0590         continue;
0591       }
0592
0593       const TVectorD& measurement(mOnPlane.getState());
0594       const AbsHMatrix* H(mOnPlane.getHMatrix());
0595       // (weighted) cov
0596       const TMatrixDSym& V((!canIgnoreWeights() && weight < 0.99999) ?
0597           1./weight * mOnPlane.getCov() :
0598           mOnPlane.getCov());
0599       if (debugLvl_ > 1) {
0600         debugOut << "\033[31m";
0601         debugOut << "State prediction: "; stateVector.Print();
0602         debugOut << "Cov prediction: "; state->getCov().Print();
0603         debugOut << "\033[0m";
0604         debugOut << "\033[34m";
0605         debugOut << "measurement: "; measurement.Print();
0606         debugOut << "measurement covariance V: "; V.Print();
0607         //cov.Print();
0608         //measurement.Print();
0609       }
0610
0611       TVectorD res(measurement - H->Hv(stateVector));
0612       if (debugLvl_ > 1) {
0613         debugOut << "Residual = (" << res(0);
0614         if (res.GetNrows() > 1)
0615           debugOut << ", " << res(1);
0616         debugOut << ")" << std::endl;
0617         debugOut << "\033[0m";
0618       }
0619
0620       TDecompChol decompR(V);
0621       decompR.Decompose();
0622       const TMatrixD& R(decompR.GetU());
0623
0624       TVectorD update(stateVector.GetNrows());
0625       tools::kalmanUpdateSqrt(S, res, R, H,
0626           update, S);
0627       stateVector += update;
0628
0629       // Square root is such that
0630       //    cov = TMatrixDSym(TMatrixDSym::kAtA, S);
0631
0632       if (debugLvl_ > 1) {
0633         debugOut << "\033[32m";
0634         debugOut << "updated state: "; stateVector.Print();
0635         debugOut << "updated cov: "; TMatrixDSym(TMatrixDSym::kAtA, S).Print() ;
0636       }
0637
0638       res -= H->Hv(update);
0639       if (debugLvl_ > 1) {
0640         debugOut << "Residual New = (" << res(0);
0641
0642         if (res.GetNrows() > 1)
0643           debugOut << ", " << res(1);
0644         debugOut << ")" << std::endl;
0645         debugOut << "\033[0m";
0646       }
0647
0648       // Calculate chi²
0649       //
0650       // There's certainly a formula using matrix square roots, but
0651       // this is not so important numerically, so we stick with the
0652       // simpler formula.
0653       TMatrixDSym HCHt(TMatrixDSym::kAtA, H->MHt(S));
0654       HCHt -= V;
0655       HCHt *= -1;
0656
0657       tools::invertMatrix(HCHt);
0658
0659       chi2inc += HCHt.Similarity(res);
0660
0661       if (!canIgnoreWeights()) {
0662         ndfInc += weight * measurement.GetNrows();
0663       }
0664       else
0665         ndfInc += measurement.GetNrows();
0666
0667       if (debugLvl_ > 0) {
0668         debugOut << "chi² increment = " << chi2inc << std::endl;
0669       }
0670     } // end loop over measurements
0671
0672     cov = TMatrixDSym(TMatrixDSym::kAtA, S);
0673   }
0674
0675   currentState_->setStateCovPlane(stateVector, cov, plane);
0676   currentState_->setAuxInfo(state->getAuxInfo());
0677
0678   chi2 += chi2inc;
0679   ndf += ndfInc;
0680
0681   // set update
0682   KalmanFittedStateOnPlane* updatedSOP = new KalmanFittedStateOnPlane(*currentState_, chi2inc, ndfInc);
0683   fi->setUpdate(updatedSOP, direction);
0684 }
0685
0686
0687 // Modified from auto-generated streamer to deal with scoped_ptr correctly.
0688 void KalmanFitter::Streamer(TBuffer &R__b)
0689 {
0690   // Stream an object of class genfit::KalmanFitter.
0691
0692   //This works around a msvc bug and should be harmless on other platforms
0693   typedef ::genfit::KalmanFitter thisClass;
0694   UInt_t R__s, R__c;
0695   if (R__b.IsReading()) {
0696     Version_t R__v = R__b.ReadVersion(&R__s, &R__c); if (R__v) { }
0697     //This works around a msvc bug and should be harmless on other platforms
0698     typedef genfit::AbsKalmanFitter baseClass0;
0699     baseClass0::Streamer(R__b);
0700     MeasuredStateOnPlane *p = 0;
0701     R__b >> p;
0702     currentState_.reset(p);
0703     R__b.CheckByteCount(R__s, R__c, thisClass::IsA());
0704   } else {
0705     R__c = R__b.WriteVersion(thisClass::IsA(), kTRUE);
0706     //This works around a msvc bug and should be harmless on other platforms
0707     typedef genfit::AbsKalmanFitter baseClass0;
0708     baseClass0::Streamer(R__b);
0709     R__b << currentState_.get();
0710     R__b.SetByteCount(R__c, kTRUE);
0711   }
0712 }