32 #include <TDecompChol.h>
33 #include <Math/ProbFunc.h>
36 using namespace genfit;
62 C_.ResizeTo(dim, dim);
66 assert(direction == +1 || direction == -1);
69 else if (direction == -1)
74 debugOut <<
"TrackPoint " <<
i <<
" has no fitterInfo -> continue. \n";
81 if (alreadyFitted && fi->
hasUpdate(direction)) {
83 debugOut <<
"TrackPoint " <<
i <<
" is already fitted -> continue. \n";
91 alreadyFitted =
false;
94 debugOut <<
" process TrackPoint nr. " <<
i <<
"\n";
109 Exception exc(
"KalmanFitterRefTrack::processTrack: Cannot process pruned track!", __LINE__,__FILE__);
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);
131 debugOut <<
" KalmanFitterRefTrack::processTrack with rep " << rep
132 <<
" (id == " << tr->
getIdForRep(rep) <<
")"<<
", iteration nr. " << nIt <<
"\n";
138 debugOut <<
"KalmanFitterRefTrack::processTrack. Track preparation did not change anything!\n";
144 if (nFailedHits == 0)
165 debugOut <<
"KalmanFitterRefTrack::processTrack. Prepared Track:";
174 debugOut <<
"KalmanFitterRefTrack::processTrack. Resorted Track:";
180 debugOut <<
"KalmanFitterRefTrack::processTrack. Prepared resorted Track:";
189 debugOut <<
"KalmanFitterRefTrack::forward fit\n";
194 debugOut <<
"KalmanFitterRefTrack::backward fit\n";
198 if (lastProcessedPoint !=
nullptr) {
204 debugOut <<
"blow up cov for backward fit at TrackPoint " << lastProcessedPoint <<
"\n";
209 fitTrack(tr, rep, chi2BW, ndfBW, -1);
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;
218 debugOut <<
"KalmanFitterRefTrack::Track after fit:"; tr->
Print(
"C");
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;
231 bool converged(
false);
232 bool finished(
false);
255 if (nFailedHits == 0)
276 debugOut <<
"KalmanFitterRefTrack::number of max iterations reached!\n";
300 if (static_cast<KalmanFitterInfo*>(tp->
getFitterInfo(rep))->hasBackwardUpdate())
301 charge = static_cast<KalmanFitterInfo*>(tp->
getFitterInfo(rep))->getBackwardUpdate()->getCharge();
331 debugOut <<
"KalmanFitterRefTrack::prepareTrack \n";
334 int notChangedUntil, notChangedFrom;
337 bool changedSmthg =
removeOutdated(tr, rep, notChangedUntil, notChangedFrom);
353 std::unique_ptr<MeasuredStateOnPlane> firstBackwardUpdate;
363 bool newRefState(
false);
364 bool prevNewRefState(
false);
374 for (; i<nPoints; ++
i) {
379 debugOut <<
"Prepare TrackPoint " << i <<
"\n";
387 debugOut <<
"TrackPoint has no rawMeasurements -> continue \n";
398 fitterInfo = dynamic_cast<KalmanFitterInfo*>(trackPoint->
getFitterInfo(rep));
401 if (fitterInfo ==
nullptr) {
403 debugOut <<
"create new KalmanFitterInfo \n";
411 debugOut <<
"TrackPoint " << i <<
" (" << trackPoint <<
") already has KalmanFitterInfo \n";
414 if (prevFitterInfo ==
nullptr) {
424 debugOut <<
"got smoothed state \n";
429 smoothedState =
nullptr;
438 if (!prevNewRefState) {
440 debugOut <<
"TrackPoint already has referenceState and previous referenceState has not been altered -> continue \n";
443 if (setSortingParams)
446 prevNewRefState = newRefState;
447 prevReferenceState = referenceState;
448 prevFitterInfo = fitterInfo;
449 prevSmoothedState = smoothedState;
455 if (prevReferenceState ==
nullptr) {
457 debugOut <<
"TrackPoint already has referenceState but previous referenceState is nullptr -> reset forward info of current reference state and continue \n";
462 if (setSortingParams)
465 prevNewRefState = newRefState;
466 prevReferenceState = referenceState;
467 prevFitterInfo = fitterInfo;
468 prevSmoothedState = smoothedState;
475 debugOut <<
"TrackPoint already has referenceState but previous referenceState has been altered -> update transport matrices and continue \n";
485 debugOut <<
"extrapolated stateToExtrapolate (prevReferenceState) by " << segmentLen <<
" cm.\n";
487 trackLen += segmentLen;
489 if (segmentLen == 0) {
514 if (setSortingParams)
517 prevNewRefState = newRefState;
518 prevReferenceState = referenceState;
519 prevFitterInfo = fitterInfo;
520 prevSmoothedState = smoothedState;
528 if (smoothedState !=
nullptr) {
530 debugOut <<
"construct plane with smoothedState \n";
533 else if (prevSmoothedState !=
nullptr) {
535 debugOut <<
"construct plane with prevSmoothedState \n";
540 else if (prevReferenceState !=
nullptr) {
542 debugOut <<
"construct plane with prevReferenceState \n";
551 debugOut <<
"construct plane with smoothed state of cardinal rep fit \n";
563 debugOut <<
"construct plane with state from track \n";
570 if (plane.get() ==
nullptr) {
571 Exception exc(
"KalmanFitterRefTrack::prepareTrack ==> construced plane is nullptr!",__LINE__,__FILE__);
579 std::unique_ptr<StateOnPlane> stateToExtrapolate(
nullptr);
580 if (prevFitterInfo ==
nullptr) {
582 debugOut <<
"prevFitterInfo == nullptr \n";
584 if (smoothedState !=
nullptr) {
586 debugOut <<
"extrapolate smoothedState to plane\n";
588 stateToExtrapolate.reset(
new StateOnPlane(*smoothedState));
590 else if (referenceState !=
nullptr) {
592 debugOut <<
"extrapolate referenceState to plane\n";
594 stateToExtrapolate.reset(
new StateOnPlane(*referenceState));
601 debugOut <<
"extrapolate smoothed state of cardinal rep fit to plane\n";
605 stateToExtrapolate.reset(
new StateOnPlane(fittedState));
606 stateToExtrapolate->
setRep(rep);
610 debugOut <<
"extrapolate seed from track to plane\n";
618 if (prevSmoothedState !=
nullptr) {
620 debugOut <<
"extrapolate prevSmoothedState to plane \n";
622 stateToExtrapolate.reset(
new StateOnPlane(*prevSmoothedState));
625 assert (prevReferenceState !=
nullptr);
627 debugOut <<
"extrapolate prevReferenceState to plane \n";
629 stateToExtrapolate.reset(
new StateOnPlane(*prevReferenceState));
634 if (prevReferenceState !=
nullptr)
638 if (prevFitterInfo !=
nullptr) {
641 debugOut <<
"extrapolated stateToExtrapolate to plane of prevFitterInfo (plane could have changed!) \n";
646 trackLen += segmentLen;
648 debugOut <<
"extrapolated stateToExtrapolate by " << segmentLen <<
" cm.\t";
649 debugOut <<
"charge of stateToExtrapolate: " << rep->
getCharge(*stateToExtrapolate) <<
" \n";
653 if (segmentLen == 0) {
673 if (setSortingParams)
678 if (prevReferenceState !=
nullptr) {
691 stateToExtrapolate->
getRep(),
704 std::vector<double> oldWeights = fitterInfo->
getWeights();
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);
719 prevNewRefState = newRefState;
720 prevReferenceState = referenceState;
721 prevFitterInfo = fitterInfo;
722 prevSmoothedState = smoothedState;
728 errorOut <<
"exception at hit " << i <<
"\n";
735 prevNewRefState =
true;
736 referenceState =
nullptr;
737 smoothedState =
nullptr;
740 if (setSortingParams)
744 debugOut <<
"There was an exception, try to continue with next TrackPoint " << i+1 <<
" \n";
755 for (; i<nPoints; ++
i) {
758 if (setSortingParams)
778 debugOut <<
"set backwards update of first point as forward prediction (with blown up cov) \n";
789 if (fitStatus !=
nullptr)
793 debugOut <<
"trackLen of reference track = " << trackLen <<
"\n";
808 debugOut <<
"KalmanFitterRefTrack::removeOutdated \n";
811 bool changedSmthg(
false);
814 notChangedUntil = nPoints-1;
818 for (
unsigned int i=0;
i<nPoints; ++
i) {
825 debugOut <<
"TrackPoint has no rawMeasurements -> continue \n";
833 fitterInfo = dynamic_cast<KalmanFitterInfo*>(trackPoint->
getFitterInfo(rep));
835 if (fitterInfo ==
nullptr)
844 debugOut <<
"reference state but not all predictions & updates -> do not touch reference state. \n";
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);
864 debugOut <<
"reference state is near smoothed state -> do not update reference state, chi2 = " << chi2 <<
"\n";
869 debugOut <<
"reference state is not close to smoothed state, chi2 = " << chi2 <<
"\n";
875 debugOut <<
"remove reference info \n";
882 if (notChangedUntil == (
int)nPoints-1)
883 notChangedUntil =
i-1;
885 notChangedFrom =
i+1;
910 if (notChangedUntil != (
int)nPoints-1) {
913 if (notChangedFrom != 0)
937 if (prevFi !=
nullptr) {
939 assert(F.GetNcols() == (int)dim);
952 debugOut <<
"F (Transport Matrix) "; F.Print();
961 debugOut <<
" Use prediction as start \n";
968 debugOut <<
" Use reference state and seed cov as start \n";
974 TMatrixDSym
dummy(
p_.GetNrows());
991 debugOut <<
" p_{k|k-1} (state prediction)";
p_.Print();
992 debugOut <<
" C_{k|k-1} (covariance prediction)";
C_.Print();
999 const std::vector<MeasurementOnPlane *> measurements =
getMeasurements(fi, tp, direction);
1000 for (std::vector<MeasurementOnPlane *>::const_iterator
it = measurements.begin();
it != measurements.end(); ++
it) {
1005 debugOut <<
"Weight of measurement is almost 0, continue ... /n";
1023 const TMatrixD& CHt(
H->MHt(
C_));
1048 debugOut <<
" p_{k|k} (updated state)";
p_.Print();
1049 debugOut <<
" C_{k|k} (updated covariance)";
C_.Print();
1071 bool couldInvert(
true);
1079 couldInvert =
false;
1111 debugOut <<
" chi² inc " << chi2inc <<
"\t";
1112 debugOut <<
" ndf inc " << ndfInc <<
"\t";
1128 const TrackPoint*
tp,
double& chi2,
double& ndf,
int direction)
1139 TMatrixD
S(dim, dim);
1142 if (prevFi !=
nullptr) {
1144 assert(F.GetNcols() == (int)dim);
1155 decompS.Decompose();
1163 debugOut <<
"F (Transport Matrix) "; F.Print();
1172 debugOut <<
" Use prediction as start \n";
1176 decompS.Decompose();
1181 debugOut <<
" Use reference state and seed cov as start \n";
1187 TMatrixDSym
dummy(
p_.GetNrows());
1195 TDecompChol decompS(mop.getCov());
1196 decompS.Decompose();
1206 debugOut <<
" p_{k|k-1} (state prediction)";
p_.Print();
1207 debugOut <<
" C_{k|k-1} (covariance prediction)";
C_.Print();
1215 const std::vector<MeasurementOnPlane *> measurements =
getMeasurements(fi, tp, direction);
1216 for (std::vector<MeasurementOnPlane *>::const_iterator
it = measurements.begin();
it != measurements.end(); ++
it) {
1221 debugOut <<
"Weight of measurement is almost 0, continue ... /n";
1231 TDecompChol decompR(V);
1232 decompR.Decompose();
1233 const TMatrixD&
R(decompR.GetU());
1254 debugOut <<
" update"; update.Print();
1261 debugOut <<
" p_{k|k} (updated state)";
p_.Print();
1262 debugOut <<
" C_{k|k} (updated covariance)";
C_.Print();
1268 res_ -=
H->Hv(update);
1278 Rinv_ = V - TMatrixDSym(TMatrixDSym::kAtA,
H->MHt(S));
1280 bool couldInvert(
true);
1288 couldInvert =
false;
1309 C_ = TMatrixDSym(TMatrixDSym::kAtA, S);
1321 debugOut <<
" chi² inc " << chi2inc <<
"\t";
1322 debugOut <<
" ndf inc " << ndfInc <<
"\t";