CMS 3D CMS Logo

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Pages
CurrentAlignmentKFUpdator.cc
Go to the documentation of this file.
2 
6 
9 
10 
12  const TrackingRecHit & aRecHit ) const
13 {
14  switch (aRecHit.dimension()) {
15  case 1: return update<1>(tsos,aRecHit);
16  case 2: return update<2>(tsos,aRecHit);
17  case 3: return update<3>(tsos,aRecHit);
18  case 4: return update<4>(tsos,aRecHit);
19  case 5: return update<5>(tsos,aRecHit);
20  }
21  throw cms::Exception("Rec hit of invalid dimension (not 1,2,3,4,5)");
22 }
23 
24 
25 template <unsigned int D>
27  const TrackingRecHit & aRecHit ) const
28 {
29  //std::cout << "[CurrentAlignmentKFUpdator::update] Start Updating." << std::endl;
30  typedef typename AlgebraicROOTObject<D,5>::Matrix MatD5;
31  typedef typename AlgebraicROOTObject<5,D>::Matrix Mat5D;
32  typedef typename AlgebraicROOTObject<D,D>::SymMatrix SMatDD;
33  typedef typename AlgebraicROOTObject<D>::Vector VecD;
34 
35  double pzSign = tsos.localParameters().pzSign();
36 
37  MeasurementExtractor me( tsos );
38 
39  AlgebraicVector5 vecX( tsos.localParameters().vector() );
40  AlgebraicSymMatrix55 matC( tsos.localError().matrix() );
41  // Measurement matrix
42  MatD5 matH = asSMatrix<D,5>( aRecHit.projectionMatrix() );
43 
44  // Residuals of aPredictedState w.r.t. aRecHit,
45  VecD vecR = asSVector<D>(aRecHit.parameters()) - me.measuredParameters<D>( aRecHit );
46 
47  // and covariance matrix of residuals
48  SMatDD matV = asSMatrix<D>( aRecHit.parametersError() );
49 
50  // add information from current estimate on the misalignment
51  includeCurrentAlignmentEstimate<D>( aRecHit, tsos, vecR, matV );
52 
53  SMatDD matR( matV + me.measuredError<D>( aRecHit ) );
54 
55  int checkInversion = 0;
56  SMatDD invR = matR.Inverse( checkInversion );
57  if ( checkInversion != 0 )
58  {
59  std::cout << "[CurrentAlignmentKFUpdator::update] Inversion of matrix R failed." << std::endl;
60  return TrajectoryStateOnSurface();
61  }
62 
63  // Compute Kalman gain matrix
64  Mat5D matK = matC*ROOT::Math::Transpose(matH)*invR ;
65 
66  // Compute local filtered state vector
67  AlgebraicVector5 fsv( vecX + matK*vecR );
68 
69  // Compute covariance matrix of local filtered state vector
71  AlgebraicMatrix55 matM( matI - matK*matH );
72  AlgebraicSymMatrix55 fse( ROOT::Math::Similarity(matM, matC) + ROOT::Math::Similarity(matK, matV) );
73 
75  tsos.surface(),&( tsos.globalParameters().magneticField() ) );
76 }
77 
78 
79 template <unsigned int D>
81  const TrajectoryStateOnSurface & tsos,
82  typename AlgebraicROOTObject<D>::Vector & vecR,
83  typename AlgebraicROOTObject<D>::SymMatrix & matV ) const
84 {
85  const GeomDet* det = aRecHit.det();
86  if ( !det ) return;
87 
89  if ( alignableDet.isNull() )
90  {
91  //std::cout << "[CurrentAlignmentKFUpdator::includeCurrentAlignmentEstimate] No AlignableDet associated with RecHit." << std::endl;
92  return;
93  }
94 
95  AlignmentParameters const* alignmentParameters = getAlignmentParameters( alignableDet );
96 
97  if ( alignmentParameters )
98  {
99  AlgebraicMatrix selectedDerivatives = alignmentParameters->selectedDerivatives( tsos, alignableDet );
100  AlgebraicVector selectedParameters = alignmentParameters->selectedParameters();
101  AlgebraicSymMatrix selectedCovariance = alignmentParameters->selectedCovariance();
102 
103  AlgebraicSymMatrix deltaV = selectedCovariance.similarityT( selectedDerivatives );
104  AlgebraicVector deltaR = selectedDerivatives.T()*selectedParameters;
105 
106  //AlignmentUserVariables* auv = alignmentParameters->userVariables();
107  //if ( !auv ) std::cout << "[CurrentAlignmentKFUpdator::includeCurrentAlignmentEstimate] No AlignmentUserVariables associated with AlignableDet." << std::endl;
108  //if ( theAnnealing ) matV *= (*theAnnealing)( auv );
109 
110  if ( deltaR.num_row() == D )
111  {
112  vecR += asSVector<D>(deltaR);
113  matV += asSMatrix<D>(deltaV);
114  }
115  else std::cout << "[CurrentAlignmentKFUpdator::includeCurrentAlignmentEstimate] Predicted state and misalignment correction not compatible." << std::endl;
116  } else std::cout << "[CurrentAlignmentKFUpdator::includeCurrentAlignmentEstimate] No AlignmentParameters associated with AlignableDet." << std::endl;
117 
118  return;
119 }
120 
121 
123 {
124  // Get alignment parameters from AlignableDet ...
125  AlignmentParameters const* alignmentParameters = alignableDet->alignmentParameters();
126  // ... or any higher level alignable.
127  if ( !alignmentParameters ) alignmentParameters = getHigherLevelParameters( alignableDet );
128  return alignmentParameters;
129 }
130 
131 
133 {
134  Alignable* higherLevelAlignable = aAlignable->mother();
135  // Alignable has no mother ... most probably the alignable is already the full tracker.
136  if ( !higherLevelAlignable ) return 0;
137  AlignmentParameters* higherLevelParameters = higherLevelAlignable->alignmentParameters();
138  // Found alignment parameters? If not, go one level higher in the hierarchy.
139  return higherLevelParameters ? higherLevelParameters : getHigherLevelParameters( higherLevelAlignable );
140 }
virtual int dimension() const =0
double pzSign() const
Sign of the z-component of the momentum in the local frame.
ROOT::Math::SMatrix< double, D1, D1, ROOT::Math::MatRepSym< double, D1 > > SymMatrix
AlignableDetOrUnitPtr alignableFromGeomDet(const GeomDet *geomDet)
Returns AlignableDetOrUnitPtr corresponding to given GeomDet.
const LocalTrajectoryParameters & localParameters() const
AlgebraicVector selectedParameters(void) const
Get selected parameters.
AlignmentParameters const * getHigherLevelParameters(const Alignable *aAlignable) const
ROOT::Math::SMatrixIdentity AlgebraicMatrixID
TrajectoryStateOnSurface update(const TrajectoryStateOnSurface &, const TrackingRecHit &) const
ROOT::Math::SMatrix< double, 5, 5, ROOT::Math::MatRepSym< double, 5 > > AlgebraicSymMatrix55
bool isNull() const
check for empty pointer
void includeCurrentAlignmentEstimate(const TrackingRecHit &aRecHit, const TrajectoryStateOnSurface &tsos, typename AlgebraicROOTObject< D >::Vector &vecR, typename AlgebraicROOTObject< D >::SymMatrix &matV) const
AlignmentParameters * alignmentParameters() const
Get the AlignmentParameters.
Definition: Alignable.h:57
virtual AlgebraicVector parameters() const =0
AlgebraicVector5 vector() const
ROOT::Math::SMatrix< double, D1, D2, ROOT::Math::MatRepStd< double, D1, D2 > > Matrix
CLHEP::HepMatrix AlgebraicMatrix
const GeomDet * det() const
AlignableNavigator * theAlignableNavigator
const AlgebraicSymMatrix55 & matrix() const
const LocalTrajectoryError & localError() const
AlgebraicSymMatrix selectedCovariance(void) const
Get covariance matrix of selected parameters.
double deltaR(double eta1, double eta2, double phi1, double phi2)
Definition: TreeUtility.cc:17
CLHEP::HepVector AlgebraicVector
ROOT::Math::SVector< double, D1 > Vector
DecomposeProduct< arg, typename Div::arg > D
Definition: Factorize.h:150
ROOT::Math::SVector< double, 5 > AlgebraicVector5
virtual AlgebraicMatrix projectionMatrix() const =0
CLHEP::HepSymMatrix AlgebraicSymMatrix
tuple cout
Definition: gather_cfg.py:121
AlignmentParameters const * getAlignmentParameters(const AlignableDetOrUnitPtr &alignableDet) const
ROOT::Math::SMatrix< double, 5, 5, ROOT::Math::MatRepStd< double, 5, 5 > > AlgebraicMatrix55
Alignable * mother() const
Return pointer to container alignable (if any)
Definition: Alignable.h:85
virtual AlgebraicMatrix selectedDerivatives(const TrajectoryStateOnSurface &tsos, const AlignableDetOrUnitPtr &alidet) const
virtual AlgebraicSymMatrix parametersError() const =0