TwoBodyDecay/src/TwoBodyDecayEstimator.cc

0001
0002 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0003 #include "MagneticField/Engine/interface/MagneticField.h"
0004
0005 #include "Alignment/TwoBodyDecay/interface/TwoBodyDecayDerivatives.h"
0006 #include "Alignment/TwoBodyDecay/interface/TwoBodyDecayEstimator.h"
0007 #include "Alignment/TwoBodyDecay/interface/TwoBodyDecayModel.h"
0008 #include "FWCore/Utilities/interface/isFinite.h"
0009 //#include "DataFormats/CLHEP/interface/Migration.h"
0010
0011 TwoBodyDecayEstimator::TwoBodyDecayEstimator(const edm::ParameterSet &config) : theNdf(0) {
0012   const edm::ParameterSet &estimatorConfig = config.getParameter<edm::ParameterSet>("EstimatorParameters");
0013
0014   theRobustificationConstant = estimatorConfig.getUntrackedParameter<double>("RobustificationConstant", 1.0);
0015   theMaxIterDiff = estimatorConfig.getUntrackedParameter<double>("MaxIterationDifference", 1e-2);
0016   theMaxIterations = estimatorConfig.getUntrackedParameter<int>("MaxIterations", 100);
0017   theUseInvariantMass = estimatorConfig.getUntrackedParameter<bool>("UseInvariantMass", true);
0018 }
0019
0020 TwoBodyDecay TwoBodyDecayEstimator::estimate(const std::vector<RefCountedLinearizedTrackState> &linTracks,
0021                                              const TwoBodyDecayParameters &linearizationPoint,
0022                                              const TwoBodyDecayVirtualMeasurement &vm) const {
0023   if (linTracks.size() != 2) {
0024     edm::LogInfo("Alignment") << "@SUB=TwoBodyDecayEstimator::estimate"
0025                               << "Need 2 linearized tracks, got " << linTracks.size() << ".\n";
0026     return TwoBodyDecay();
0027   }
0028
0029   AlgebraicVector vecM;
0030   AlgebraicSymMatrix matG;
0031   AlgebraicMatrix matA;
0032
0033   bool check = constructMatrices(linTracks, linearizationPoint, vm, vecM, matG, matA);
0034   if (!check)
0035     return TwoBodyDecay();
0036
0037   AlgebraicSymMatrix matGPrime;
0038   AlgebraicSymMatrix invAtGPrimeA;
0039   AlgebraicVector vecEstimate;
0040   AlgebraicVector res;
0041
0042   int nIterations = 0;
0043   bool stopIteration = false;
0044
0045   // initialization - values are never used
0046   int checkInversion = 0;
0047   double chi2 = 0.;
0048   double oldChi2 = 0.;
0049   bool isValid = true;
0050
0051   while (!stopIteration) {
0052     matGPrime = matG;
0053
0054     // compute weights
0055     if (nIterations > 0) {
0056       for (int i = 0; i < 10; i++) {
0057         double sigma = 1. / sqrt(matG[i][i]);
0058         double sigmaTimesR = sigma * theRobustificationConstant;
0059         double absRes = fabs(res[i]);
0060         if (absRes > sigmaTimesR)
0061           matGPrime[i][i] *= sigmaTimesR / absRes;
0062       }
0063     }
0064
0065     // make LS-fit
0066     invAtGPrimeA = (matGPrime.similarityT(matA)).inverse(checkInversion);
0067     if (checkInversion != 0) {
0068       LogDebug("Alignment") << "@SUB=TwoBodyDecayEstimator::estimate"
0069                             << "Matrix At*G'*A not invertible (in iteration " << nIterations
0070                             << ", ifail = " << checkInversion << ").\n";
0071       isValid = false;
0072       break;
0073     }
0074     vecEstimate = invAtGPrimeA * matA.T() * matGPrime * vecM;
0075     res = matA * vecEstimate - vecM;
0076     chi2 = dot(res, matGPrime * res);
0077
0078     if ((nIterations > 0) && (fabs(chi2 - oldChi2) < theMaxIterDiff))
0079       stopIteration = true;
0080     if (nIterations == theMaxIterations)
0081       stopIteration = true;
0082
0083     oldChi2 = chi2;
0084     nIterations++;
0085   }
0086
0087   if (isValid) {
0088     AlgebraicSymMatrix pullsCov = matGPrime.inverse(checkInversion) - invAtGPrimeA.similarity(matA);
0089     thePulls = AlgebraicVector(matG.num_col(), 0);
0090     for (int i = 0; i < pullsCov.num_col(); i++)
0091       thePulls[i] = res[i] / sqrt(pullsCov[i][i]);
0092   }
0093
0094   theNdf = matA.num_row() - matA.num_col();
0095
0096   return TwoBodyDecay(TwoBodyDecayParameters(vecEstimate, invAtGPrimeA), chi2, isValid, vm);
0097 }
0098
0099 bool TwoBodyDecayEstimator::constructMatrices(const std::vector<RefCountedLinearizedTrackState> &linTracks,
0100                                               const TwoBodyDecayParameters &linearizationPoint,
0101                                               const TwoBodyDecayVirtualMeasurement &vm,
0102                                               AlgebraicVector &vecM,
0103                                               AlgebraicSymMatrix &matG,
0104                                               AlgebraicMatrix &matA) const {
0105   PerigeeLinearizedTrackState *linTrack1 = dynamic_cast<PerigeeLinearizedTrackState *>(linTracks[0].get());
0106   PerigeeLinearizedTrackState *linTrack2 = dynamic_cast<PerigeeLinearizedTrackState *>(linTracks[1].get());
0107
0108   if (!linTrack1 || !linTrack2)
0109     return false;
0110
0111   AlgebraicVector trackParam1 = asHepVector(linTrack1->predictedStateParameters());
0112   AlgebraicVector trackParam2 = asHepVector(linTrack2->predictedStateParameters());
0113
0114   if (checkValues(trackParam1) || checkValues(trackParam2) || checkValues(linearizationPoint.parameters()))
0115     return false;
0116
0117   AlgebraicVector vecLinParam = linearizationPoint.sub(TwoBodyDecayParameters::px, TwoBodyDecayParameters::mass);
0118
0119   double zMagField = linTrack1->track().field()->inInverseGeV(linTrack1->linearizationPoint()).z();
0120
0121   int checkInversion = 0;
0122
0123   TwoBodyDecayDerivatives tpeDerivatives(linearizationPoint[TwoBodyDecayParameters::mass], vm.secondaryMass());
0124   std::pair<AlgebraicMatrix, AlgebraicMatrix> derivatives = tpeDerivatives.derivatives(linearizationPoint);
0125
0126   TwoBodyDecayModel decayModel(linearizationPoint[TwoBodyDecayParameters::mass], vm.secondaryMass());
0127   std::pair<AlgebraicVector, AlgebraicVector> linCartMomenta = decayModel.cartesianSecondaryMomenta(linearizationPoint);
0128
0129   // first track
0130   AlgebraicMatrix matA1 = asHepMatrix(linTrack1->positionJacobian());
0131   AlgebraicMatrix matB1 = asHepMatrix(linTrack1->momentumJacobian());
0132   AlgebraicVector vecC1 = asHepVector(linTrack1->constantTerm());
0133
0134   AlgebraicVector curvMomentum1 = asHepVector(linTrack1->predictedStateMomentumParameters());
0135   AlgebraicMatrix curv2cart1 = decayModel.curvilinearToCartesianJacobian(curvMomentum1, zMagField);
0136
0137   AlgebraicVector cartMomentum1 = decayModel.convertCurvilinearToCartesian(curvMomentum1, zMagField);
0138   vecC1 += matB1 * (curvMomentum1 - curv2cart1 * cartMomentum1);
0139   matB1 = matB1 * curv2cart1;
0140
0141   AlgebraicMatrix matF1 = derivatives.first;
0142   AlgebraicVector vecD1 = linCartMomenta.first - matF1 * vecLinParam;
0143   AlgebraicVector vecM1 = trackParam1 - vecC1 - matB1 * vecD1;
0144   AlgebraicSymMatrix covM1 = asHepMatrix(linTrack1->predictedStateError());
0145
0146   AlgebraicSymMatrix matG1 = covM1.inverse(checkInversion);
0147   if (checkInversion != 0) {
0148     LogDebug("Alignment") << "@SUB=TwoBodyDecayEstimator::constructMatrices"
0149                           << "Matrix covM1 not invertible.";
0150     return false;
0151   }
0152
0153   // diagonalize for robustification
0154   AlgebraicMatrix matU1 = diagonalize(&matG1).T();
0155
0156   // second track
0157   AlgebraicMatrix matA2 = asHepMatrix(linTrack2->positionJacobian());
0158   AlgebraicMatrix matB2 = asHepMatrix(linTrack2->momentumJacobian());
0159   AlgebraicVector vecC2 = asHepVector(linTrack2->constantTerm());
0160
0161   AlgebraicVector curvMomentum2 = asHepVector(linTrack2->predictedStateMomentumParameters());
0162   AlgebraicMatrix curv2cart2 = decayModel.curvilinearToCartesianJacobian(curvMomentum2, zMagField);
0163
0164   AlgebraicVector cartMomentum2 = decayModel.convertCurvilinearToCartesian(curvMomentum2, zMagField);
0165   vecC2 += matB2 * (curvMomentum2 - curv2cart2 * cartMomentum2);
0166   matB2 = matB2 * curv2cart2;
0167
0168   AlgebraicMatrix matF2 = derivatives.second;
0169   AlgebraicVector vecD2 = linCartMomenta.second - matF2 * vecLinParam;
0170   AlgebraicVector vecM2 = trackParam2 - vecC2 - matB2 * vecD2;
0171   AlgebraicSymMatrix covM2 = asHepMatrix(linTrack2->predictedStateError());
0172
0173   AlgebraicSymMatrix matG2 = covM2.inverse(checkInversion);
0174   if (checkInversion != 0) {
0175     LogDebug("Alignment") << "@SUB=TwoBodyDecayEstimator::constructMatrices"
0176                           << "Matrix covM2 not invertible.";
0177     return false;
0178   }
0179
0180   // diagonalize for robustification
0181   AlgebraicMatrix matU2 = diagonalize(&matG2).T();
0182
0183   // combine first and second track
0184   vecM = AlgebraicVector(14, 0);
0185   vecM.sub(1, matU1 * vecM1);
0186   vecM.sub(6, matU2 * vecM2);
0187   // virtual measurement of the primary mass
0188   vecM(11) = vm.primaryMass();
0189   // virtual measurement of the beam spot
0190   vecM.sub(12, vm.beamSpotPosition());
0191
0192   // full weight matrix
0193   matG = AlgebraicSymMatrix(14, 0);
0194   matG.sub(1, matG1);
0195   matG.sub(6, matG2);
0196   // virtual measurement error of the primary mass
0197   matG[10][10] = 1. / (vm.primaryWidth() * vm.primaryWidth());
0198   // virtual measurement error of the beam spot
0199   matG.sub(12, vm.beamSpotError().inverse(checkInversion));
0200
0201   // full derivative matrix
0202   matA = AlgebraicMatrix(14, 9, 0);
0203   matA.sub(1, 1, matU1 * matA1);
0204   matA.sub(6, 1, matU2 * matA2);
0205   matA.sub(1, 4, matU1 * matB1 * matF1);
0206   matA.sub(6, 4, matU2 * matB2 * matF2);
0207   matA(11, 9) = 1.;  // mass
0208   matA(12, 1) = 1.;  // vx
0209   matA(13, 2) = 1.;  // vy
0210   matA(14, 3) = 1.;  // vz
0211
0212   return true;
0213 }
0214
0215 bool TwoBodyDecayEstimator::checkValues(const AlgebraicVector &vec) const {
0216   bool isNotFinite = false;
0217
0218   for (int i = 0; i < vec.num_col(); ++i)
0219     isNotFinite |= edm::isNotFinite(vec[i]);
0220
0221   return isNotFinite;
0222 }