| 1 |
|
| 2 |
#include "FWCore/MessageLogger/interface/MessageLogger.h"
|
| 3 |
#include "MagneticField/Engine/interface/MagneticField.h"
|
| 4 |
|
| 5 |
#include "Alignment/TwoBodyDecay/interface/TwoBodyDecayEstimator.h"
|
| 6 |
#include "Alignment/TwoBodyDecay/interface/TwoBodyDecayModel.h"
|
| 7 |
#include "Alignment/TwoBodyDecay/interface/TwoBodyDecayDerivatives.h"
|
| 8 |
//#include "DataFormats/CLHEP/interface/Migration.h"
|
| 9 |
|
| 10 |
TwoBodyDecayEstimator::TwoBodyDecayEstimator( const edm::ParameterSet & config )
|
| 11 |
{
|
| 12 |
const edm::ParameterSet & estimatorConfig = config.getParameter< edm::ParameterSet >( "EstimatorParameters" );
|
| 13 |
|
| 14 |
theRobustificationConstant = estimatorConfig.getUntrackedParameter< double >( "RobustificationConstant", 1.0 );
|
| 15 |
theMaxIterDiff = estimatorConfig.getUntrackedParameter< double >( "MaxIterationDifference", 1e-2 );
|
| 16 |
theMaxIterations = estimatorConfig.getUntrackedParameter< int >( "MaxIterations", 100 );
|
| 17 |
theUseInvariantMass = estimatorConfig.getUntrackedParameter< bool >( "UseInvariantMass", true );
|
| 18 |
}
|
| 19 |
|
| 20 |
|
| 21 |
TwoBodyDecay TwoBodyDecayEstimator::estimate( const std::vector< RefCountedLinearizedTrackState > & linTracks,
|
| 22 |
const TwoBodyDecayParameters & linearizationPoint,
|
| 23 |
const TwoBodyDecayVirtualMeasurement & vm ) const
|
| 24 |
{
|
| 25 |
if ( linTracks.size() != 2 )
|
| 26 |
{
|
| 27 |
edm::LogInfo( "Alignment" ) << "@SUB=TwoBodyDecayEstimator::estimate"
|
| 28 |
<< "Need 2 linearized tracks, got " << linTracks.size() << ".\n";
|
| 29 |
return TwoBodyDecay();
|
| 30 |
}
|
| 31 |
|
| 32 |
AlgebraicVector vecM;
|
| 33 |
AlgebraicSymMatrix matG;
|
| 34 |
AlgebraicMatrix matA;
|
| 35 |
|
| 36 |
bool check = constructMatrices( linTracks, linearizationPoint, vm, vecM, matG, matA );
|
| 37 |
if ( !check ) return TwoBodyDecay();
|
| 38 |
|
| 39 |
AlgebraicSymMatrix matGPrime;
|
| 40 |
AlgebraicSymMatrix invAtGPrimeA;
|
| 41 |
AlgebraicVector vecEstimate;
|
| 42 |
AlgebraicVector res;
|
| 43 |
|
| 44 |
int nIterations = 0;
|
| 45 |
bool stopIteration = false;
|
| 46 |
|
| 47 |
// initialization - values are never used
|
| 48 |
int checkInversion = 0;
|
| 49 |
double chi2 = 0.;
|
| 50 |
double oldChi2 = 0.;
|
| 51 |
bool isValid = true;
|
| 52 |
|
| 53 |
while( !stopIteration )
|
| 54 |
{
|
| 55 |
matGPrime = matG;
|
| 56 |
|
| 57 |
// compute weights
|
| 58 |
if ( nIterations > 0 )
|
| 59 |
{
|
| 60 |
for ( int i = 0; i < 10; i++ )
|
| 61 |
{
|
| 62 |
double sigma = 1./sqrt( matG[i][i] );
|
| 63 |
double sigmaTimesR = sigma*theRobustificationConstant;
|
| 64 |
double absRes = fabs( res[i] );
|
| 65 |
if ( absRes > sigmaTimesR ) matGPrime[i][i] *= sigmaTimesR/absRes;
|
| 66 |
}
|
| 67 |
}
|
| 68 |
|
| 69 |
// make LS-fit
|
| 70 |
invAtGPrimeA = ( matGPrime.similarityT(matA) ).inverse( checkInversion );
|
| 71 |
if ( checkInversion != 0 )
|
| 72 |
{
|
| 73 |
LogDebug( "Alignment" ) << "@SUB=TwoBodyDecayEstimator::estimate"
|
| 74 |
<< "Matrix At*G'*A not invertible (in iteration " << nIterations
|
| 75 |
<< ", ifail = " << checkInversion << ").\n";
|
| 76 |
isValid = false;
|
| 77 |
break;
|
| 78 |
}
|
| 79 |
vecEstimate = invAtGPrimeA*matA.T()*matGPrime*vecM;
|
| 80 |
res = matA*vecEstimate - vecM;
|
| 81 |
chi2 = dot( res, matGPrime*res );
|
| 82 |
|
| 83 |
if ( ( nIterations > 0 ) && ( fabs( chi2 - oldChi2 ) < theMaxIterDiff ) ) stopIteration = true;
|
| 84 |
if ( nIterations == theMaxIterations ) stopIteration = true;
|
| 85 |
|
| 86 |
oldChi2 = chi2;
|
| 87 |
nIterations++;
|
| 88 |
}
|
| 89 |
|
| 90 |
if ( isValid )
|
| 91 |
{
|
| 92 |
AlgebraicSymMatrix pullsCov = matGPrime.inverse( checkInversion ) - invAtGPrimeA.similarity( matA );
|
| 93 |
thePulls = AlgebraicVector( matG.num_col(), 0 );
|
| 94 |
for ( int i = 0; i < pullsCov.num_col(); i++ ) thePulls[i] = res[i]/sqrt( pullsCov[i][i] );
|
| 95 |
}
|
| 96 |
|
| 97 |
theNdf = matA.num_row() - matA.num_col();
|
| 98 |
|
| 99 |
return TwoBodyDecay( TwoBodyDecayParameters( vecEstimate, invAtGPrimeA ), chi2, isValid, vm );
|
| 100 |
}
|
| 101 |
|
| 102 |
|
| 103 |
bool TwoBodyDecayEstimator::constructMatrices( const std::vector< RefCountedLinearizedTrackState > & linTracks,
|
| 104 |
const TwoBodyDecayParameters & linearizationPoint,
|
| 105 |
const TwoBodyDecayVirtualMeasurement & vm,
|
| 106 |
AlgebraicVector & vecM, AlgebraicSymMatrix & matG, AlgebraicMatrix & matA ) const
|
| 107 |
{
|
| 108 |
|
| 109 |
PerigeeLinearizedTrackState* linTrack1 = dynamic_cast<PerigeeLinearizedTrackState*>( linTracks[0].get() );
|
| 110 |
PerigeeLinearizedTrackState* linTrack2 = dynamic_cast<PerigeeLinearizedTrackState*>( linTracks[1].get() );
|
| 111 |
|
| 112 |
AlgebraicVector trackParam1 = asHepVector( linTrack1->predictedStateParameters() );
|
| 113 |
AlgebraicVector trackParam2 = asHepVector( linTrack2->predictedStateParameters() );
|
| 114 |
|
| 115 |
if ( checkValues( trackParam1 ) || checkValues( trackParam2 ) || checkValues( linearizationPoint.parameters() ) ) return false;
|
| 116 |
|
| 117 |
AlgebraicVector vecLinParam = linearizationPoint.sub( TwoBodyDecayParameters::px,
|
| 118 |
TwoBodyDecayParameters::mass );
|
| 119 |
|
| 120 |
double zMagField = linTrack1->track().field()->inInverseGeV( linTrack1->linearizationPoint() ).z();
|
| 121 |
|
| 122 |
int checkInversion = 0;
|
| 123 |
|
| 124 |
TwoBodyDecayDerivatives tpeDerivatives( linearizationPoint[TwoBodyDecayParameters::mass], vm.secondaryMass() );
|
| 125 |
std::pair< AlgebraicMatrix, AlgebraicMatrix > derivatives = tpeDerivatives.derivatives( linearizationPoint );
|
| 126 |
|
| 127 |
TwoBodyDecayModel decayModel( linearizationPoint[TwoBodyDecayParameters::mass], vm.secondaryMass() );
|
| 128 |
std::pair< AlgebraicVector, AlgebraicVector > linCartMomenta = decayModel.cartesianSecondaryMomenta( linearizationPoint );
|
| 129 |
|
| 130 |
// first track
|
| 131 |
AlgebraicMatrix matA1 = asHepMatrix( linTrack1->positionJacobian() );
|
| 132 |
AlgebraicMatrix matB1 = asHepMatrix( linTrack1->momentumJacobian() );
|
| 133 |
AlgebraicVector vecC1 = asHepVector( linTrack1->constantTerm() );
|
| 134 |
|
| 135 |
AlgebraicVector curvMomentum1 = asHepVector( linTrack1->predictedStateMomentumParameters() );
|
| 136 |
AlgebraicMatrix curv2cart1 = decayModel.curvilinearToCartesianJacobian( curvMomentum1, zMagField );
|
| 137 |
|
| 138 |
AlgebraicVector cartMomentum1 = decayModel.convertCurvilinearToCartesian( curvMomentum1, zMagField );
|
| 139 |
vecC1 += matB1*( curvMomentum1 - curv2cart1*cartMomentum1 );
|
| 140 |
matB1 = matB1*curv2cart1;
|
| 141 |
|
| 142 |
AlgebraicMatrix matF1 = derivatives.first;
|
| 143 |
AlgebraicVector vecD1 = linCartMomenta.first - matF1*vecLinParam;
|
| 144 |
AlgebraicVector vecM1 = trackParam1 - vecC1 - matB1*vecD1;
|
| 145 |
AlgebraicSymMatrix covM1 = asHepMatrix( linTrack1->predictedStateError() );
|
| 146 |
|
| 147 |
|
| 148 |
AlgebraicSymMatrix matG1 = covM1.inverse( checkInversion );
|
| 149 |
if ( checkInversion != 0 )
|
| 150 |
{
|
| 151 |
LogDebug( "Alignment" ) << "@SUB=TwoBodyDecayEstimator::constructMatrices"
|
| 152 |
<< "Matrix covM1 not invertible.";
|
| 153 |
return false;
|
| 154 |
}
|
| 155 |
|
| 156 |
// diagonalize for robustification
|
| 157 |
AlgebraicMatrix matU1 = diagonalize( &matG1 ).T();
|
| 158 |
|
| 159 |
// second track
|
| 160 |
AlgebraicMatrix matA2 = asHepMatrix( linTrack2->positionJacobian() );
|
| 161 |
AlgebraicMatrix matB2 = asHepMatrix( linTrack2->momentumJacobian() );
|
| 162 |
AlgebraicVector vecC2 = asHepVector( linTrack2->constantTerm() );
|
| 163 |
|
| 164 |
AlgebraicVector curvMomentum2 = asHepVector( linTrack2->predictedStateMomentumParameters() );
|
| 165 |
AlgebraicMatrix curv2cart2 = decayModel.curvilinearToCartesianJacobian( curvMomentum2, zMagField );
|
| 166 |
|
| 167 |
AlgebraicVector cartMomentum2 = decayModel.convertCurvilinearToCartesian( curvMomentum2, zMagField );
|
| 168 |
vecC2 += matB2*( curvMomentum2 - curv2cart2*cartMomentum2 );
|
| 169 |
matB2 = matB2*curv2cart2;
|
| 170 |
|
| 171 |
AlgebraicMatrix matF2 = derivatives.second;
|
| 172 |
AlgebraicVector vecD2 = linCartMomenta.second - matF2*vecLinParam;
|
| 173 |
AlgebraicVector vecM2 = trackParam2 - vecC2 - matB2*vecD2;
|
| 174 |
AlgebraicSymMatrix covM2 = asHepMatrix( linTrack2->predictedStateError() );
|
| 175 |
|
| 176 |
AlgebraicSymMatrix matG2 = covM2.inverse( checkInversion );
|
| 177 |
if ( checkInversion != 0 )
|
| 178 |
{
|
| 179 |
LogDebug( "Alignment" ) << "@SUB=TwoBodyDecayEstimator::constructMatrices"
|
| 180 |
<< "Matrix covM2 not invertible.";
|
| 181 |
return false;
|
| 182 |
}
|
| 183 |
|
| 184 |
// diagonalize for robustification
|
| 185 |
AlgebraicMatrix matU2 = diagonalize( &matG2 ).T();
|
| 186 |
|
| 187 |
// combine first and second track
|
| 188 |
vecM = AlgebraicVector( 14, 0 );
|
| 189 |
vecM.sub( 1, matU1*vecM1 );
|
| 190 |
vecM.sub( 6, matU2*vecM2 );
|
| 191 |
// virtual measurement of the primary mass
|
| 192 |
vecM( 11 ) = vm.primaryMass();
|
| 193 |
// virtual measurement of the beam spot
|
| 194 |
vecM.sub( 12, vm.beamSpotPosition() );
|
| 195 |
|
| 196 |
// full weight matrix
|
| 197 |
matG = AlgebraicSymMatrix( 14, 0 );
|
| 198 |
matG.sub( 1, matG1 );
|
| 199 |
matG.sub( 6, matG2 );
|
| 200 |
// virtual measurement error of the primary mass
|
| 201 |
matG[10][10] = 1./( vm.primaryWidth()*vm.primaryWidth() );
|
| 202 |
// virtual measurement error of the beam spot
|
| 203 |
matG.sub( 12, vm.beamSpotError().inverse( checkInversion ) );
|
| 204 |
|
| 205 |
// full derivative matrix
|
| 206 |
matA = AlgebraicMatrix( 14, 9, 0 );
|
| 207 |
matA.sub( 1, 1, matU1*matA1 );
|
| 208 |
matA.sub( 6, 1, matU2*matA2 );
|
| 209 |
matA.sub( 1, 4, matU1*matB1*matF1 );
|
| 210 |
matA.sub( 6, 4, matU2*matB2*matF2 );
|
| 211 |
matA( 11, 9 ) = 1.;//mass
|
| 212 |
matA( 12, 1 ) = 1.;//vx
|
| 213 |
matA( 13, 2 ) = 1.;//vy
|
| 214 |
matA( 14, 3 ) = 1.;//vz
|
| 215 |
|
| 216 |
return true;
|
| 217 |
}
|
| 218 |
|
| 219 |
|
| 220 |
bool TwoBodyDecayEstimator::checkValues( const AlgebraicVector & vec ) const
|
| 221 |
{
|
| 222 |
bool isNan = false;
|
| 223 |
bool isInf = false;
|
| 224 |
|
| 225 |
for ( int i = 0; i < vec.num_col(); ++i )
|
| 226 |
{
|
| 227 |
isNan = isNan || std::isnan( vec[i] );
|
| 228 |
isInf = isInf || std::isinf( vec[i] );
|
| 229 |
}
|
| 230 |
|
| 231 |
return ( isNan || isInf );
|
| 232 |
}
|
