GENFIT  Rev:NoNumberAvailable
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules
KalmanFitterRefTrack.cc
Go to the documentation of this file.
1 /* Copyright 2013, Ludwig-Maximilians Universität München, Technische 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 Kalman fitter with reference track. */
21 
22 #include "Tools.h"
23 #include "Track.h"
24 #include "TrackPoint.h"
25 #include "Exception.h"
26 #include "IO.h"
27 
28 #include "KalmanFitterRefTrack.h"
29 #include "KalmanFitterInfo.h"
30 #include "KalmanFitStatus.h"
31 
32 #include "boost/scoped_ptr.hpp"
33 
34 #include <TDecompChol.h>
35 #include <Math/ProbFunc.h>
36 
37 
38 using namespace genfit;
39 
40 
41 TrackPoint* KalmanFitterRefTrack::fitTrack(Track* tr, const AbsTrackRep* rep, double& chi2, double& ndf, int direction)
42 {
43 
44  //if (!isTrackPrepared(tr, rep)) {
45  // Exception exc("KalmanFitterRefTrack::fitTrack ==> track is not properly prepared.",__LINE__,__FILE__);
46  // throw exc;
47  //}
48 
49  unsigned int dim = rep->getDim();
50 
51  chi2 = 0;
52  ndf = -1. * dim;
53  KalmanFitterInfo* prevFi(NULL);
54 
55  TrackPoint* retVal(NULL);
56 
57  if (debugLvl_ > 0) {
58  debugOut << tr->getNumPoints() << " TrackPoints with measurements in this track." << std::endl;
59  }
60 
61  bool alreadyFitted(!refitAll_);
62 
63  p_.ResizeTo(dim);
64  C_.ResizeTo(dim, dim);
65 
66  for (size_t i = 0; i < tr->getNumPointsWithMeasurement(); ++i) {
67  TrackPoint *tp = 0;
68  assert(direction == +1 || direction == -1);
69  if (direction == +1)
70  tp = tr->getPointWithMeasurement(i);
71  else if (direction == -1)
72  tp = tr->getPointWithMeasurement(-i-1);
73 
74  if (! tp->hasFitterInfo(rep)) {
75  if (debugLvl_ > 0) {
76  debugOut << "TrackPoint " << i << " has no fitterInfo -> continue. \n";
77  }
78  continue;
79  }
80 
81  KalmanFitterInfo* fi = static_cast<KalmanFitterInfo*>(tp->getFitterInfo(rep));
82 
83  if (alreadyFitted && fi->hasUpdate(direction)) {
84  if (debugLvl_ > 0) {
85  debugOut << "TrackPoint " << i << " is already fitted -> continue. \n";
86  }
87  prevFi = fi;
88  chi2 += fi->getUpdate(direction)->getChiSquareIncrement();
89  ndf += fi->getUpdate(direction)->getNdf();
90  continue;
91  }
92 
93  alreadyFitted = false;
94 
95  if (debugLvl_ > 0) {
96  debugOut << " process TrackPoint nr. " << i << "\n";
97  }
98  processTrackPoint(fi, prevFi, tp, chi2, ndf, direction);
99  retVal = tp;
100 
101  prevFi = fi;
102  }
103 
104  return retVal;
105 }
106 
107 
108 void KalmanFitterRefTrack::processTrackWithRep(Track* tr, const AbsTrackRep* rep, bool resortHits)
109 {
110 
111  if (tr->getFitStatus(rep) != NULL && tr->getFitStatus(rep)->isTrackPruned()) {
112  Exception exc("KalmanFitterRefTrack::processTrack: Cannot process pruned track!", __LINE__,__FILE__);
113  throw exc;
114  }
115 
116  double oldChi2FW = 1e6;
117  double oldPvalFW = 0.;
118  double oldChi2BW = 1e6;
119  double oldPvalBW = 0.;
120  double chi2FW(0), ndfFW(0);
121  double chi2BW(0), ndfBW(0);
122  int nFailedHits(0);
123 
124  KalmanFitStatus* status = new KalmanFitStatus();
125  tr->setFitStatus(status, rep);
126 
127  status->setIsFittedWithReferenceTrack(true);
128 
129  unsigned int nIt=0;
130  for (;;) {
131 
132  try {
133  if (debugLvl_ > 0) {
134  debugOut << " KalmanFitterRefTrack::processTrack with rep " << rep
135  << " (id == " << tr->getIdForRep(rep) << ")"<< ", iteration nr. " << nIt << "\n";
136  }
137 
138  // prepare
139  if (!prepareTrack(tr, rep, resortHits, nFailedHits) && !refitAll_) {
140  if (debugLvl_ > 0) {
141  debugOut << "KalmanFitterRefTrack::processTrack. Track preparation did not change anything!\n";
142  }
143 
144  status->setIsFitted();
145 
146  status->setIsFitConvergedPartially();
147  if (nFailedHits == 0)
148  status->setIsFitConvergedFully();
149  else
150  status->setIsFitConvergedFully(false);
151 
152  status->setNFailedPoints(nFailedHits);
153 
154  status->setHasTrackChanged(false);
155  status->setCharge(rep->getCharge(*static_cast<KalmanFitterInfo*>(tr->getPointWithMeasurement(0)->getFitterInfo(rep))->getBackwardUpdate()));
156  status->setNumIterations(nIt);
157  status->setForwardChi2(chi2FW);
158  status->setBackwardChi2(chi2BW);
159  status->setForwardNdf(std::max(0., ndfFW));
160  status->setBackwardNdf(std::max(0., ndfBW));
161  if (debugLvl_ > 0) {
162  status->Print();
163  }
164  return;
165  }
166 
167  if (debugLvl_ > 0) {
168  debugOut << "KalmanFitterRefTrack::processTrack. Prepared Track:";
169  tr->Print("C");
170  //tr->Print();
171  }
172 
173  // resort
174  if (resortHits) {
175  if (tr->sort()) {
176  if (debugLvl_ > 0) {
177  debugOut << "KalmanFitterRefTrack::processTrack. Resorted Track:";
178  tr->Print("C");
179  }
180  prepareTrack(tr, rep, resortHits, nFailedHits);// re-prepare if order of hits has changed!
181  status->setNFailedPoints(nFailedHits);
182  if (debugLvl_ > 0) {
183  debugOut << "KalmanFitterRefTrack::processTrack. Prepared resorted Track:";
184  tr->Print("C");
185  }
186  }
187  }
188 
189 
190  // fit forward
191  if (debugLvl_ > 0)
192  debugOut << "KalmanFitterRefTrack::forward fit\n";
193  TrackPoint* lastProcessedPoint = fitTrack(tr, rep, chi2FW, ndfFW, +1);
194 
195  // fit backward
196  if (debugLvl_ > 0) {
197  debugOut << "KalmanFitterRefTrack::backward fit\n";
198  }
199 
200  // backward fit must not necessarily start at last hit, set prediction = forward update and blow up cov
201  if (lastProcessedPoint != NULL) {
202  KalmanFitterInfo* lastInfo = static_cast<KalmanFitterInfo*>(lastProcessedPoint->getFitterInfo(rep));
203  if (! lastInfo->hasBackwardPrediction()) {
204  lastInfo->setBackwardPrediction(new MeasuredStateOnPlane(*(lastInfo->getForwardUpdate())));
206  if (debugLvl_ > 0) {
207  debugOut << "blow up cov for backward fit at TrackPoint " << lastProcessedPoint << "\n";
208  }
209  }
210  }
211 
212  fitTrack(tr, rep, chi2BW, ndfBW, -1);
213 
214  ++nIt;
215 
216 
217  double PvalBW = std::max(0.,ROOT::Math::chisquared_cdf_c(chi2BW, ndfBW));
218  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.
219 
220  if (debugLvl_ > 0) {
221  debugOut << "KalmanFitterRefTrack::Track after fit:"; tr->Print("C");
222 
223  debugOut << "old chi2s: " << oldChi2BW << ", " << oldChi2FW
224  << " new chi2s: " << chi2BW << ", " << chi2FW << std::endl;
225  debugOut << "old pVals: " << oldPvalBW << ", " << oldPvalFW
226  << " new pVals: " << PvalBW << ", " << PvalFW << std::endl;
227  }
228 
229  // See if p-value only changed little. If the initial
230  // parameters are very far off, initial chi^2 and the chi^2
231  // after the first iteration will be both very close to zero, so
232  // we need to have at least two iterations here. Convergence
233  // doesn't make much sense before running twice anyway.
234  bool converged(false);
235  bool finished(false);
236  if (nIt >= minIterations_ && fabs(oldPvalBW - PvalBW) < deltaPval_) {
237  // if pVal ~ 0, check if chi2 has changed significantly
238  if (fabs(1 - fabs(oldChi2BW / chi2BW)) > relChi2Change_) {
239  finished = false;
240  }
241  else {
242  finished = true;
243  converged = true;
244  }
245 
246  if (PvalBW == 0.)
247  converged = false;
248  }
249 
250  if (finished) {
251  if (debugLvl_ > 0) {
252  debugOut << "Fit is finished! ";
253  if(converged)
254  debugOut << "Fit is converged! ";
255  debugOut << "\n";
256  }
257 
258  if (nFailedHits == 0)
259  status->setIsFitConvergedFully(converged);
260  else
261  status->setIsFitConvergedFully(false);
262 
263  status->setIsFitConvergedPartially(converged);
264  status->setNFailedPoints(nFailedHits);
265 
266  break;
267  }
268  else {
269  oldPvalBW = PvalBW;
270  oldChi2BW = chi2BW;
271  if (debugLvl_ > 0) {
272  oldPvalFW = PvalFW;
273  oldChi2FW = chi2FW;
274  }
275  }
276 
277  if (nIt >= maxIterations_) {
278  if (debugLvl_ > 0) {
279  debugOut << "KalmanFitterRefTrack::number of max iterations reached!\n";
280  }
281  break;
282  }
283  }
284  catch(Exception& e) {
285  errorOut << e.what();
286  status->setIsFitted(false);
287  status->setIsFitConvergedFully(false);
288  status->setIsFitConvergedPartially(false);
289  status->setNFailedPoints(nFailedHits);
290  if (debugLvl_ > 0) {
291  status->Print();
292  }
293  return;
294  }
295 
296  }
297 
298 
300 
301  double charge(0);
302  if (tp != NULL) {
303  if (static_cast<KalmanFitterInfo*>(tp->getFitterInfo(rep))->hasBackwardUpdate())
304  charge = static_cast<KalmanFitterInfo*>(tp->getFitterInfo(rep))->getBackwardUpdate()->getCharge();
305  }
306  status->setCharge(charge);
307 
308  if (tp != NULL) {
309  status->setIsFitted();
310  }
311  else { // none of the trackPoints has a fitterInfo
312  status->setIsFitted(false);
313  status->setIsFitConvergedFully(false);
314  status->setIsFitConvergedPartially(false);
315  status->setNFailedPoints(nFailedHits);
316  }
317 
318  status->setHasTrackChanged(false);
319  status->setNumIterations(nIt);
320  status->setForwardChi2(chi2FW);
321  status->setBackwardChi2(chi2BW);
322  status->setForwardNdf(ndfFW);
323  status->setBackwardNdf(ndfBW);
324 
325  if (debugLvl_ > 0) {
326  status->Print();
327  }
328 }
329 
330 
331 bool KalmanFitterRefTrack::prepareTrack(Track* tr, const AbsTrackRep* rep, bool setSortingParams, int& nFailedHits) {
332 
333  if (debugLvl_ > 0) {
334  debugOut << "KalmanFitterRefTrack::prepareTrack \n";
335  }
336 
337  int notChangedUntil, notChangedFrom;
338 
339  // remove outdated reference states
340  bool changedSmthg = removeOutdated(tr, rep, notChangedUntil, notChangedFrom);
341 
342 
343  // declare matrices
344  FTransportMatrix_.ResizeTo(rep->getDim(), rep->getDim());
345  FTransportMatrix_.UnitMatrix();
346  BTransportMatrix_.ResizeTo(rep->getDim(), rep->getDim());
347 
348  FNoiseMatrix_.ResizeTo(rep->getDim(), rep->getDim());
349  BNoiseMatrix_.ResizeTo(rep->getDim(), rep->getDim());
350 
351  forwardDeltaState_.ResizeTo(rep->getDim());
352  backwardDeltaState_.ResizeTo(rep->getDim());
353 
354  // declare stuff
355  KalmanFitterInfo* prevFitterInfo(NULL);
356  boost::scoped_ptr<MeasuredStateOnPlane> firstBackwardUpdate;
357 
358  ReferenceStateOnPlane* referenceState(NULL);
359  ReferenceStateOnPlane* prevReferenceState(NULL);
360 
361  const MeasuredStateOnPlane* smoothedState(NULL);
362  const MeasuredStateOnPlane* prevSmoothedState(NULL);
363 
364  double trackLen(0);
365 
366  bool newRefState(false); // has the current Point a new reference state?
367  bool prevNewRefState(false); // has the last successfull point a new reference state?
368 
369  unsigned int nPoints = tr->getNumPoints();
370 
371 
372  unsigned int i=0;
373  nFailedHits = 0;
374 
375 
376  // loop over TrackPoints
377  for (; i<nPoints; ++i) {
378 
379  try {
380 
381  if (debugLvl_ > 0) {
382  debugOut << "Prepare TrackPoint " << i << "\n";
383  }
384 
385  TrackPoint* trackPoint = tr->getPoint(i);
386 
387  // check if we have a measurement
388  if (!trackPoint->hasRawMeasurements()) {
389  if (debugLvl_ > 0) {
390  debugOut << "TrackPoint has no rawMeasurements -> continue \n";
391  }
392  continue;
393  }
394 
395  newRefState = false;
396 
397 
398  // get fitterInfo
399  KalmanFitterInfo* fitterInfo(NULL);
400  if (trackPoint->hasFitterInfo(rep))
401  fitterInfo = dynamic_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(rep));
402 
403  // create new fitter info if none available
404  if (fitterInfo == NULL) {
405  if (debugLvl_ > 0) {
406  debugOut << "create new KalmanFitterInfo \n";
407  }
408  changedSmthg = true;
409  fitterInfo = new KalmanFitterInfo(trackPoint, rep);
410  trackPoint->setFitterInfo(fitterInfo);
411  }
412  else {
413  if (debugLvl_ > 0) {
414  debugOut << "TrackPoint " << i << " (" << trackPoint << ") already has KalmanFitterInfo \n";
415  }
416 
417  if (prevFitterInfo == NULL) {
418  if (fitterInfo->hasBackwardUpdate())
419  firstBackwardUpdate.reset(new MeasuredStateOnPlane(*(fitterInfo->getBackwardUpdate())));
420  }
421  }
422 
423  // get smoothedState if available
424  if (fitterInfo->hasPredictionsAndUpdates()) {
425  smoothedState = &(fitterInfo->getFittedState(true));
426  if (debugLvl_ > 0) {
427  debugOut << "got smoothed state \n";
428  //smoothedState->Print();
429  }
430  }
431  else {
432  smoothedState = NULL;
433  }
434 
435 
436  if (fitterInfo->hasReferenceState()) {
437 
438  referenceState = fitterInfo->getReferenceState();
439 
440 
441  if (!prevNewRefState) {
442  if (debugLvl_ > 0) {
443  debugOut << "TrackPoint already has referenceState and previous referenceState has not been altered -> continue \n";
444  }
445  trackLen += referenceState->getForwardSegmentLength();
446  if (setSortingParams)
447  trackPoint->setSortingParameter(trackLen);
448 
449  prevNewRefState = newRefState;
450  prevReferenceState = referenceState;
451  prevFitterInfo = fitterInfo;
452  prevSmoothedState = smoothedState;
453 
454  continue;
455  }
456 
457 
458  if (prevReferenceState == NULL) {
459  if (debugLvl_ > 0) {
460  debugOut << "TrackPoint already has referenceState but previous referenceState is NULL -> reset forward info of current reference state and continue \n";
461  }
462 
463  referenceState->resetForward();
464 
465  if (setSortingParams)
466  trackPoint->setSortingParameter(trackLen);
467 
468  prevNewRefState = newRefState;
469  prevReferenceState = referenceState;
470  prevFitterInfo = fitterInfo;
471  prevSmoothedState = smoothedState;
472 
473  continue;
474  }
475 
476  // previous refState has been altered ->need to update transport matrices
477  if (debugLvl_ > 0) {
478  debugOut << "TrackPoint already has referenceState but previous referenceState has been altered -> update transport matrices and continue \n";
479  }
480  StateOnPlane stateToExtrapolate(*prevReferenceState);
481 
482  // make sure track is consistent if extrapolation fails
483  prevReferenceState->resetBackward();
484  referenceState->resetForward();
485 
486  double segmentLen = rep->extrapolateToPlane(stateToExtrapolate, fitterInfo->getReferenceState()->getPlane(), false, true);
487  if (debugLvl_ > 0) {
488  debugOut << "extrapolated stateToExtrapolate (prevReferenceState) by " << segmentLen << " cm.\n";
489  }
490  trackLen += segmentLen;
491 
492  if (segmentLen == 0) {
493  FTransportMatrix_.UnitMatrix();
494  FNoiseMatrix_.Zero();
495  forwardDeltaState_.Zero();
496  BTransportMatrix_.UnitMatrix();
497  BNoiseMatrix_.Zero();
498  backwardDeltaState_.Zero();
499  }
500  else {
503  }
504 
505  prevReferenceState->setBackwardSegmentLength(-segmentLen);
506  prevReferenceState->setBackwardTransportMatrix(BTransportMatrix_);
507  prevReferenceState->setBackwardNoiseMatrix(BNoiseMatrix_);
508  prevReferenceState->setBackwardDeltaState(backwardDeltaState_);
509 
510  referenceState->setForwardSegmentLength(segmentLen);
512  referenceState->setForwardNoiseMatrix(FNoiseMatrix_);
513  referenceState->setForwardDeltaState(forwardDeltaState_);
514 
515  newRefState = true;
516 
517  if (setSortingParams)
518  trackPoint->setSortingParameter(trackLen);
519 
520  prevNewRefState = newRefState;
521  prevReferenceState = referenceState;
522  prevFitterInfo = fitterInfo;
523  prevSmoothedState = smoothedState;
524 
525  continue;
526  }
527 
528 
529  // Construct plane
530  SharedPlanePtr plane;
531  if (smoothedState != NULL) {
532  if (debugLvl_ > 0)
533  debugOut << "construct plane with smoothedState \n";
534  plane = trackPoint->getRawMeasurement(0)->constructPlane(*smoothedState);
535  }
536  else if (prevSmoothedState != NULL) {
537  if (debugLvl_ > 0) {
538  debugOut << "construct plane with prevSmoothedState \n";
539  //prevSmoothedState->Print();
540  }
541  plane = trackPoint->getRawMeasurement(0)->constructPlane(*prevSmoothedState);
542  }
543  else if (prevReferenceState != NULL) {
544  if (debugLvl_ > 0) {
545  debugOut << "construct plane with prevReferenceState \n";
546  }
547  plane = trackPoint->getRawMeasurement(0)->constructPlane(*prevReferenceState);
548  }
549  else if (rep != tr->getCardinalRep() &&
550  trackPoint->hasFitterInfo(tr->getCardinalRep()) &&
551  dynamic_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep())) != NULL &&
552  static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->hasPredictionsAndUpdates() ) {
553  if (debugLvl_ > 0) {
554  debugOut << "construct plane with smoothed state of cardinal rep fit \n";
555  }
556  TVector3 pos, mom;
557  const MeasuredStateOnPlane& fittedState = static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->getFittedState(true);
558  tr->getCardinalRep()->getPosMom(fittedState, pos, mom);
559  StateOnPlane cardinalState(rep);
560  rep->setPosMom(cardinalState, pos, mom);
561  rep->setQop(cardinalState, tr->getCardinalRep()->getQop(fittedState));
562  plane = trackPoint->getRawMeasurement(0)->constructPlane(cardinalState);
563  }
564  else {
565  if (debugLvl_ > 0) {
566  debugOut << "construct plane with state from track \n";
567  }
568  StateOnPlane seedFromTrack(rep);
569  rep->setPosMom(seedFromTrack, tr->getStateSeed()); // also fills auxInfo
570  plane = trackPoint->getRawMeasurement(0)->constructPlane(seedFromTrack);
571  }
572 
573  if (plane.get() == NULL) {
574  Exception exc("KalmanFitterRefTrack::prepareTrack ==> construced plane is NULL!",__LINE__,__FILE__);
575  exc.setFatal();
576  throw exc;
577  }
578 
579 
580 
581  // do extrapolation and set reference state infos
582  boost::scoped_ptr<StateOnPlane> stateToExtrapolate(NULL);
583  if (prevFitterInfo == NULL) { // first measurement
584  if (debugLvl_ > 0) {
585  debugOut << "prevFitterInfo == NULL \n";
586  }
587  if (smoothedState != NULL) {
588  if (debugLvl_ > 0) {
589  debugOut << "extrapolate smoothedState to plane\n";
590  }
591  stateToExtrapolate.reset(new StateOnPlane(*smoothedState));
592  }
593  else if (referenceState != NULL) {
594  if (debugLvl_ > 0) {
595  debugOut << "extrapolate referenceState to plane\n";
596  }
597  stateToExtrapolate.reset(new StateOnPlane(*referenceState));
598  }
599  else if (rep != tr->getCardinalRep() &&
600  trackPoint->hasFitterInfo(tr->getCardinalRep()) &&
601  dynamic_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep())) != NULL &&
602  static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->hasPredictionsAndUpdates() ) {
603  if (debugLvl_ > 0) {
604  debugOut << "extrapolate smoothed state of cardinal rep fit to plane\n";
605  }
606  TVector3 pos, mom;
607  const MeasuredStateOnPlane& fittedState = static_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(tr->getCardinalRep()))->getFittedState(true);
608  stateToExtrapolate.reset(new StateOnPlane(fittedState));
609  }
610  else {
611  if (debugLvl_ > 0) {
612  debugOut << "extrapolate seed from track to plane\n";
613  }
614  stateToExtrapolate.reset(new StateOnPlane(rep));
615  rep->setPosMom(*stateToExtrapolate, tr->getStateSeed());
616  rep->setTime(*stateToExtrapolate, tr->getTimeSeed());
617  }
618  } // end if (prevFitterInfo == NULL)
619  else {
620  if (prevSmoothedState != NULL) {
621  if (debugLvl_ > 0) {
622  debugOut << "extrapolate prevSmoothedState to plane \n";
623  }
624  stateToExtrapolate.reset(new StateOnPlane(*prevSmoothedState));
625  }
626  else {
627  assert (prevReferenceState != NULL);
628  if (debugLvl_ > 0) {
629  debugOut << "extrapolate prevReferenceState to plane \n";
630  }
631  stateToExtrapolate.reset(new StateOnPlane(*prevReferenceState));
632  }
633  }
634 
635  // make sure track is consistent if extrapolation fails
636  if (prevReferenceState != NULL)
637  prevReferenceState->resetBackward();
638  fitterInfo->deleteReferenceInfo();
639 
640  if (prevFitterInfo != NULL) {
641  rep->extrapolateToPlane(*stateToExtrapolate, prevFitterInfo->getPlane());
642  if (debugLvl_ > 0) {
643  debugOut << "extrapolated stateToExtrapolate to plane of prevFitterInfo (plane could have changed!) \n";
644  }
645  }
646 
647  double segmentLen = rep->extrapolateToPlane(*stateToExtrapolate, plane, false, true);
648  trackLen += segmentLen;
649  if (debugLvl_ > 0) {
650  debugOut << "extrapolated stateToExtrapolate by " << segmentLen << " cm.\t";
651  debugOut << "charge of stateToExtrapolate: " << rep->getCharge(*stateToExtrapolate) << " \n";
652  }
653 
654  // get jacobians and noise matrices
655  if (segmentLen == 0) {
656  FTransportMatrix_.UnitMatrix();
657  FNoiseMatrix_.Zero();
658  forwardDeltaState_.Zero();
659  BTransportMatrix_.UnitMatrix();
660  BNoiseMatrix_.Zero();
661  backwardDeltaState_.Zero();
662  }
663  else {
664  if (i>0)
667  }
668 
669 
670  if (i==0) {
671  // if we are at first measurement and seed state is defined somewhere else
672  segmentLen = 0;
673  trackLen = 0;
674  }
675  if (setSortingParams)
676  trackPoint->setSortingParameter(trackLen);
677 
678 
679  // set backward matrices for previous reference state
680  if (prevReferenceState != NULL) {
681  prevReferenceState->setBackwardSegmentLength(-segmentLen);
682  prevReferenceState->setBackwardTransportMatrix(BTransportMatrix_);
683  prevReferenceState->setBackwardNoiseMatrix(BNoiseMatrix_);
684  prevReferenceState->setBackwardDeltaState(backwardDeltaState_);
685  }
686 
687 
688  // create new reference state
689  newRefState = true;
690  changedSmthg = true;
691  referenceState = new ReferenceStateOnPlane(stateToExtrapolate->getState(),
692  stateToExtrapolate->getPlane(),
693  stateToExtrapolate->getRep(),
694  stateToExtrapolate->getAuxInfo());
695  referenceState->setForwardSegmentLength(segmentLen);
697  referenceState->setForwardNoiseMatrix(FNoiseMatrix_);
698  referenceState->setForwardDeltaState(forwardDeltaState_);
699 
700  referenceState->resetBackward();
701 
702  fitterInfo->setReferenceState(referenceState);
703 
704 
705  // get MeasurementsOnPlane
706  std::vector<double> oldWeights = fitterInfo->getWeights();
707  bool oldWeightsFixed = fitterInfo->areWeightsFixed();
708  fitterInfo->deleteMeasurementInfo();
709  const std::vector<AbsMeasurement*>& rawMeasurements = trackPoint->getRawMeasurements();
710  for ( std::vector< genfit::AbsMeasurement* >::const_iterator measurement = rawMeasurements.begin(), lastMeasurement = rawMeasurements.end(); measurement != lastMeasurement; ++measurement) {
711  assert((*measurement) != NULL);
712  fitterInfo->addMeasurementsOnPlane((*measurement)->constructMeasurementsOnPlane(*referenceState));
713  }
714  if (oldWeights.size() == fitterInfo->getNumMeasurements()) {
715  fitterInfo->setWeights(oldWeights);
716  fitterInfo->fixWeights(oldWeightsFixed);
717  }
718 
719 
720  // if we made it here, no Exceptions were thrown and the TrackPoint could successfully be processed
721  prevNewRefState = newRefState;
722  prevReferenceState = referenceState;
723  prevFitterInfo = fitterInfo;
724  prevSmoothedState = smoothedState;
725 
726  }
727  catch (Exception& e) {
728 
729  if (debugLvl_ > 0) {
730  errorOut << "exception at hit " << i << "\n";
731  debugOut << e.what();
732  }
733 
734 
735  ++nFailedHits;
736  if (maxFailedHits_<0 || nFailedHits <= maxFailedHits_) {
737  prevNewRefState = true;
738  referenceState = NULL;
739  smoothedState = NULL;
740  tr->getPoint(i)->deleteFitterInfo(rep);
741 
742  if (setSortingParams)
743  tr->getPoint(i)->setSortingParameter(trackLen);
744 
745  if (debugLvl_ > 0) {
746  debugOut << "There was an exception, try to continue with next TrackPoint " << i+1 << " \n";
747  }
748 
749  continue;
750  }
751 
752 
753  // clean up
754  removeForwardBackwardInfo(tr, rep, notChangedUntil, notChangedFrom);
755 
756  // set sorting parameters of rest of TrackPoints and remove FitterInfos
757  for (; i<nPoints; ++i) {
758  TrackPoint* trackPoint = tr->getPoint(i);
759 
760  if (setSortingParams)
761  trackPoint->setSortingParameter(trackLen);
762 
763  trackPoint->deleteFitterInfo(rep);
764  }
765  return true;
766 
767  }
768 
769  } // end loop over TrackPoints
770 
771 
772 
773 
774  removeForwardBackwardInfo(tr, rep, notChangedUntil, notChangedFrom);
775 
776  if (firstBackwardUpdate && tr->getPointWithMeasurementAndFitterInfo(0, rep)) {
778  if (fi && ! fi->hasForwardPrediction()) {
779  if (debugLvl_ > 0) {
780  debugOut << "set backwards update of first point as forward prediction (with blown up cov) \n";
781  }
782  if (fi->getPlane() != firstBackwardUpdate->getPlane()) {
783  rep->extrapolateToPlane(*firstBackwardUpdate, fi->getPlane());
784  }
785  firstBackwardUpdate->blowUpCov(blowUpFactor_, resetOffDiagonals_, blowUpMaxVal_);
786  fi->setForwardPrediction(new MeasuredStateOnPlane(*firstBackwardUpdate));
787  }
788  }
789 
790  KalmanFitStatus* fitStatus = dynamic_cast<KalmanFitStatus*>(tr->getFitStatus(rep));
791  if (fitStatus != NULL)
792  fitStatus->setTrackLen(trackLen);
793 
794  if (debugLvl_ > 0) {
795  debugOut << "trackLen of reference track = " << trackLen << "\n";
796  }
797 
798  // self check
799  //assert(tr->checkConsistency());
800  assert(isTrackPrepared(tr, rep));
801 
802  return changedSmthg;
803 }
804 
805 
806 bool
807 KalmanFitterRefTrack::removeOutdated(Track* tr, const AbsTrackRep* rep, int& notChangedUntil, int& notChangedFrom) {
808 
809  if (debugLvl_ > 0) {
810  debugOut << "KalmanFitterRefTrack::removeOutdated \n";
811  }
812 
813  bool changedSmthg(false);
814 
815  unsigned int nPoints = tr->getNumPoints();
816  notChangedUntil = nPoints-1;
817  notChangedFrom = 0;
818 
819  // loop over TrackPoints
820  for (unsigned int i=0; i<nPoints; ++i) {
821 
822  TrackPoint* trackPoint = tr->getPoint(i);
823 
824  // check if we have a measurement
825  if (!trackPoint->hasRawMeasurements()) {
826  if (debugLvl_ > 0) {
827  debugOut << "TrackPoint has no rawMeasurements -> continue \n";
828  }
829  continue;
830  }
831 
832  // get fitterInfo
833  KalmanFitterInfo* fitterInfo(NULL);
834  if (trackPoint->hasFitterInfo(rep))
835  fitterInfo = dynamic_cast<KalmanFitterInfo*>(trackPoint->getFitterInfo(rep));
836 
837  if (fitterInfo == NULL)
838  continue;
839 
840 
841  // check if we need to calculate or update reference state
842  if (fitterInfo->hasReferenceState()) {
843 
844  if (! fitterInfo->hasPredictionsAndUpdates()) {
845  if (debugLvl_ > 0) {
846  debugOut << "reference state but not all predictions & updates -> do not touch reference state. \n";
847  }
848  continue;
849  }
850 
851 
852  const MeasuredStateOnPlane& smoothedState = fitterInfo->getFittedState(true);
853  resM_.ResizeTo(smoothedState.getState());
854  resM_ = smoothedState.getState();
855  resM_ -= fitterInfo->getReferenceState()->getState();
856  double chi2(0);
857 
858  // calculate chi2, ignore off diagonals
859  double* resArray = resM_.GetMatrixArray();
860  for (int j=0; j<smoothedState.getCov().GetNcols(); ++j)
861  chi2 += resArray[j]*resArray[j] / smoothedState.getCov()(j,j);
862 
863  if (chi2 < deltaChi2Ref_) {
864  // reference state is near smoothed state -> do not update reference state
865  if (debugLvl_ > 0) {
866  debugOut << "reference state is near smoothed state -> do not update reference state, chi2 = " << chi2 << "\n";
867  }
868  continue;
869  } else {
870  if (debugLvl_ > 0) {
871  debugOut << "reference state is not close to smoothed state, chi2 = " << chi2 << "\n";
872  }
873  }
874  }
875 
876  if (debugLvl_ > 0) {
877  debugOut << "remove reference info \n";
878  }
879 
880  fitterInfo->deleteReferenceInfo();
881  changedSmthg = true;
882 
883  // decided to update reference state -> set notChangedUntil (only once)
884  if (notChangedUntil == (int)nPoints-1)
885  notChangedUntil = i-1;
886 
887  notChangedFrom = i+1;
888 
889  } // end loop over TrackPoints
890 
891 
892  if (debugLvl_ > 0) {
893  tr->Print("C");
894  }
895 
896  return changedSmthg;
897 }
898 
899 
900 void
901 KalmanFitterRefTrack::removeForwardBackwardInfo(Track* tr, const AbsTrackRep* rep, int notChangedUntil, int notChangedFrom) const {
902 
903  unsigned int nPoints = tr->getNumPoints();
904 
905  if (refitAll_) {
906  tr->deleteForwardInfo(0, -1, rep);
907  tr->deleteBackwardInfo(0, -1, rep);
908  return;
909  }
910 
911  // delete forward/backward info from/to points where reference states have changed
912  if (notChangedUntil != (int)nPoints-1) {
913  tr->deleteForwardInfo(notChangedUntil+1, -1, rep);
914  }
915  if (notChangedFrom != 0)
916  tr->deleteBackwardInfo(0, notChangedFrom-1, rep);
917 
918 }
919 
920 
921 void
922 KalmanFitterRefTrack::processTrackPoint(KalmanFitterInfo* fi, const KalmanFitterInfo* prevFi, const TrackPoint* tp, double& chi2, double& ndf, int direction)
923 {
925  processTrackPointSqrt(fi, prevFi, tp, chi2, ndf, direction);
926  return;
927  }
928 
929  if (debugLvl_ > 0) {
930  debugOut << " KalmanFitterRefTrack::processTrackPoint " << fi->getTrackPoint() << "\n";
931  }
932 
933  unsigned int dim = fi->getRep()->getDim();
934 
935  p_.Zero(); // p_{k|k-1}
936  C_.Zero(); // C_{k|k-1}
937 
938  // predict
939  if (prevFi != NULL) {
940  const TMatrixD& F = fi->getReferenceState()->getTransportMatrix(direction); // Transport matrix
941  assert(F.GetNcols() == (int)dim);
942  const TMatrixDSym& N = fi->getReferenceState()->getNoiseMatrix(direction); // Noise matrix
943  //p_ = ( F * prevFi->getUpdate(direction)->getState() ) + fi->getReferenceState()->getDeltaState(direction);
944  p_ = prevFi->getUpdate(direction)->getState();
945  p_ *= F;
946  p_ += fi->getReferenceState()->getDeltaState(direction);
947 
948  C_ = prevFi->getUpdate(direction)->getCov();
949  C_.Similarity(F);
950  C_ += N;
952  if (debugLvl_ > 1) {
953  debugOut << "\033[31m";
954  debugOut << "F (Transport Matrix) "; F.Print();
955  debugOut << "p_{k,r} (reference state) "; fi->getReferenceState()->getState().Print();
956  debugOut << "c (delta state) "; fi->getReferenceState()->getDeltaState(direction).Print();
957  debugOut << "F*p_{k-1,r} + c "; (F *prevFi->getReferenceState()->getState() + fi->getReferenceState()->getDeltaState(direction)).Print();
958  }
959  }
960  else {
961  if (fi->hasPrediction(direction)) {
962  if (debugLvl_ > 0) {
963  debugOut << " Use prediction as start \n";
964  }
965  p_ = fi->getPrediction(direction)->getState();
966  C_ = fi->getPrediction(direction)->getCov();
967  }
968  else {
969  if (debugLvl_ > 0) {
970  debugOut << " Use reference state and seed cov as start \n";
971  }
972  const AbsTrackRep *rep = fi->getReferenceState()->getRep();
973  p_ = fi->getReferenceState()->getState();
974 
975  // Convert from 6D covariance of the seed to whatever the trackRep wants.
976  TMatrixDSym dummy(p_.GetNrows());
977  MeasuredStateOnPlane mop(p_, dummy, fi->getReferenceState()->getPlane(), rep, fi->getReferenceState()->getAuxInfo());
978  TVector3 pos, mom;
979  rep->getPosMom(mop, pos, mom);
980  rep->setPosMomCov(mop, pos, mom, fi->getTrackPoint()->getTrack()->getCovSeed());
981  // Blow up, set.
983  fi->setPrediction(new MeasuredStateOnPlane(mop), direction);
984  C_ = mop.getCov();
985  }
986  if (debugLvl_ > 1) {
987  debugOut << "\033[31m";
988  debugOut << "p_{k,r} (reference state)"; fi->getReferenceState()->getState().Print();
989  }
990  }
991 
992  if (debugLvl_ > 1) {
993  debugOut << " p_{k|k-1} (state prediction)"; p_.Print();
994  debugOut << " C_{k|k-1} (covariance prediction)"; C_.Print();
995  debugOut << "\033[0m";
996  }
997 
998  // update(s)
999  double chi2inc = 0;
1000  double ndfInc = 0;
1001  const std::vector<MeasurementOnPlane *> measurements = getMeasurements(fi, tp, direction);
1002  for (std::vector<MeasurementOnPlane *>::const_iterator it = measurements.begin(); it != measurements.end(); ++it) {
1003  const MeasurementOnPlane& m = **it;
1004 
1005  if (!canIgnoreWeights() && m.getWeight() <= 1.01E-10) {
1006  if (debugLvl_ > 1) {
1007  debugOut << "Weight of measurement is almost 0, continue ... /n";
1008  }
1009  continue;
1010  }
1011 
1012  const AbsHMatrix* H(m.getHMatrix());
1013  // (weighted) cov
1014  const TMatrixDSym& V((!canIgnoreWeights() && m.getWeight() < 0.99999) ?
1015  1./m.getWeight() * m.getCov() :
1016  m.getCov());
1017 
1018  covSumInv_.ResizeTo(C_);
1019  covSumInv_ = C_; // (V_k + H_k C_{k|k-1} H_k^T)^(-1)
1020  H->HMHt(covSumInv_);
1021  covSumInv_ += V;
1022 
1024 
1025  const TMatrixD& CHt(H->MHt(C_));
1026 
1027  res_.ResizeTo(m.getState());
1028  res_ = m.getState();
1029  res_ -= H->Hv(p_); // residual
1030  if (debugLvl_ > 1) {
1031  debugOut << "\033[34m";
1032  debugOut << "m (measurement) "; m.getState().Print();
1033  debugOut << "V ((weighted) measurement covariance) "; (1./m.getWeight() * m.getCov()).Print();
1034  debugOut << "residual "; res_.Print();
1035  debugOut << "\033[0m";
1036  }
1037  p_ += TMatrixD(CHt, TMatrixD::kMult, covSumInv_) * res_; // updated state
1038  if (debugLvl_ > 1) {
1039  debugOut << "\033[32m";
1040  debugOut << " update"; (TMatrixD(CHt, TMatrixD::kMult, covSumInv_) * res_).Print();
1041  debugOut << "\033[0m";
1042  }
1043 
1044  covSumInv_.Similarity(CHt); // with (C H^T)^T = H C^T = H C (C is symmetric)
1045  C_ -= covSumInv_; // updated Cov
1046 
1047  if (debugLvl_ > 1) {
1048  //debugOut << " C update "; covSumInv_.Print();
1049  debugOut << "\033[32m";
1050  debugOut << " p_{k|k} (updated state)"; p_.Print();
1051  debugOut << " C_{k|k} (updated covariance)"; C_.Print();
1052  debugOut << "\033[0m";
1053  }
1054 
1055  // Calculate chi² increment. At the first point chi2inc == 0 and
1056  // the matrix will not be invertible.
1057  res_ = m.getState();
1058  res_ -= H->Hv(p_); // new residual
1059  if (debugLvl_ > 1) {
1060  debugOut << " resNew ";
1061  res_.Print();
1062  }
1063 
1064  // only calculate chi2inc if res != NULL.
1065  // If matrix inversion fails, chi2inc = 0
1066  if (res_ != 0) {
1067  Rinv_.ResizeTo(C_);
1068  Rinv_ = C_;
1069  H->HMHt(Rinv_);
1070  Rinv_ -= V;
1071  Rinv_ *= -1;
1072 
1073  bool couldInvert(true);
1074  try {
1076  }
1077  catch (Exception& e) {
1078  if (debugLvl_ > 1) {
1079  debugOut << e.what();
1080  }
1081  couldInvert = false;
1082  }
1083 
1084  if (couldInvert) {
1085  if (debugLvl_ > 1) {
1086  debugOut << " Rinv ";
1087  Rinv_.Print();
1088  }
1089  chi2inc += Rinv_.Similarity(res_);
1090  }
1091  }
1092 
1093 
1094  if (!canIgnoreWeights()) {
1095  ndfInc += m.getWeight() * m.getState().GetNrows();
1096  }
1097  else
1098  ndfInc += m.getState().GetNrows();
1099 
1100 
1101  } // end loop over measurements
1102 
1103  chi2 += chi2inc;
1104  ndf += ndfInc;
1105 
1106 
1108  upState->setAuxInfo(fi->getReferenceState()->getAuxInfo());
1109  fi->setUpdate(upState, direction);
1110 
1111 
1112  if (debugLvl_ > 0) {
1113  debugOut << " chi² inc " << chi2inc << "\t";
1114  debugOut << " ndf inc " << ndfInc << "\t";
1115  debugOut << " charge of update " << fi->getRep()->getCharge(*upState) << "\n";
1116  }
1117 
1118  // check
1119  assert(fi->checkConsistency());
1120 
1121 }
1122 
1123 
1124 
1125 
1126 void
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 != NULL) {
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.
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 {
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 
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  assert(fi->checkConsistency());
1328 
1329 }
double blowUpMaxVal_
Limit the cov entries to this maxuimum value when blowing up the cov.
void setBackwardDeltaState(const TVectorD &mat)
const AbsHMatrix * getHMatrix() const
const AbsTrackRep * getRep() const
Definition: StateOnPlane.h:66
TrackPoint * getPointWithMeasurementAndFitterInfo(int id, const AbsTrackRep *rep=NULL) const
Definition: Track.cc:228
void setWeights(const std::vector< double > &)
Set weights of measurements.
bool areWeightsFixed() const
Are the weights fixed?
unsigned int maxIterations_
Maximum number of iterations to attempt. Forward and backward are counted as one iteration.
virtual void getForwardJacobianAndNoise(TMatrixD &jacobian, TMatrixDSym &noise, TVectorD &deltaState) const =0
Get the jacobian and noise matrix of the last extrapolation.
bool hasBackwardPrediction() const
void setTrackLen(double trackLen)
boost::shared_ptr< genfit::DetPlane > SharedPlanePtr
Shared Pointer to a DetPlane.
bool hasUpdate(int direction) const
virtual SharedPlanePtr constructPlane(const StateOnPlane &state) const =0
void setUpdate(KalmanFittedStateOnPlane *update, int direction)
void setPrediction(MeasuredStateOnPlane *prediction, int direction)
void setSortingParameter(double sortingParameter)
Definition: TrackPoint.h:111
void processTrackPointSqrt(KalmanFitterInfo *fi, const KalmanFitterInfo *prevFi, const TrackPoint *tp, double &chi2, double &ndf, int direction)
void deleteBackwardInfo(int startId=0, int endId=-1, const AbsTrackRep *rep=NULL)
Definition: Track.cc:767
virtual bool checkConsistency(const genfit::PruneFlags *=NULL) const
AbsTrackRep * getCardinalRep() const
Get cardinal track representation.
Definition: Track.h:143
const TMatrixD & getTransportMatrix(int direction) const
unsigned int debugLvl_
Definition: AbsFitter.h:55
const TVectorD & getState() const
Definition: StateOnPlane.h:61
void noiseMatrixSqrt(const TMatrixDSym &noise, TMatrixD &noiseSqrt)
Calculate a sqrt for the positive semidefinite noise matrix. Rows corresponding to zero eigenvalues a...
Definition: Tools.cc:443
bool hasReferenceState() const
bool hasFitterInfo(const AbsTrackRep *rep) const
Definition: TrackPoint.h:103
bool resetOffDiagonals_
Reset the off-diagonals to 0 when blowing up the cov.
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, the extrapolated state.
void setBackwardNdf(double bNdf)
const TVectorD & getAuxInfo() const
Definition: StateOnPlane.h:63
bool prepareTrack(Track *tr, const AbsTrackRep *rep, bool setSortingParams, int &nFailedHits)
Prepare the track.
HMatrix for projecting from AbsTrackRep parameters to measured parameters in a DetPlane.
Definition: AbsHMatrix.h:37
unsigned int getNumPoints() const
Definition: Track.h:110
void processTrackPoint(KalmanFitterInfo *fi, const KalmanFitterInfo *prevFi, const TrackPoint *tp, double &chi2, double &ndf, int direction)
ReferenceStateOnPlane * getReferenceState() const
unsigned int minIterations_
Minimum number of iterations to attempt. Forward and backward are counted as one iteration.
void setAuxInfo(const TVectorD &auxInfo)
Definition: StateOnPlane.h:71
KalmanFittedStateOnPlane * getBackwardUpdate() const
double deltaPval_
Convergence criterion.
const std::vector< genfit::AbsMeasurement * > & getRawMeasurements() const
Definition: TrackPoint.h:93
virtual void setPosMom(StateOnPlane &state, const TVector3 &pos, const TVector3 &mom) const =0
Set position and momentum of state.
void setIsFittedWithReferenceTrack(bool fittedWithReferenceTrack=true)
std::ostream debugOut
void deleteForwardInfo(int startId=0, int endId=-1, const AbsTrackRep *rep=NULL)
Definition: Track.cc:738
void setNumIterations(unsigned int numIterations)
void setForwardPrediction(MeasuredStateOnPlane *forwardPrediction)
void kalmanUpdateSqrt(const TMatrixD &S, const TVectorD &res, const TMatrixD &R, const AbsHMatrix *H, TVectorD &update, TMatrixD &SNew)
Calculate the Kalman measurement update with no transport. x, S : state prediction, covariance square root res, R, H : residual, measurement covariance square root, H matrix of the measurement.
Definition: Tools.cc:517
unsigned int getNumMeasurements() const
void setBackwardNoiseMatrix(const TMatrixDSym &mat)
double getTimeSeed() const
Definition: Track.h:161
void setNFailedPoints(int nFailedPoints)
Definition: FitStatus.h:131
int getIdForRep(const AbsTrackRep *rep) const
This is used when streaming TrackPoints.
Definition: Track.cc:302
const SharedPlanePtr & getPlane() const
Definition: AbsFitterInfo.h:74
void setForwardTransportMatrix(const TMatrixD &mat)
const TMatrixDSym & getCovSeed() const
Definition: Track.h:168
AbsMeasurement * getRawMeasurement(int i=0) const
Definition: TrackPoint.cc:147
MeasuredStateOnPlane with additional info produced by a Kalman filter or DAF.
virtual void getBackwardJacobianAndNoise(TMatrixD &jacobian, TMatrixDSym &noise, TVectorD &deltaState) const =0
Get the jacobian and noise matrix of the last extrapolation if it would have been done in opposite di...
StateOnPlane with linearized transport to that ReferenceStateOnPlane from previous and next Reference...
unsigned int getNumPointsWithMeasurement() const
Definition: Track.h:114
bool canIgnoreWeights() const
returns if the fitter can ignore the weights and handle the MeasurementOnPlanes as if they had weight...
void setIsFitted(bool fitted=true)
Definition: FitStatus.h:128
bool removeOutdated(Track *tr, const AbsTrackRep *rep, int &notChangedUntil, int &notChangedFrom)
Remove referenceStates if they are too far from smoothed states.
StateOnPlane with additional covariance matrix.
virtual void setTime(StateOnPlane &state, double time) const =0
Set time at which the state was defined.
void setBackwardChi2(double bChi2)
TrackPoint * getPoint(int id) const
Definition: Track.cc:212
bool sort()
Sort TrackPoint and according to their sorting parameters.
Definition: Track.cc:634
void addMeasurementsOnPlane(const std::vector< genfit::MeasurementOnPlane * > &measurementsOnPlane)
const TMatrixDSym & getNoiseMatrix(int direction) const
virtual void Print(const Option_t *="") const
virtual const char * what() const
Standard error message handling for exceptions. use like "std::cerr << e.what();".
Definition: Exception.cc:52
void setFatal(bool b=true)
Set fatal flag.
Definition: Exception.h:61
void setBackwardTransportMatrix(const TMatrixD &mat)
void setReferenceState(ReferenceStateOnPlane *referenceState)
virtual void setQop(StateOnPlane &state, double qop) const =0
Set charge/momentum.
Abstract base class for a track representation.
Definition: AbsTrackRep.h:66
bool hasPredictionsAndUpdates() const
TrackPoint * fitTrack(Track *tr, const AbsTrackRep *rep, double &chi2, double &ndf, int direction)
Fit the track.
Collects information needed and produced by a AbsKalmanFitter implementations and is specific to one ...
virtual void getPosMom(const StateOnPlane &state, TVector3 &pos, TVector3 &mom) const =0
Get cartesian position and momentum vector of a state.
void kalmanPredictionCovSqrt(const TMatrixD &S, const TMatrixD &F, const TMatrixD &Q, TMatrixD &Snew)
Calculates the square root of the covariance matrix after the Kalman prediction (i.e. extrapolation) with transport matrix F and the noise square root Q. Gives the new covariance square root.
Definition: Tools.cc:492
virtual bool hasPrediction(int direction) const
Definition: AbsFitterInfo.h:64
bool isTrackPruned() const
Has the track been pruned after the fit?
Definition: FitStatus.h:114
void fixWeights(bool arg=true)
Exception class for error handling in GENFIT (provides storage for diagnostic information) ...
Definition: Exception.h:48
void setForwardNdf(double fNdf)
std::vector< double > getWeights() const
Get weights of measurements.
MeasuredStateOnPlane * getBackwardPrediction() const
void invertMatrix(const TMatrixDSym &mat, TMatrixDSym &inv, double *determinant=NULL)
Invert a matrix, throwing an Exception when inversion fails. Optional calculation of determinant...
Definition: Tools.cc:40
Object containing AbsMeasurement and AbsFitterInfo objects.
Definition: TrackPoint.h:50
bool isTrackPrepared(const Track *tr, const AbsTrackRep *rep) const
double blowUpFactor_
Blow up the covariance of the forward (backward) fit by this factor before seeding the backward (forw...
const TVectorD & getDeltaState(int direction) const
void setForwardDeltaState(const TVectorD &mat)
bool hasForwardPrediction() const
KalmanFittedStateOnPlane * getForwardUpdate() const
void blowUpCov(double blowUpFac, bool resetOffDiagonals=true, double maxVal=-1.)
Blow up covariance matrix with blowUpFac. Per default, off diagonals are reset to 0 and the maximum v...
void setCharge(double charge)
Definition: FitStatus.h:133
void setForwardNoiseMatrix(const TMatrixDSym &mat)
void processTrackWithRep(Track *tr, const AbsTrackRep *rep, bool resortHits=false)
virtual void setPosMomCov(MeasuredStateOnPlane &state, const TVector3 &pos, const TVector3 &mom, const TMatrixDSym &cov6x6) const =0
Set position, momentum and covariance of state.
const SharedPlanePtr & getPlane() const
Definition: StateOnPlane.h:65
A state with arbitrary dimension defined in a DetPlane.
Definition: StateOnPlane.h:45
bool hasBackwardUpdate() const
KalmanFittedStateOnPlane * getUpdate(int direction) const
std::ostream errorOut
Track * getTrack() const
Definition: TrackPoint.h:90
TrackPoint * getPointWithMeasurement(int id) const
Definition: Track.cc:220
FitStatus for use with AbsKalmanFitter implementations.
void setHasTrackChanged(bool trackChanged=true)
Definition: FitStatus.h:132
bool hasRawMeasurements() const
Definition: TrackPoint.h:96
MeasuredStateOnPlane * getPrediction(int direction) const
const AbsTrackRep * getRep() const
Definition: AbsFitterInfo.h:55
const std::vector< MeasurementOnPlane * > getMeasurements(const KalmanFitterInfo *fi, const TrackPoint *tp, int direction) const
get the measurementsOnPlane taking the multipleMeasurementHandling_ into account
Collection of TrackPoint objects, AbsTrackRep objects and FitStatus objects.
Definition: Track.h:71
const TrackPoint * getTrackPoint() const
Definition: AbsFitterInfo.h:54
void setBackwardPrediction(MeasuredStateOnPlane *backwardPrediction)
Measured coordinates on a plane.
virtual double getQop(const StateOnPlane &state) const =0
Get charge over momentum.
void setIsFitConvergedPartially(bool fitConverged=true)
Definition: FitStatus.h:130
const TMatrixDSym & getCov() const
const MeasuredStateOnPlane & getFittedState(bool biased=true) const
Get unbiased or biased (default) smoothed state.
virtual double getCharge(const StateOnPlane &state) const =0
Get the (fitted) charge of a state. This is not always equal the pdg charge (e.g. if the charge sign ...
void removeForwardBackwardInfo(Track *tr, const AbsTrackRep *rep, int notChangedUntil, int notChangedFrom) const
If refitAll_, remove all information.
void Print(const Option_t *="") const
Definition: Track.cc:1109
void setIsFitConvergedFully(bool fitConverged=true)
Definition: FitStatus.h:129
AbsFitterInfo * getFitterInfo(const AbsTrackRep *rep=NULL) const
Get fitterInfo for rep. Per default, use cardinal rep.
Definition: TrackPoint.cc:169
void setForwardChi2(double fChi2)
Defines for I/O streams used for error and debug printing.
void setFitStatus(FitStatus *fitStatus, const AbsTrackRep *rep)
Definition: Track.cc:341
void setFitterInfo(genfit::AbsFitterInfo *fitterInfo)
Takes Ownership.
Definition: TrackPoint.cc:193
virtual unsigned int getDim() const =0
Get the dimension of the state vector used by the track representation.
void deleteFitterInfo(const AbsTrackRep *rep)
Definition: TrackPoint.h:117
const TVectorD & getStateSeed() const
Definition: Track.h:164
FitStatus * getFitStatus(const AbsTrackRep *rep=NULL) const
Get FitStatus for a AbsTrackRep. Per default, return FitStatus for cardinalRep.
Definition: Track.h:152