33#include <Math/ProbFunc.h>
34#include <TDecompChol.h>
35#include <TMatrixDSymEigen.h>
43 double& chi2,
double& ndf,
44 int startId,
int endId,
int& nFailedHits)
51 Exception exc(
"KalmanFitter::fitTrack ==> cannot use (un)weightedClosestToReference(Wire) as multiple measurement handling.",__LINE__,__FILE__);
73 for (
int i = startId; ; i+=direction) {
75 assert(direction == +1 || direction == -1);
78 std::cout <<
" process TrackPoint nr. " << i <<
" (" << tp <<
")\n";
85 std::cerr << e.
what();
95 std::cout <<
"There was an exception, try to continue with next TrackPoint " << i+direction <<
" \n";
115 Exception exc(
"KalmanFitter::processTrack: Cannot process pruned track!", __LINE__,__FILE__);
127 std::cout <<
"take backward update of previous iteration as seed \n";
134 std::cout <<
"take smoothed state of cardinal rep fit as seed \n";
147 std::cout <<
"take seed state of track as seed \n";
154 double oldChi2FW(1.e6);
155 double oldChi2BW(1.e6);
156 double oldPvalFW(0.);
158 double oldPvalBW = 0.;
159 double chi2FW(0), ndfFW(0);
160 double chi2BW(0), ndfBW(0);
162 int nFailedHitsForward(0), nFailedHitsBackward(0);
168 unsigned int nIt = 0;
172 std::cout <<
"\033[1;21mstate pre" << std::endl;
174 std::cout <<
"\033[0mfitting" << std::endl;
177 if (!
fitTrack(tr, rep, chi2FW, ndfFW, 0, -1, nFailedHitsForward)) {
178 status->setIsFitted(
false);
179 status->setIsFitConvergedFully(
false);
180 status->setIsFitConvergedPartially(
false);
181 status->setNFailedPoints(nFailedHitsForward);
186 std::cout <<
"\033[1;21mstate post forward" << std::endl;
188 std::cout <<
"\033[0m";
194 if (!
fitTrack(tr, rep, chi2BW, ndfBW, -1, 0, nFailedHitsBackward)) {
195 status->setIsFitted(
false);
196 status->setIsFitConvergedFully(
false);
197 status->setIsFitConvergedPartially(
false);
198 status->setNFailedPoints(nFailedHitsBackward);
203 std::cout <<
"\033[1;21mstate post backward" << std::endl;
205 std::cout <<
"\033[0m";
207 std::cout <<
"old chi2s: " << oldChi2BW <<
", " << oldChi2FW
208 <<
" new chi2s: " << chi2BW <<
", " << chi2FW
209 <<
" oldPvals " << oldPvalFW <<
", " << oldPvalBW << std::endl;
213 double PvalBW = ROOT::Math::chisquared_cdf_c(chi2BW, ndfBW);
214 double PvalFW = (
debugLvl_ > 0) ? ROOT::Math::chisquared_cdf_c(chi2FW, ndfFW) : 0;
220 bool converged(
false);
221 bool finished(
false);
241 std::cout <<
"Fit is finished! ";
243 std::cout <<
"Fit is converged! ";
247 if (nFailedHitsForward == 0 && nFailedHitsBackward == 0)
248 status->setIsFitConvergedFully(converged);
250 status->setIsFitConvergedFully(
false);
252 status->setIsFitConvergedPartially(converged);
254 status->setNFailedPoints(std::max(nFailedHitsForward, nFailedHitsBackward));
270 std::cout <<
"KalmanFitter::number of max iterations reached!\n";
275 std::cerr << e.
what();
276 status->setIsFitted(
false);
277 status->setIsFitConvergedFully(
false);
278 status->setIsFitConvergedPartially(
false);
279 status->setNFailedPoints(std::max(nFailedHitsForward, nFailedHitsBackward));
284 status->setIsFitted();
289 charge =
static_cast<KalmanFitterInfo*
>(tp->getFitterInfo(rep))->getBackwardUpdate()->getCharge();
291 status->setNFailedPoints(std::max(nFailedHitsForward, nFailedHitsBackward));
292 status->setCharge(charge);
293 status->setNumIterations(nIt);
294 status->setForwardChi2(chi2FW);
295 status->setBackwardChi2(chi2BW);
296 status->setForwardNdf(std::max(0., ndfFW));
297 status->setBackwardNdf(std::max(0., ndfBW));
305 Exception exc(
"KalmanFitter::processTrack: Cannot process pruned track!", __LINE__,__FILE__);
323 if (direction == 1 && startId > 0)
329 if (prevTrackPoint != NULL &&
335 std::cout <<
"take update of previous fitter info as seed \n";
342 std::cout <<
"take smoothed state of cardinal rep fit as seed \n";
355 std::cout <<
"take seed of track as seed \n";
362 std::cout <<
"blow up seed \n";
366 std::cout <<
"\033[1;21mstate pre" << std::endl;
368 std::cout <<
"\033[0mfitting" << std::endl;
373 fitTrack(tr, rep, chi2, ndf, startId, endId, nFailedHits);
380 const AbsTrackRep* rep,
double& chi2,
double& ndf,
int direction)
382 assert(direction == -1 || direction == +1);
384 if (!tp->hasRawMeasurements())
390 if (! tp->hasFitterInfo(rep)) {
392 tp->setFitterInfo(fi);
399 bool oldWeightsFixed(
false);
400 std::vector<double> oldWeights;
414 const std::vector< genfit::AbsMeasurement* >& rawMeasurements = tp->getRawMeasurements();
427 std::cout <<
"extrapolated by " << extLen << std::endl;
438 const std::vector< genfit::AbsMeasurement* >& rawMeasurements = tp->getRawMeasurements();
439 for (std::vector< genfit::AbsMeasurement* >::const_iterator it = rawMeasurements.begin(); it != rawMeasurements.end(); ++it) {
446 std::cout <<
"set old weights \n";
454 std::cout <<
"its plane is at R = " << plane->getO().Perp()
455 <<
" with normal pointing along (" << plane->getNormal().X() <<
", " << plane->getNormal().Y() <<
", " << plane->getNormal().Z() <<
")" << std::endl;
460 TVectorD stateVector(state->
getState());
461 TMatrixDSym cov(state->
getCov());
465 const std::vector<MeasurementOnPlane *> measurements =
getMeasurements(fi, tp, direction);
466 for (std::vector<MeasurementOnPlane *>::const_iterator it = measurements.begin(); it != measurements.end(); ++it) {
468 const double weight = mOnPlane.
getWeight();
471 std::cout <<
"Weight of measurement: " << weight <<
"\n";
476 std::cout <<
"Weight of measurement is almost 0, continue ... \n";
481 const TVectorD& measurement(mOnPlane.
getState());
485 1./weight * mOnPlane.
getCov() :
488 std::cout <<
"\033[31m";
489 std::cout <<
"State prediction: "; stateVector.Print();
490 std::cout <<
"Cov prediction: "; state->
getCov().Print();
491 std::cout <<
"\033[0m";
492 std::cout <<
"\033[34m";
493 std::cout <<
"measurement: "; measurement.Print();
494 std::cout <<
"measurement covariance V: "; V.Print();
499 TVectorD res(measurement - H->Hv(stateVector));
501 std::cout <<
"Residual = (" << res(0);
502 if (res.GetNrows() > 1)
503 std::cout <<
", " << res(1);
504 std::cout <<
")" << std::endl;
505 std::cout <<
"\033[0m";
535 TMatrixDSymEigen eig(noise);
536 TMatrixD Q(eig.GetEigenVectors());
537 const TVectorD& evs(eig.GetEigenValues());
541 for (; iCol < Q.GetNcols(); ++iCol) {
542 double ev = evs(iCol) > 0 ? sqrt(evs(iCol)) : 0;
545 for (
int j = 0; j < Q.GetNrows(); ++j)
548 if (iCol < Q.GetNrows()) {
550 Q.ResizeTo(iCol, Q.GetNcols());
556 TDecompChol oldCovDecomp(oldCov);
557 oldCovDecomp.Decompose();
558 const TMatrixD& S(oldCovDecomp.GetU());
560 TDecompChol decompR(V);
563 TMatrixD pre(S.GetNrows() + Q.GetNrows() + V.GetNrows(),
564 S.GetNcols() + V.GetNcols());
565 TMatrixD SFt(S, TMatrixD::kMultTranspose, F);
566 pre.SetSub( 0, 0,decompR.GetU());
567 pre.SetSub( V.GetNrows(), 0,H->MHt(SFt)); pre.SetSub(V.GetNrows(), V.GetNcols(),SFt);
568 if (Q.GetNrows() > 0) {
569 pre.SetSub(S.GetNrows()+V.GetNrows(),0,H->MHt(Q)); pre.SetSub(S.GetNrows()+V.GetNrows(),V.GetNcols(), Q);
573 const TMatrixD& r = pre;
575 TMatrixD R(r.GetSub(0, V.GetNrows()-1, 0, V.GetNcols()-1));
577 TMatrixD K(TMatrixD::kTransposed, r.GetSub(0, V.GetNrows()-1, V.GetNcols(), pre.GetNcols()-1));
580 TMatrixD Snew(r.GetSub(V.GetNrows(), V.GetNrows() + S.GetNrows() - 1,
581 V.GetNcols(), pre.GetNcols()-1));
584 TVectorD update(res);
587 stateVector += update;
588 cov = TMatrixDSym(TMatrixDSym::kAtA, Snew);
594 TMatrixDSym covSumInv(cov);
599 TMatrixD CHt(H->MHt(cov));
600 TVectorD update(TMatrixD(CHt, TMatrixD::kMult, covSumInv) * res);
606 std::cout <<
"\033[32m";
607 std::cout <<
"Update: "; update.Print();
608 std::cout <<
"\033[0m";
612 stateVector += update;
613 covSumInv.Similarity(CHt);
618 std::cout <<
"\033[32m";
619 std::cout <<
"updated state: "; stateVector.Print();
620 std::cout <<
"updated cov: "; cov.Print();
623 TVectorD resNew(measurement - H->Hv(stateVector));
625 std::cout <<
"Residual New = (" << resNew(0);
627 if (resNew.GetNrows() > 1)
628 std::cout <<
", " << resNew(1);
629 std::cout <<
")" << std::endl;
630 std::cout <<
"\033[0m";
643 TMatrixDSym HCHt(cov);
650 chi2inc += HCHt.Similarity(resNew);
653 ndfInc += weight * measurement.GetNrows();
656 ndfInc += measurement.GetNrows();
659 std::cout <<
"chi² increment = " << chi2inc << std::endl;
676void KalmanFitter::Streamer(TBuffer &R__b)
683 if (R__b.IsReading()) {
684 Version_t R__v = R__b.ReadVersion(&R__s, &R__c);
if (R__v) { }
687 baseClass0::Streamer(R__b);
691 R__b.CheckByteCount(R__s, R__c, thisClass::IsA());
693 R__c = R__b.WriteVersion(thisClass::IsA(), kTRUE);
696 baseClass0::Streamer(R__b);
698 R__b.SetByteCount(R__c, kTRUE);
const SharedPlanePtr & getPlane() const
HMatrix for projecting from AbsTrackRep parameters to measured parameters in a DetPlane.
Abstract base class for Kalman fitter and derived fitting algorithms.
double blowUpFactor_
Blow up the covariance of the forward (backward) fit by this factor before seeding the backward (forw...
unsigned int maxIterations_
Maximum number of iterations to attempt. Forward and backward are counted as one iteration.
bool canIgnoreWeights() const
returns if the fitter can ignore the weights and handle the MeasurementOnPlanes as if they had weight...
const std::vector< MeasurementOnPlane * > getMeasurements(const KalmanFitterInfo *fi, const TrackPoint *tp, int direction) const
get the measurementsOnPlane taking the multipleMeasurementHandling_ into account
double deltaPval_
Convergence criterion.
unsigned int minIterations_
Minimum number of iterations to attempt. Forward and backward are counted as one iteration.
eMultipleMeasurementHandling multipleMeasurementHandling_
How to handle if there are multiple MeasurementsOnPlane.
Abstract base class for a track representation.
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,...
virtual double getQop(const StateOnPlane &state) const =0
Get charge over momentum.
virtual void setPosMomCov(MeasuredStateOnPlane &state, const TVector3 &pos, const TVector3 &mom, const TMatrixDSym &cov6x6) const =0
Set position, momentum and covariance of state.
virtual void getForwardJacobianAndNoise(TMatrixD &jacobian, TMatrixDSym &noise, TVectorD &deltaState) const =0
Get the jacobian and noise matrix of the last extrapolation.
virtual void setQop(StateOnPlane &state, double qop) const =0
Set charge/momentum.
virtual unsigned int getDim() const =0
Get the dimension of the state vector used by the track representation.
virtual void getPosMomCov(const MeasuredStateOnPlane &state, TVector3 &pos, TVector3 &mom, TMatrixDSym &cov) const =0
Translates MeasuredStateOnPlane into 3D position, momentum and 6x6 covariance.
Exception class for error handling in GENFIT (provides storage for diagnostic information)
void setFatal(bool b=true)
Set fatal flag.
virtual const char * what() const
Standard error message handling for exceptions. use like "std::cerr << e.what();".
bool isTrackPruned() const
Has the track been pruned after the fit?
FitStatus for use with AbsKalmanFitter implementations.
MeasuredStateOnPlane with additional info produced by a Kalman filter or DAF.
Collects information needed and produced by a AbsKalmanFitter implementations and is specific to one ...
bool hasPredictionsAndUpdates() const
const MeasuredStateOnPlane & getFittedState(bool biased=true) const
Get unbiased or biased (default) smoothed state.
virtual bool checkConsistency() const
KalmanFittedStateOnPlane * getUpdate(int direction) const
void setWeights(const std::vector< double > &)
Set weights of measurements.
void addMeasurementsOnPlane(const std::vector< genfit::MeasurementOnPlane * > &measurementsOnPlane)
void deleteBackwardInfo()
std::vector< double > getWeights() const
Get weights of measurements.
void setPrediction(MeasuredStateOnPlane *prediction, int direction)
unsigned int getNumMeasurements() const
bool hasBackwardUpdate() const
void fixWeights(bool arg=true)
bool hasUpdate(int direction) const
bool areWeightsFixed() const
Are the weights fixed?
void setUpdate(KalmanFittedStateOnPlane *update, int direction)
void deleteMeasurementInfo()
Simple Kalman filter implementation.
void processTrackPartially(Track *tr, const AbsTrackRep *rep, int startId=0, int endId=-1)
void processTrackWithRep(Track *tr, const AbsTrackRep *rep, bool resortHits=false)
Hit resorting currently NOT supported.
bool squareRootFormalism_
boost::scoped_ptr< MeasuredStateOnPlane > currentState_
bool fitTrack(Track *tr, const AbsTrackRep *rep, double &chi2, double &ndf, int startId, int endId, int &nFailedHits)
void processTrackPoint(TrackPoint *tp, const AbsTrackRep *rep, double &chi2, double &ndf, int direction)
StateOnPlane with additional covariance matrix.
const TMatrixDSym & getCov() const
Measured coordinates on a plane.
const AbsHMatrix * getHMatrix() const
const TVectorD & getState() const
const TVectorD & getAuxInfo() const
Object containing AbsMeasurement and AbsFitterInfo objects.
bool hasFitterInfo(const AbsTrackRep *rep) const
void deleteFitterInfo(const AbsTrackRep *rep)
AbsFitterInfo * getFitterInfo(const AbsTrackRep *rep=NULL) const
Get fitterInfo for rep. Per default, use cardinal rep.
Collection of TrackPoint objects, AbsTrackRep objects and FitStatus objects.
TrackPoint * getPoint(int id) const
const TVectorD & getStateSeed() const
TrackPoint * getPointWithMeasurement(int id) const
unsigned int getNumPointsWithMeasurement() const
const TMatrixDSym & getCovSeed() const
FitStatus * getFitStatus(const AbsTrackRep *rep=NULL) const
Get FitStatus for a AbsTrackRep. Per default, return FitStatus for cardinalRep.
AbsTrackRep * getCardinalRep() const
Get cardinal track representation.
TrackPoint * getPointWithMeasurementAndFitterInfo(int id, const AbsTrackRep *rep) const
void setFitStatus(FitStatus *fitStatus, const AbsTrackRep *rep)
@ weightedClosestToReference
@ weightedClosestToReferenceWire
@ unweightedClosestToReference
@ unweightedClosestToReferenceWire
boost::shared_ptr< genfit::DetPlane > SharedPlanePtr
Shared Pointer to a DetPlane.