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