|
|
|
00001 #include "TrackingTools/KalmanUpdators/interface/KFStripUpdator.h" 00002 #include "TrackingTools/KalmanUpdators/interface/StripMeasurementTransformator.h" 00003 #include "DataFormats/Math/interface/invertPosDefMatrix.h" 00004 00005 00006 TrajectoryStateOnSurface 00007 KFStripUpdator::update(const TSOS& aTsos, const TransientTrackingRecHit& aHit) const { 00008 00009 double pzSign = aTsos.localParameters().pzSign(); 00010 00011 StripMeasurementTransformator myTrafo(aTsos, aHit); 00012 00013 AlgebraicMatrix25 H(myTrafo.projectionMatrix()); 00014 AlgebraicVector2 m(myTrafo.hitParameters()); 00015 AlgebraicVector5 x(myTrafo.trajectoryParameters()); 00016 AlgebraicVector2 px(myTrafo.projectedTrajectoryParameters()); 00017 // AlgebraicVector px = H*x; 00018 00019 AlgebraicSymMatrix22 V(myTrafo.hitError()); 00020 const AlgebraicSymMatrix55 &C = myTrafo.trajectoryError(); 00021 AlgebraicSymMatrix22 pC(myTrafo.projectedTrajectoryError()); 00022 // AlgebraicSymMatrix pC = C.similarity(H); 00023 00024 AlgebraicSymMatrix22 R(V + pC); 00025 //int ierr; R.invert(ierr); // if (ierr != 0) throw exception; 00026 invertPosDefMatrix(R); 00027 00028 // Compute Kalman gain matrix 00029 // AlgebraicMatrix Hm2l(myTrafo.measurement2LocalProj()); 00030 AlgebraicMatrix52 K(C * ROOT::Math::Transpose(H) * R); 00031 00032 // Compute local filtered state vector 00033 AlgebraicVector5 fsv(x + K * (m - px)); 00034 00035 // Compute covariance matrix of local filtered state vector 00036 AlgebraicMatrix55 I = AlgebraicMatrixID(); 00037 AlgebraicMatrix55 M = (I - K * H); 00038 AlgebraicSymMatrix55 fse = ROOT::Math::Similarity(M,C) + ROOT::Math::Similarity(K,V); 00039 00040 return TSOS( LTP(fsv, pzSign), LTE(fse), aTsos.surface(),&(aTsos.globalParameters().magneticField())); 00041 } 00042 00043 00044 00045
1.7.3