Belle II Software  release-05-01-25
KalmanFitter.cc
1 /* Copyright 2013, Ludwig-Maximilians Universität München,
2  Authors: Tobias Schlüter & Johannes Rauch
3 
4  This file is part of GENFIT.
5 
6  GENFIT is free software: you can redistribute it and/or modify
7  it under the terms of the GNU Lesser General Public License as published
8  by the Free Software Foundation, either version 3 of the License, or
9  (at your option) any later version.
10 
11  GENFIT is distributed in the hope that it will be useful,
12  but WITHOUT ANY WARRANTY; without even the implied warranty of
13  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  GNU Lesser General Public License for more details.
15 
16  You should have received a copy of the GNU Lesser General Public License
17  along with GENFIT. If not, see <http://www.gnu.org/licenses/>.
18  */
19 
20 /* This implements the simple Kalman fitter with no reference track
21  that uses the stateSeed only until it forgets about it after the
22  first few hits. */
23 
24 #include "KalmanFitter.h"
25 
26 #include "Exception.h"
27 #include "KalmanFitterInfo.h"
28 #include "KalmanFitStatus.h"
29 #include "Track.h"
30 #include "TrackPoint.h"
31 #include "Tools.h"
32 #include "IO.h"
33 
34 #include <Math/ProbFunc.h>
35 #include <TBuffer.h>
36 #include <TDecompChol.h>
37 #include <TMatrixDSymEigen.h>
38 #include <algorithm>
39 
40 using namespace genfit;
41 
42 
43 bool KalmanFitter::fitTrack(Track* tr, const AbsTrackRep* rep,
44  double& chi2, double& ndf,
45  int startId, int endId, int& nFailedHits)
46 {
47 
52  Exception exc("KalmanFitter::fitTrack ==> cannot use (un)weightedClosestToReference(Wire) as multiple measurement handling.",__LINE__,__FILE__);
53  exc.setFatal();
54  throw exc;
55  }
56 
57  if (startId < 0)
58  startId += tr->getNumPointsWithMeasurement();
59  if (endId < 0)
60  endId += tr->getNumPointsWithMeasurement();
61 
62  int direction(1);
63  if (endId < startId)
64  direction = -1;
65 
66  chi2 = 0;
67  ndf = -1. * rep->getDim();
68 
69  nFailedHits = 0;
70 
71  if (debugLvl_ > 0) {
72  debugOut << tr->getNumPointsWithMeasurement() << " TrackPoints w/ measurement in this track." << std::endl;
73  }
74  for (int i = startId; ; i+=direction) {
75  TrackPoint *tp = tr->getPointWithMeasurement(i);
76  assert(direction == +1 || direction == -1);
77 
78  if (debugLvl_ > 0) {
79  debugOut << " process TrackPoint nr. " << i << " (" << tp << ")\n";
80  }
81 
82  try {
83  processTrackPoint(tp, rep, chi2, ndf, direction);
84  }
85  catch (Exception& e) {
86  errorOut << e.what();
87 
88  ++nFailedHits;
89  if (maxFailedHits_<0 || nFailedHits <= maxFailedHits_) {
90  tr->getPoint(i)->deleteFitterInfo(rep);
91 
92  if (i == endId)
93  break;
94 
95  if (debugLvl_ > 0)
96  debugOut << "There was an exception, try to continue with next TrackPoint " << i+direction << " \n";
97 
98  continue;
99  }
100 
101  return false;
102  }
103 
104  if (i == endId)
105  break;
106  }
107 
108  return true;
109 }
110 
111 
113 {
114 
115  if (tr->hasFitStatus(rep) and tr->getFitStatus(rep)->isTrackPruned()) {
116  Exception exc("KalmanFitter::processTrack: Cannot process pruned track!", __LINE__,__FILE__);
117  throw exc;
118  }
119 
120  TrackPoint* trackPoint = tr->getPointWithMeasurement(0);
121 
122  if (trackPoint->hasFitterInfo(rep) &&
123  dynamic_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(rep)) != nullptr &&
124  static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(rep))->hasUpdate(-1)) {
125  MeasuredStateOnPlane* update = static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(rep))->getUpdate(-1);
126  currentState_.reset(new MeasuredStateOnPlane(*update));
127  if (debugLvl_ > 0)
128  debugOut << "take backward update of previous iteration as seed \n";
129  }
130  if (rep != tr->getCardinalRep() &&
131  trackPoint->hasFitterInfo(tr->getCardinalRep()) &&
132  dynamic_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep())) != nullptr &&
133  static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->hasPredictionsAndUpdates() ) {
134  if (debugLvl_ > 0)
135  debugOut << "take smoothed state of cardinal rep fit as seed \n";
136  TVector3 pos, mom;
137  TMatrixDSym cov;
138  const MeasuredStateOnPlane& fittedState = static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->getFittedState(true);
139  tr->getCardinalRep()->getPosMomCov(fittedState, pos, mom, cov);
140  currentState_.reset(new MeasuredStateOnPlane(rep));
141  rep->setPosMomCov(*currentState_, pos, mom, cov);
142  rep->setQop(*currentState_, tr->getCardinalRep()->getQop(fittedState));
143  }
144  else {
145  currentState_.reset(new MeasuredStateOnPlane(rep));
146  rep->setTime(*currentState_, tr->getTimeSeed());
147  rep->setPosMomCov(*currentState_, tr->getStateSeed(), tr->getCovSeed());
148  if (debugLvl_ > 0)
149  debugOut << "take seed state of track as seed \n";
150  }
151 
152  // Only after we have linearly propagated the error into the TrackRep can we
153  // blow up the error in good faith.
154  currentState_->blowUpCov(blowUpFactor_, resetOffDiagonals_, blowUpMaxVal_);
155 
156  double oldChi2FW(1.e6);
157  double oldChi2BW(1.e6);
158  double oldPvalFW(0.);
159 
160  double oldPvalBW = 0.;
161  double chi2FW(0), ndfFW(0);
162  double chi2BW(0), ndfBW(0);
163 
164  int nFailedHitsForward(0), nFailedHitsBackward(0);
165 
166 
167  KalmanFitStatus* status = new KalmanFitStatus();
168  tr->setFitStatus(status, rep);
169 
170  unsigned int nIt = 0;
171  for(;;) {
172  try {
173  if (debugLvl_ > 0) {
174  debugOut << "\033[1;21mstate pre" << std::endl;
175  currentState_->Print();
176  debugOut << "\033[0mfitting" << std::endl;
177  }
178 
179  if (!fitTrack(tr, rep, chi2FW, ndfFW, 0, -1, nFailedHitsForward)) {
180  status->setIsFitted(false);
181  status->setIsFitConvergedFully(false);
182  status->setIsFitConvergedPartially(false);
183  status->setNFailedPoints(nFailedHitsForward);
184  return;
185  }
186 
187  if (debugLvl_ > 0) {
188  debugOut << "\033[1;21mstate post forward" << std::endl;
189  currentState_->Print();
190  debugOut << "\033[0m";
191  }
192 
193  // Backwards iteration:
194  currentState_->blowUpCov(blowUpFactor_, resetOffDiagonals_, blowUpMaxVal_); // blow up cov
195 
196  if (!fitTrack(tr, rep, chi2BW, ndfBW, -1, 0, nFailedHitsBackward)) {
197  status->setIsFitted(false);
198  status->setIsFitConvergedFully(false);
199  status->setIsFitConvergedPartially(false);
200  status->setNFailedPoints(nFailedHitsBackward);
201  return;
202  }
203 
204  if (debugLvl_ > 0) {
205  debugOut << "\033[1;21mstate post backward" << std::endl;
206  currentState_->Print();
207  debugOut << "\033[0m";
208 
209  debugOut << "old chi2s: " << oldChi2BW << ", " << oldChi2FW
210  << " new chi2s: " << chi2BW << ", " << chi2FW
211  << " oldPvals " << oldPvalFW << ", " << oldPvalBW << std::endl;
212  }
213  ++nIt;
214 
215  double PvalBW = std::max(0.,ROOT::Math::chisquared_cdf_c(chi2BW, ndfBW));
216  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.
217  // See if p-value only changed little. If the initial
218  // parameters are very far off, initial chi^2 and the chi^2
219  // after the first iteration will be both very close to zero, so
220  // we need to force at least two iterations here. Convergence
221  // doesn't make much sense before running twice anyway.
222  bool converged(false);
223  bool finished(false);
224  if (nIt >= minIterations_ && fabs(oldPvalBW - PvalBW) < deltaPval_) {
225  // if pVal ~ 0, check if chi2 has changed significantly
226  if (chi2BW == 0) {
227  finished = false;
228  }
229  else if (fabs(1 - fabs(oldChi2BW / chi2BW)) > relChi2Change_) {
230  finished = false;
231  }
232  else {
233  finished = true;
234  converged = true;
235  }
236 
237  if (PvalBW == 0.)
238  converged = false;
239  }
240 
241  if (finished) {
242  if (debugLvl_ > 0) {
243  debugOut << "Fit is finished! ";
244  if(converged)
245  debugOut << "Fit is converged! ";
246  debugOut << "\n";
247  }
248 
249  if (nFailedHitsForward == 0 && nFailedHitsBackward == 0)
250  status->setIsFitConvergedFully(converged);
251  else
252  status->setIsFitConvergedFully(false);
253 
254  status->setIsFitConvergedPartially(converged);
255 
256  status->setNFailedPoints(std::max(nFailedHitsForward, nFailedHitsBackward));
257 
258  break;
259  }
260  else {
261  oldPvalBW = PvalBW;
262  oldChi2BW = chi2BW;
263  if (debugLvl_ > 0) {
264  oldPvalFW = PvalFW;
265  oldChi2FW = chi2FW;
266  }
267  currentState_->blowUpCov(blowUpFactor_, resetOffDiagonals_, blowUpMaxVal_); // blow up cov
268  }
269 
270  if (nIt >= maxIterations_) {
271  if (debugLvl_ > 0)
272  debugOut << "KalmanFitter::number of max iterations reached!\n";
273  break;
274  }
275  }
276  catch(Exception& e) { // should not happen, but I leave it in for safety
277  errorOut << e.what();
278  status->setIsFitted(false);
279  status->setIsFitConvergedFully(false);
280  status->setIsFitConvergedPartially(false);
281  status->setNFailedPoints(std::max(nFailedHitsForward, nFailedHitsBackward));
282  return;
283  }
284  }
285 
286  status->setIsFitted();
287  double charge(0);
288  TrackPoint* tp = tr->getPointWithMeasurementAndFitterInfo(0, rep);
289  if (tp != nullptr) {
290  if (static_cast<KalmanFitterInfo*>(tp->getFitterInfo(rep))->hasBackwardUpdate())
291  charge = static_cast<KalmanFitterInfo*>(tp->getFitterInfo(rep))->getBackwardUpdate()->getCharge();
292  }
293  status->setNFailedPoints(std::max(nFailedHitsForward, nFailedHitsBackward));
294  status->setCharge(charge);
295  status->setNumIterations(nIt);
296  status->setForwardChi2(chi2FW);
297  status->setBackwardChi2(chi2BW);
298  status->setForwardNdf(std::max(0., ndfFW));
299  status->setBackwardNdf(std::max(0., ndfBW));
300 }
301 
302 
303 void
304 KalmanFitter::processTrackPartially(Track* tr, const AbsTrackRep* rep, int startId, int endId) {
305 
306  if (tr->getFitStatus(rep) != nullptr && tr->getFitStatus(rep)->isTrackPruned()) {
307  Exception exc("KalmanFitter::processTrack: Cannot process pruned track!", __LINE__,__FILE__);
308  throw exc;
309  }
310 
311  if (startId < 0)
312  startId += tr->getNumPointsWithMeasurement();
313  if (endId < 0)
314  endId += tr->getNumPointsWithMeasurement();
315 
316  int direction(1);
317  if (endId < startId)
318  direction = -1;
319 
320 
321  TrackPoint* trackPoint = tr->getPointWithMeasurement(startId);
322  TrackPoint* prevTrackPoint(nullptr);
323 
324 
325  if (direction == 1 && startId > 0)
326  prevTrackPoint = tr->getPointWithMeasurement(startId - 1);
327  else if (direction == -1 && startId < (int)tr->getNumPointsWithMeasurement() - 1)
328  prevTrackPoint = tr->getPointWithMeasurement(startId + 1);
329 
330 
331  if (prevTrackPoint != nullptr &&
332  prevTrackPoint->hasFitterInfo(rep) &&
333  dynamic_cast<KalmanFitterInfo*>(prevTrackPoint->getFitterInfo(rep)) != nullptr &&
334  static_cast<KalmanFitterInfo*>(prevTrackPoint->getFitterInfo(rep))->hasUpdate(direction)) {
335  currentState_.reset(new MeasuredStateOnPlane(*(static_cast<KalmanFitterInfo*>(prevTrackPoint->getFitterInfo(rep))->getUpdate(direction))));
336  if (debugLvl_ > 0)
337  debugOut << "take update of previous fitter info as seed \n";
338  }
339  else if (rep != tr->getCardinalRep() &&
340  trackPoint->hasFitterInfo(tr->getCardinalRep()) &&
341  dynamic_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep())) != nullptr &&
342  static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->hasPredictionsAndUpdates() ) {
343  if (debugLvl_ > 0)
344  debugOut << "take smoothed state of cardinal rep fit as seed \n";
345  TVector3 pos, mom;
346  TMatrixDSym cov;
347  const MeasuredStateOnPlane& fittedState = static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->getFittedState(true);
348  tr->getCardinalRep()->getPosMomCov(fittedState, pos, mom, cov);
349  currentState_.reset(new MeasuredStateOnPlane(rep));
350  rep->setPosMomCov(*currentState_, pos, mom, cov);
351  rep->setQop(*currentState_, tr->getCardinalRep()->getQop(fittedState));
352  }
353  else {
354  currentState_.reset(new MeasuredStateOnPlane(rep));
355  rep->setTime(*currentState_, tr->getTimeSeed());
356  rep->setPosMomCov(*currentState_, tr->getStateSeed(), tr->getCovSeed());
357  if (debugLvl_ > 0)
358  debugOut << "take seed of track as seed \n";
359  }
360 
361  // if at first or last hit, blow up
362  if (startId == 0 || startId == (int)tr->getNumPointsWithMeasurement() - 1) {
363  currentState_->blowUpCov(blowUpFactor_, resetOffDiagonals_, blowUpMaxVal_);
364  if (debugLvl_ > 0)
365  debugOut << "blow up seed \n";
366  }
367 
368  if (debugLvl_ > 0) {
369  debugOut << "\033[1;21mstate pre" << std::endl;
370  currentState_->Print();
371  debugOut << "\033[0mfitting" << std::endl;
372  }
373 
374  double chi2, ndf;
375  int nFailedHits;
376  fitTrack(tr, rep, chi2, ndf, startId, endId, nFailedHits); // return value has no consequences here
377 
378 }
379 
380 
381 void
382 KalmanFitter::processTrackPoint(TrackPoint* tp,
383  const AbsTrackRep* rep, double& chi2, double& ndf, int direction)
384 {
385  assert(direction == -1 || direction == +1);
386 
387  if (!tp->hasRawMeasurements())
388  return;
389 
390  bool newFi(!tp->hasFitterInfo(rep));
391 
392  KalmanFitterInfo* fi;
393  if (newFi) {
394  fi = new KalmanFitterInfo(tp, rep);
395  tp->setFitterInfo(fi);
396  }
397  else
398  fi = static_cast<KalmanFitterInfo*>(tp->getFitterInfo(rep));
399 
400  SharedPlanePtr plane;
401  bool oldWeightsFixed(false);
402  std::vector<double> oldWeights;
403 
404  // construct measurementsOnPlane if forward fit
405  if (newFi) {
406  // remember old weights
407  oldWeights = fi->getWeights();
408  oldWeightsFixed = fi->areWeightsFixed();
409 
410  // delete outdated stuff
411  fi->deleteForwardInfo();
412  fi->deleteBackwardInfo();
413  fi->deleteMeasurementInfo();
414 
415  // construct plane with first measurement
416  const std::vector< genfit::AbsMeasurement* >& rawMeasurements = tp->getRawMeasurements();
417  plane = rawMeasurements[0]->constructPlane(*currentState_);
418  }
419  else
420  plane = fi->getPlane();
421 
422  double extLen = rep->extrapolateToPlane(*currentState_, plane);
423  if (debugLvl_ > 0) {
424  debugOut << "extrapolated by " << extLen << std::endl;
425  }
426  fi->setPrediction(currentState_->clone(), direction);
427  MeasuredStateOnPlane *state = fi->getPrediction(direction);
428 
429  // construct new MeasurementsOnPlane
430  if (newFi) {
431  const std::vector< genfit::AbsMeasurement* >& rawMeasurements = tp->getRawMeasurements();
432  for (std::vector< genfit::AbsMeasurement* >::const_iterator it = rawMeasurements.begin(); it != rawMeasurements.end(); ++it) {
433  fi->addMeasurementsOnPlane((*it)->constructMeasurementsOnPlane(*state));
434  }
435  if (oldWeights.size() == fi->getNumMeasurements()) {
436  fi->setWeights(oldWeights);
437  fi->fixWeights(oldWeightsFixed);
438  if (debugLvl_ > 0) {
439  debugOut << "set old weights \n";
440  }
441  }
442  assert(fi->getPlane() == plane);
443  assert(fi->checkConsistency());
444  }
445 
446  if (debugLvl_ > 0) {
447  debugOut << "its plane is at R = " << plane->getO().Perp()
448  << " with normal pointing along (" << plane->getNormal().X() << ", " << plane->getNormal().Y() << ", " << plane->getNormal().Z() << ")" << std::endl;
449  }
450 
451  TVectorD stateVector(state->getState());
452  TMatrixDSym cov(state->getCov());
453  double chi2inc = 0;
454  double ndfInc = 0;
455 
456  if (!squareRootFormalism_) {
457  // update(s)
458  const std::vector<MeasurementOnPlane *>& measurements = getMeasurements(fi, tp, direction);
459  for (std::vector<MeasurementOnPlane *>::const_iterator it = measurements.begin(); it != measurements.end(); ++it) {
460  const MeasurementOnPlane& mOnPlane = **it;
461  const double weight = mOnPlane.getWeight();
462 
463  if (debugLvl_ > 0) {
464  debugOut << "Weight of measurement: " << weight << "\n";
465  }
466 
467  if (!canIgnoreWeights() && weight <= 1.01E-10) {
468  if (debugLvl_ > 0) {
469  debugOut << "Weight of measurement is almost 0, continue ... \n";
470  }
471  continue;
472  }
473 
474  const TVectorD& measurement(mOnPlane.getState());
475  const AbsHMatrix* H(mOnPlane.getHMatrix());
476  // (weighted) cov
477  const TMatrixDSym& V((!canIgnoreWeights() && weight < 0.99999) ?
478  1./weight * mOnPlane.getCov() :
479  mOnPlane.getCov());
480  if (debugLvl_ > 1) {
481  debugOut << "\033[31m";
482  debugOut << "State prediction: "; stateVector.Print();
483  debugOut << "Cov prediction: "; state->getCov().Print();
484  debugOut << "\033[0m";
485  debugOut << "\033[34m";
486  debugOut << "measurement: "; measurement.Print();
487  debugOut << "measurement covariance V: "; V.Print();
488  //cov.Print();
489  //measurement.Print();
490  }
491 
492  TVectorD res(measurement - H->Hv(stateVector));
493  if (debugLvl_ > 1) {
494  debugOut << "Residual = (" << res(0);
495  if (res.GetNrows() > 1)
496  debugOut << ", " << res(1);
497  debugOut << ")" << std::endl;
498  debugOut << "\033[0m";
499  }
500  // If hit, do Kalman algebra.
501 
502  {
503  // calculate kalman gain ------------------------------
504  // calculate covsum (V + HCH^T) and invert
505  TMatrixDSym covSumInv(cov);
506  H->HMHt(covSumInv);
507  covSumInv += V;
508  tools::invertMatrix(covSumInv);
509 
510  TMatrixD CHt(H->MHt(cov));
511  TVectorD update(TMatrixD(CHt, TMatrixD::kMult, covSumInv) * res);
512  //TMatrixD(CHt, TMatrixD::kMult, covSumInv).Print();
513 
514  if (debugLvl_ > 1) {
515  //debugOut << "STATUS:" << std::endl;
516  //stateVector.Print();
517  debugOut << "\033[32m";
518  debugOut << "Update: "; update.Print();
519  debugOut << "\033[0m";
520  //cov.Print();
521  }
522 
523  stateVector += update;
524  covSumInv.Similarity(CHt); // with (C H^T)^T = H C^T = H C (C is symmetric)
525  cov -= covSumInv;
526  }
527 
528  if (debugLvl_ > 1) {
529  debugOut << "\033[32m";
530  debugOut << "updated state: "; stateVector.Print();
531  debugOut << "updated cov: "; cov.Print();
532  }
533 
534  TVectorD resNew(measurement - H->Hv(stateVector));
535  if (debugLvl_ > 1) {
536  debugOut << "Residual New = (" << resNew(0);
537 
538  if (resNew.GetNrows() > 1)
539  debugOut << ", " << resNew(1);
540  debugOut << ")" << std::endl;
541  debugOut << "\033[0m";
542  }
543 
544  // Calculate chi²
545  TMatrixDSym HCHt(cov);
546  H->HMHt(HCHt);
547  HCHt -= V;
548  HCHt *= -1;
549 
550  tools::invertMatrix(HCHt);
551 
552  chi2inc += HCHt.Similarity(resNew);
553 
554  if (!canIgnoreWeights()) {
555  ndfInc += weight * measurement.GetNrows();
556  }
557  else
558  ndfInc += measurement.GetNrows();
559 
560  if (debugLvl_ > 0) {
561  debugOut << "chi² increment = " << chi2inc << std::endl;
562  }
563  } // end loop over measurements
564  } else {
565  // The square-root formalism is applied only to the updates, not
566  // the prediction even though the addition of the noise covariance
567  // (which in implicit in the extrapolation) is probably the most
568  // fragile part of the numerical procedure. This would require
569  // calculating the transport matrices also here, which would be
570  // possible but is not done, as this is not the preferred form of
571  // the Kalman Fitter, anyway.
572 
573  TDecompChol decompCov(cov);
574  decompCov.Decompose();
575  TMatrixD S(decompCov.GetU());
576 
577  const std::vector<MeasurementOnPlane *>& measurements = getMeasurements(fi, tp, direction);
578  for (std::vector<MeasurementOnPlane *>::const_iterator it = measurements.begin(); it != measurements.end(); ++it) {
579  const MeasurementOnPlane& mOnPlane = **it;
580  const double weight = mOnPlane.getWeight();
581 
582  if (debugLvl_ > 0) {
583  debugOut << "Weight of measurement: " << weight << "\n";
584  }
585 
586  if (!canIgnoreWeights() && weight <= 1.01E-10) {
587  if (debugLvl_ > 0) {
588  debugOut << "Weight of measurement is almost 0, continue ... \n";
589  }
590  continue;
591  }
592 
593  const TVectorD& measurement(mOnPlane.getState());
594  const AbsHMatrix* H(mOnPlane.getHMatrix());
595  // (weighted) cov
596  const TMatrixDSym& V((!canIgnoreWeights() && weight < 0.99999) ?
597  1./weight * mOnPlane.getCov() :
598  mOnPlane.getCov());
599  if (debugLvl_ > 1) {
600  debugOut << "\033[31m";
601  debugOut << "State prediction: "; stateVector.Print();
602  debugOut << "Cov prediction: "; state->getCov().Print();
603  debugOut << "\033[0m";
604  debugOut << "\033[34m";
605  debugOut << "measurement: "; measurement.Print();
606  debugOut << "measurement covariance V: "; V.Print();
607  //cov.Print();
608  //measurement.Print();
609  }
610 
611  TVectorD res(measurement - H->Hv(stateVector));
612  if (debugLvl_ > 1) {
613  debugOut << "Residual = (" << res(0);
614  if (res.GetNrows() > 1)
615  debugOut << ", " << res(1);
616  debugOut << ")" << std::endl;
617  debugOut << "\033[0m";
618  }
619 
620  TDecompChol decompR(V);
621  decompR.Decompose();
622  const TMatrixD& R(decompR.GetU());
623 
624  TVectorD update(stateVector.GetNrows());
625  tools::kalmanUpdateSqrt(S, res, R, H,
626  update, S);
627  stateVector += update;
628 
629  // Square root is such that
630  // cov = TMatrixDSym(TMatrixDSym::kAtA, S);
631 
632  if (debugLvl_ > 1) {
633  debugOut << "\033[32m";
634  debugOut << "updated state: "; stateVector.Print();
635  debugOut << "updated cov: "; TMatrixDSym(TMatrixDSym::kAtA, S).Print() ;
636  }
637 
638  res -= H->Hv(update);
639  if (debugLvl_ > 1) {
640  debugOut << "Residual New = (" << res(0);
641 
642  if (res.GetNrows() > 1)
643  debugOut << ", " << res(1);
644  debugOut << ")" << std::endl;
645  debugOut << "\033[0m";
646  }
647 
648  // Calculate chi²
649  //
650  // There's certainly a formula using matrix square roots, but
651  // this is not so important numerically, so we stick with the
652  // simpler formula.
653  TMatrixDSym HCHt(TMatrixDSym::kAtA, H->MHt(S));
654  HCHt -= V;
655  HCHt *= -1;
656 
657  tools::invertMatrix(HCHt);
658 
659  chi2inc += HCHt.Similarity(res);
660 
661  if (!canIgnoreWeights()) {
662  ndfInc += weight * measurement.GetNrows();
663  }
664  else
665  ndfInc += measurement.GetNrows();
666 
667  if (debugLvl_ > 0) {
668  debugOut << "chi² increment = " << chi2inc << std::endl;
669  }
670  } // end loop over measurements
671 
672  cov = TMatrixDSym(TMatrixDSym::kAtA, S);
673  }
674 
675  currentState_->setStateCovPlane(stateVector, cov, plane);
676  currentState_->setAuxInfo(state->getAuxInfo());
677 
678  chi2 += chi2inc;
679  ndf += ndfInc;
680 
681  // set update
682  KalmanFittedStateOnPlane* updatedSOP = new KalmanFittedStateOnPlane(*currentState_, chi2inc, ndfInc);
683  fi->setUpdate(updatedSOP, direction);
684 }
685 
686 
687 // Modified from auto-generated streamer to deal with scoped_ptr correctly.
688 void KalmanFitter::Streamer(TBuffer &R__b)
689 {
690  // Stream an object of class genfit::KalmanFitter.
691 
692  //This works around a msvc bug and should be harmless on other platforms
693  typedef ::genfit::KalmanFitter thisClass;
694  UInt_t R__s, R__c;
695  if (R__b.IsReading()) {
696  Version_t R__v = R__b.ReadVersion(&R__s, &R__c); if (R__v) { }
697  //This works around a msvc bug and should be harmless on other platforms
698  typedef genfit::AbsKalmanFitter baseClass0;
699  baseClass0::Streamer(R__b);
700  MeasuredStateOnPlane *p = 0;
701  R__b >> p;
702  currentState_.reset(p);
703  R__b.CheckByteCount(R__s, R__c, thisClass::IsA());
704  } else {
705  R__c = R__b.WriteVersion(thisClass::IsA(), kTRUE);
706  //This works around a msvc bug and should be harmless on other platforms
707  typedef genfit::AbsKalmanFitter baseClass0;
708  baseClass0::Streamer(R__b);
709  R__b << currentState_.get();
710  R__b.SetByteCount(R__c, kTRUE);
711  }
712 }
genfit::unweightedClosestToReferenceWire
@ unweightedClosestToReferenceWire
if corresponding TrackPoint has one WireMeasurement, select closest to reference, weighted with 1.
Definition: AbsKalmanFitter.h:43
genfit::Exception
Exception class for error handling in GENFIT (provides storage for diagnostic information)
Definition: Exception.h:48
genfit::TrackPoint
Object containing AbsMeasurement and AbsFitterInfo objects.
Definition: TrackPoint.h:46
genfit::errorOut
std::ostream errorOut
Default stream for error output.
genfit::SharedPlanePtr
std::shared_ptr< genfit::DetPlane > SharedPlanePtr
Shared Pointer to a DetPlane.
Definition: SharedPlanePtr.h:40
genfit::AbsKalmanFitter::getMeasurements
const std::vector< MeasurementOnPlane * > getMeasurements(const KalmanFitterInfo *fi, const TrackPoint *tp, int direction) const
get the measurementsOnPlane taking the multipleMeasurementHandling_ into account
Definition: AbsKalmanFitter.cc:177
genfit::KalmanFitter::processTrackPartially
void processTrackPartially(Track *tr, const AbsTrackRep *rep, int startId=0, int endId=-1)
process only a part of the track.
Definition: KalmanFitter.cc:304
prepareAsicCrosstalkSimDB.e
e
aux.
Definition: prepareAsicCrosstalkSimDB.py:53
genfit::MeasuredStateOnPlane
#StateOnPlane with additional covariance matrix.
Definition: MeasuredStateOnPlane.h:39
genfit::AbsTrackRep::extrapolateToPlane
virtual double extrapolateToPlane(StateOnPlane &state, const genfit::SharedPlanePtr &plane, bool stopAtBoundary=false, bool calcJacobianNoise=false) const =0
Extrapolates the state to plane, and returns the extrapolation length and, via reference,...
genfit::FitStatus::isTrackPruned
bool isTrackPruned() const
Has the track been pruned after the fit?
Definition: FitStatus.h:116
genfit::KalmanFitStatus
FitStatus for use with AbsKalmanFitter implementations.
Definition: KalmanFitStatus.h:36
genfit::Track::getFitStatus
FitStatus * getFitStatus(const AbsTrackRep *rep=nullptr) const
Get FitStatus for a AbsTrackRep. Per default, return FitStatus for cardinalRep.
Definition: Track.h:154
genfit::unweightedClosestToReference
@ unweightedClosestToReference
closest to reference, weighted with 1
Definition: AbsKalmanFitter.h:39
genfit::weightedClosestToReference
@ weightedClosestToReference
closest to reference, weighted with its weight_
Definition: AbsKalmanFitter.h:38
genfit
Defines for I/O streams used for error and debug printing.
Definition: AlignablePXDRecoHit.h:19
genfit::TrackPoint::setFitterInfo
void setFitterInfo(genfit::AbsFitterInfo *fitterInfo)
Takes Ownership.
Definition: TrackPoint.cc:194
genfit::Track
Collection of TrackPoint objects, AbsTrackRep objects and FitStatus objects.
Definition: Track.h:71
genfit::AbsKalmanFitter::blowUpFactor_
double blowUpFactor_
Blow up the covariance of the forward (backward) fit by this factor before seeding the backward (forw...
Definition: AbsKalmanFitter.h:161
genfit::AbsTrackRep
Abstract base class for a track representation.
Definition: AbsTrackRep.h:66
genfit::AbsTrackRep::setTime
virtual void setTime(StateOnPlane &state, double time) const =0
Set time at which the state was defined.
genfit::AbsKalmanFitter::multipleMeasurementHandling_
eMultipleMeasurementHandling multipleMeasurementHandling_
How to handle if there are multiple MeasurementsOnPlane.
Definition: AbsKalmanFitter.h:168
genfit::AbsKalmanFitter::relChi2Change_
double relChi2Change_
@ brief Non-convergence criterion
Definition: AbsKalmanFitter.h:158
genfit::TrackPoint::getFitterInfo
AbsFitterInfo * getFitterInfo(const AbsTrackRep *rep=nullptr) const
Get fitterInfo for rep. Per default, use cardinal rep.
Definition: TrackPoint.cc:170
genfit::AbsTrackRep::setQop
virtual void setQop(StateOnPlane &state, double qop) const =0
Set charge/momentum.
genfit::AbsKalmanFitter::blowUpMaxVal_
double blowUpMaxVal_
Limit the cov entries to this maxuimum value when blowing up the cov.
Definition: AbsKalmanFitter.h:165
genfit::KalmanFitterInfo
Collects information needed and produced by a AbsKalmanFitter implementations and is specific to one ...
Definition: KalmanFitterInfo.h:44
genfit::AbsTrackRep::getPosMomCov
virtual void getPosMomCov(const MeasuredStateOnPlane &state, TVector3 &pos, TVector3 &mom, TMatrixDSym &cov) const =0
Translates MeasuredStateOnPlane into 3D position, momentum and 6x6 covariance.
genfit::AbsKalmanFitter::deltaPval_
double deltaPval_
Convergence criterion.
Definition: AbsKalmanFitter.h:148
genfit::weightedClosestToReferenceWire
@ weightedClosestToReferenceWire
if corresponding TrackPoint has one WireMeasurement, select closest to reference, weighted with its w...
Definition: AbsKalmanFitter.h:42
genfit::AbsKalmanFitter::canIgnoreWeights
bool canIgnoreWeights() const
returns if the fitter can ignore the weights and handle the MeasurementOnPlanes as if they had weight...
Definition: AbsKalmanFitter.cc:256
prepareAsicCrosstalkSimDB.fi
fi
open root file to store x-talk probability histogram
Definition: prepareAsicCrosstalkSimDB.py:88
genfit::AbsKalmanFitter
Abstract base class for Kalman fitter and derived fitting algorithms.
Definition: AbsKalmanFitter.h:51
genfit::AbsTrackRep::getDim
virtual unsigned int getDim() const =0
Get the dimension of the state vector used by the track representation.
genfit::AbsKalmanFitter::resetOffDiagonals_
bool resetOffDiagonals_
Reset the off-diagonals to 0 when blowing up the cov.
Definition: AbsKalmanFitter.h:163
genfit::AbsKalmanFitter::maxIterations_
unsigned int maxIterations_
Maximum number of iterations to attempt. Forward and backward are counted as one iteration.
Definition: AbsKalmanFitter.h:141
genfit::AbsTrackRep::setPosMomCov
virtual void setPosMomCov(MeasuredStateOnPlane &state, const TVector3 &pos, const TVector3 &mom, const TMatrixDSym &cov6x6) const =0
Set position, momentum and covariance of state.
genfit::AbsKalmanFitter::maxFailedHits_
int maxFailedHits_
after how many failed hits (exception during construction of plane, extrapolation etc....
Definition: AbsKalmanFitter.h:172
genfit::KalmanFitterInfo::getFittedState
const MeasuredStateOnPlane & getFittedState(bool biased=true) const override
Get unbiased or biased (default) smoothed state.
Definition: KalmanFitterInfo.cc:179
genfit::AbsHMatrix
HMatrix for projecting from AbsTrackRep parameters to measured parameters in a DetPlane.
Definition: AbsHMatrix.h:37
genfit::KalmanFitter::processTrackWithRep
void processTrackWithRep(Track *tr, const AbsTrackRep *rep, bool resortHits=false) override
Hit resorting currently NOT supported.
Definition: KalmanFitter.cc:112
genfit::KalmanFittedStateOnPlane
#MeasuredStateOnPlane with additional info produced by a Kalman filter or DAF.
Definition: KalmanFittedStateOnPlane.h:35
genfit::AbsTrackRep::getQop
virtual double getQop(const StateOnPlane &state) const =0
Get charge over momentum.
genfit::Track::hasFitStatus
bool hasFitStatus(const AbsTrackRep *rep=nullptr) const
Check if track has a FitStatus for given AbsTrackRep. Per default, check for cardinal rep.
Definition: Track.cc:311
genfit::Track::getCardinalRep
AbsTrackRep * getCardinalRep() const
Get cardinal track representation.
Definition: Track.h:145
genfit::MeasurementOnPlane
Measured coordinates on a plane.
Definition: MeasurementOnPlane.h:46
genfit::AbsKalmanFitter::minIterations_
unsigned int minIterations_
Minimum number of iterations to attempt. Forward and backward are counted as one iteration.
Definition: AbsKalmanFitter.h:138
genfit::debugOut
std::ostream debugOut
Default stream for debug output.