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