GhostTrackFitter/src/KalmanGhostTrackUpdater.cc

0001 #include <vector>
0002 #include <cmath>
0003
0004 #include <Math/SVector.h>
0005 #include <Math/SMatrix.h>
0006 #include <Math/MatrixFunctions.h>
0007
0008 #include "DataFormats/GeometryVector/interface/GlobalPoint.h"
0009 #include "DataFormats/GeometryVector/interface/GlobalVector.h"
0010 #include "DataFormats/GeometryCommonDetAlgo/interface/GlobalError.h"
0011
0012 #include "RecoVertex/GhostTrackFitter/interface/GhostTrackPrediction.h"
0013 #include "RecoVertex/GhostTrackFitter/interface/GhostTrackState.h"
0014
0015 #include "RecoVertex/GhostTrackFitter/interface/KalmanGhostTrackUpdater.h"
0016
0017 using namespace reco;
0018
0019 namespace {
0020 #ifndef __clang__
0021   static inline double sqr(double arg) { return arg * arg; }
0022 #endif
0023   using namespace ROOT::Math;
0024
0025   typedef SVector<double, 4> Vector4;
0026   typedef SVector<double, 2> Vector2;
0027
0028   typedef SMatrix<double, 4, 4, MatRepSym<double, 4> > Matrix4S;
0029   typedef SMatrix<double, 3, 3, MatRepSym<double, 3> > Matrix3S;
0030   typedef SMatrix<double, 2, 2, MatRepSym<double, 2> > Matrix2S;
0031   typedef SMatrix<double, 4, 4> Matrix44;
0032   typedef SMatrix<double, 4, 2> Matrix42;
0033   typedef SMatrix<double, 2, 4> Matrix24;
0034   typedef SMatrix<double, 2, 3> Matrix23;
0035   typedef SMatrix<double, 2, 2> Matrix22;
0036
0037   struct KalmanState {
0038     KalmanState(const GhostTrackPrediction &pred, const GhostTrackState &state);
0039
0040     Vector2 residual;
0041     Matrix2S measErr, measPredErr;
0042     Matrix24 h;
0043   };
0044 }  // namespace
0045
0046 KalmanState::KalmanState(const GhostTrackPrediction &pred, const GhostTrackState &state) {
0047   using namespace ROOT::Math;
0048
0049   GlobalPoint point = state.globalPosition();
0050
0051   // precomputed values
0052   double x = std::cos(pred.phi());
0053   double y = std::sin(pred.phi());
0054   double dz = pred.cotTheta();
0055   double lip = x * point.x() + y * point.y();
0056   double tip = x * point.y() - y * point.x();
0057
0058   // jacobian of global -> local
0059   Matrix23 measToLocal;
0060   measToLocal(0, 0) = -dz * x;
0061   measToLocal(0, 1) = -dz * y;
0062   measToLocal(0, 2) = 1.;
0063   measToLocal(1, 0) = y;
0064   measToLocal(1, 1) = -x;
0065
0066   // measurement error on the 2d plane projection
0067   measErr = Similarity(measToLocal, state.cartesianCovariance());
0068
0069   // jacobian of representation to measurement transformation
0070   h(0, 0) = 1.;
0071   h(0, 2) = lip;
0072   h(0, 3) = dz * tip;
0073   h(1, 1) = -1.;
0074   h(1, 3) = -lip;
0075
0076   // error on prediction
0077   measPredErr = Similarity(h, pred.covariance());
0078
0079   // residual
0080   residual[0] = point.z() - pred.z() - dz * lip;
0081   residual[1] = pred.ip() - tip;
0082 }
0083
0084 GhostTrackPrediction KalmanGhostTrackUpdater::update(const GhostTrackPrediction &pred,
0085                                                      const GhostTrackState &state,
0086                                                      double &ndof,
0087                                                      double &chi2) const {
0088   using namespace ROOT::Math;
0089
0090   KalmanState kalmanState(pred, state);
0091
0092   if (state.weight() < 1.0e-3)
0093     return pred;
0094
0095   // inverted combined error
0096   Matrix2S invErr = kalmanState.measPredErr + (1.0 / state.weight()) * kalmanState.measErr;
0097   if (!invErr.Invert())
0098     return pred;
0099
0100   // gain
0101   Matrix42 gain = pred.covariance() * Transpose(kalmanState.h) * invErr;
0102
0103   // new prediction
0104   Vector4 newPred = pred.prediction() + (gain * kalmanState.residual);
0105   Matrix44 tmp44 = SMatrixIdentity();
0106   tmp44 = (tmp44 - gain * kalmanState.h) * pred.covariance();
0107   Matrix4S newError(tmp44.LowerBlock());
0108
0109   // filtered residuals
0110   Matrix22 tmp22 = SMatrixIdentity();
0111   tmp22 = tmp22 - kalmanState.h * gain;
0112   Vector2 filtRes = tmp22 * kalmanState.residual;
0113   tmp22 *= kalmanState.measErr;
0114   Matrix2S filtResErr(tmp22.LowerBlock());
0115   if (!filtResErr.Invert())
0116     return pred;
0117
0118   ndof += state.weight() * 2.;
0119   chi2 += state.weight() * Similarity(filtRes, filtResErr);
0120
0121   return GhostTrackPrediction(newPred, newError);
0122 }
0123
0124 void KalmanGhostTrackUpdater::contribution(const GhostTrackPrediction &pred,
0125                                            const GhostTrackState &state,
0126                                            double &ndof,
0127                                            double &chi2,
0128                                            bool withPredError) const {
0129   using namespace ROOT::Math;
0130
0131   KalmanState kalmanState(pred, state);
0132
0133   // this is called on the full predicted state,
0134   // so the residual is already with respect to the filtered state
0135
0136   // inverted error
0137   Matrix2S invErr = kalmanState.measErr;
0138   if (withPredError)
0139     invErr += kalmanState.measPredErr;
0140   if (!invErr.Invert()) {
0141     ndof = 0.;
0142     chi2 = 0.;
0143   }
0144
0145   ndof = 2.;
0146   chi2 = Similarity(kalmanState.residual, invErr);
0147 }