PatternTools/src/CollinearFitAtTM.cc

0001 #include "TrackingTools/PatternTools/interface/CollinearFitAtTM.h"
0002 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0003
0004 CollinearFitAtTM2::CollinearFitAtTM2(const TrajectoryMeasurement& tm) : valid_(false), chi2_(-1), ndof_(-1) {
0005   //
0006   // check input
0007   //
0008   if (!tm.forwardPredictedState().isValid() || !tm.backwardPredictedState().isValid() || !tm.updatedState().isValid()) {
0009     edm::LogWarning("CollinearFitAtTM2") << "Invalid state in TrajectoryMeasurement";
0010     return;
0011   }
0012   //
0013   // prepare fit
0014   //
0015   initJacobian();
0016   AlgebraicVector5 fwdPar = tm.forwardPredictedState().localParameters().vector();
0017   AlgebraicSymMatrix55 fwdCov = tm.forwardPredictedState().localError().matrix();
0018   AlgebraicVector5 bwdPar = tm.backwardPredictedState().localParameters().vector();
0019   AlgebraicSymMatrix55 bwdCov = tm.backwardPredictedState().localError().matrix();
0020
0021   LocalPoint hitPos(0., 0., 0.);
0022   LocalError hitErr(-1., -1., -1.);
0023   if (tm.recHit()->isValid()) {
0024     hitPos = tm.recHit()->localPosition();
0025     hitErr = tm.recHit()->localPositionError();
0026   }
0027
0028   fit(fwdPar, fwdCov, bwdPar, bwdCov, hitPos, hitErr);
0029 }
0030
0031 CollinearFitAtTM2::CollinearFitAtTM2(const AlgebraicVector5& fwdParameters,
0032                                      const AlgebraicSymMatrix55& fwdCovariance,
0033                                      const AlgebraicVector5& bwdParameters,
0034                                      const AlgebraicSymMatrix55& bwdCovariance,
0035                                      const LocalPoint& hitPosition,
0036                                      const LocalError& hitErrors)
0037     : valid_(false), chi2_(-1), ndof_(-1) {
0038   //
0039   // prepare fit
0040   //
0041   initJacobian();
0042
0043   fit(fwdParameters, fwdCovariance, bwdParameters, bwdCovariance, hitPosition, hitErrors);
0044 }
0045
0046 void CollinearFitAtTM2::initJacobian() {
0047   //
0048   // Jacobian
0049   //
0050   for (int i = 0; i < 12; ++i) {
0051     for (int j = 0; j < 6; ++j)
0052       jacobian_(i, j) = 0;
0053   }
0054   for (int i = 1; i < 5; ++i) {
0055     jacobian_(i, ParQpOut + i) = jacobian_(i + 5, ParQpOut + i) = 1;
0056   }
0057   jacobian_(0, ParQpIn) = 1.;
0058   jacobian_(5, ParQpOut) = 1.;
0059 }
0060
0061 bool CollinearFitAtTM2::fit(const AlgebraicVector5& fwdParameters,
0062                             const AlgebraicSymMatrix55& fwdCovariance,
0063                             const AlgebraicVector5& bwdParameters,
0064                             const AlgebraicSymMatrix55& bwdCovariance,
0065                             const LocalPoint& hitPos,
0066                             const LocalError& hitErr) {
0067   if (hitErr.xx() > 0)
0068     jacobian_(10, ParX) = jacobian_(11, ParY) = 1;
0069   else
0070     jacobian_(10, ParX) = jacobian_(11, ParY) = 0;
0071
0072   for (int i = 0; i < 12; ++i) {
0073     for (int j = 0; j < 12; ++j)
0074       weightMatrix_(i, j) = 0;
0075   }
0076
0077   for (int i = 0; i < 5; ++i)
0078     measurements_(i) = fwdParameters(i);
0079   weightMatrix_.Place_at(fwdCovariance, 0, 0);
0080   for (int i = 0; i < 5; ++i)
0081     measurements_(i + 5) = bwdParameters(i);
0082   weightMatrix_.Place_at(bwdCovariance, 5, 5);
0083   if (hitErr.xx() > 0) {
0084     measurements_(10) = hitPos.x();
0085     measurements_(11) = hitPos.y();
0086     weightMatrix_(10, 10) = hitErr.xx();
0087     weightMatrix_(10, 11) = weightMatrix_(11, 10) = hitErr.xy();
0088     weightMatrix_(11, 11) = hitErr.yy();
0089   } else {
0090     measurements_(10) = measurements_(11) = 0.;
0091     weightMatrix_(10, 10) = weightMatrix_(11, 11) = 1.;
0092     weightMatrix_(10, 11) = weightMatrix_(11, 10) = 0.;
0093   }
0094   //
0095   // invert covariance matrix
0096   //
0097   if (!weightMatrix_.Invert()) {
0098     edm::LogWarning("CollinearFitAtTM2") << "Inversion of input covariance matrix failed";
0099     return false;
0100   }
0101
0102   //
0103   projectedMeasurements_ = ROOT::Math::Transpose(jacobian_) * (weightMatrix_ * measurements_);
0104   //
0105   // Fitted parameters and covariance matrix
0106   //
0107   covariance_ = ROOT::Math::SimilarityT(jacobian_, weightMatrix_);
0108   if (!covariance_.Invert()) {
0109     edm::LogWarning("CollinearFitAtTM2") << "Inversion of resulting weight matrix failed";
0110     return false;
0111   }
0112
0113   parameters_ = covariance_ * projectedMeasurements_;
0114
0115   //
0116   // chi2
0117   //
0118   chi2_ = ROOT::Math::Similarity(measurements_, weightMatrix_) -
0119           ROOT::Math::Similarity(projectedMeasurements_, covariance_);
0120   ndof_ = hitErr.xx() > 0 ? 6 : 4;
0121
0122   valid_ = true;
0123   return true;
0124 }
0125
0126 Measurement1D CollinearFitAtTM2::deltaP() const {
0127   //
0128   // check validity
0129   //
0130   if (!valid_)
0131     return Measurement1D();
0132   //
0133   // deltaP = 1/qpout - 1/qpin ; uncertainty from linear error propagation
0134   //
0135   double qpIn = parameters_(0);
0136   double sig2In = covariance_(0, 0);
0137   double qpOut = parameters_(1);
0138   double sig2Out = covariance_(1, 1);
0139   double corrInOut = covariance_(0, 1);
0140   double pIn = 1. / fabs(qpIn);
0141   double pOut = 1. / fabs(qpOut);
0142   double sig2DeltaP = pIn / qpIn * pIn / qpIn * sig2In - 2 * pIn / qpIn * pOut / qpOut * corrInOut +
0143                       pOut / qpOut * pOut / qpOut * sig2Out;
0144
0145   return Measurement1D(pOut - pIn, sig2DeltaP ? sqrt(sig2DeltaP) : 0.);
0146 }