KalmanUpdators/src/KFStrip1DUpdator.cc

0001 #include "TrackingTools/KalmanUpdators/interface/KFStrip1DUpdator.h"
0002 #include "TrackingTools/KalmanUpdators/interface/Strip1DMeasurementTransformator.h"
0003
0004 TrajectoryStateOnSurface KFStrip1DUpdator::update(const TSOS& aTsos, const TrackingRecHit& aHit) const {
0005   double pzSign = aTsos.localParameters().pzSign();
0006
0007   Strip1DMeasurementTransformator myTrafo(aTsos, aHit);
0008
0009   double m = myTrafo.hitParameters();
0010   AlgebraicVector5 x(myTrafo.trajectoryParameters());
0011   double px = myTrafo.projectedTrajectoryParameters();
0012
0013   AlgebraicMatrix15 H(myTrafo.projectionMatrix());
0014   double V = myTrafo.hitError();
0015   const AlgebraicSymMatrix55& C(myTrafo.trajectoryError());
0016   double pC = myTrafo.projectedTrajectoryError();
0017
0018   double R = 1. / (V + pC);
0019
0020   // Compute Kalman gain matrix
0021   AlgebraicMatrix51 K(R * (C * ROOT::Math::Transpose(H)));
0022
0023   // Compute local filtered state vector
0024   AlgebraicVector5 fsv = x + K.Col(0) * (m - px);
0025
0026   // Compute covariance matrix of local filtered state vector
0027   AlgebraicSymMatrix55 I = AlgebraicMatrixID();
0028   AlgebraicMatrix55 M = I - K * H;
0029   AlgebraicSymMatrix55 fse = ROOT::Math::Similarity(M, C) + ROOT::Math::Similarity(K, AlgebraicSymMatrix11(V));
0030   //   AlgebraicMatrix M((I - K * H)*C);            // already commented when CLHEP was in use
0031   //   AlgebraicSymMatrix fse(5,0); fse.assign(M);  // already commented when CLHEP was in use
0032
0033   return TSOS(
0034       LTP(fsv, pzSign), LTE(fse), aTsos.surface(), &(aTsos.globalParameters().magneticField()), aTsos.surfaceSide());
0035 }