CMS 3D CMS Logo

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Pages
KalmanGhostTrackUpdater.cc
Go to the documentation of this file.
1 #include <vector>
2 #include <cmath>
3 
4 #include <Math/SVector.h>
5 #include <Math/SMatrix.h>
6 #include <Math/MatrixFunctions.h>
7 
11 
14 
16 
17 using namespace reco;
18 
19 namespace {
20  static inline double sqr(double arg) { return arg * arg; }
21 
22  using namespace ROOT::Math;
23 
24  typedef SVector<double, 4> Vector4;
25  typedef SVector<double, 2> Vector2;
26 
27  typedef SMatrix<double, 4, 4, MatRepSym<double, 4> > Matrix4S;
28  typedef SMatrix<double, 3, 3, MatRepSym<double, 3> > Matrix3S;
29  typedef SMatrix<double, 2, 2, MatRepSym<double, 2> > Matrix2S;
30  typedef SMatrix<double, 4, 4> Matrix44;
31  typedef SMatrix<double, 4, 2> Matrix42;
32  typedef SMatrix<double, 2, 4> Matrix24;
33  typedef SMatrix<double, 2, 3> Matrix23;
34  typedef SMatrix<double, 2, 2> Matrix22;
35 
36  struct KalmanState {
37  KalmanState(const GhostTrackPrediction &pred,
38  const GhostTrackState &state);
39 
40  Vector2 residual;
41  Matrix2S measErr, measPredErr;
42  Matrix24 h;
43  };
44 }
45 
46 KalmanState::KalmanState(const GhostTrackPrediction &pred,
47  const GhostTrackState &state)
48 {
49  using namespace ROOT::Math;
50 
52 
53  // precomputed values
54  double x = std::cos(pred.phi());
55  double y = std::sin(pred.phi());
56  double dz = pred.cotTheta();
57  double lip = x * point.x() + y * point.y();
58  double tip = x * point.y() - y * point.x();
59 
60  // jacobian of global -> local
61  Matrix23 measToLocal;
62  measToLocal(0, 0) = - dz * x;
63  measToLocal(0, 1) = - dz * y;
64  measToLocal(0, 2) = 1.;
65  measToLocal(1, 0) = y;
66  measToLocal(1, 1) = -x;
67 
68  // measurement error on the 2d plane projection
69  measErr = Similarity(measToLocal, state.cartesianCovariance());
70 
71  // jacobian of representation to measurement transformation
72  h(0, 0) = 1.;
73  h(0, 2) = lip;
74  h(0, 3) = dz * tip;
75  h(1, 1) = -1.;
76  h(1, 3) = -lip;
77 
78  // error on prediction
79  measPredErr = Similarity(h, pred.covariance());
80 
81  // residual
82  residual[0] = point.z() - pred.z() - dz * lip;
83  residual[1] = pred.ip() - tip;
84 }
85 
87  const GhostTrackPrediction &pred,
88  const GhostTrackState &state,
89  double &ndof, double &chi2) const
90 {
91  using namespace ROOT::Math;
92 
93  KalmanState kalmanState(pred, state);
94 
95  if (state.weight() < 1.0e-3)
96  return pred;
97 
98  // inverted combined error
99  Matrix2S invErr = kalmanState.measPredErr +
100  (1.0 / state.weight()) * kalmanState.measErr;
101  if (!invErr.Invert())
102  return pred;
103 
104  // gain
105  Matrix42 gain = pred.covariance() * Transpose(kalmanState.h) * invErr;
106 
107  // new prediction
108  Vector4 newPred = pred.prediction() + (gain * kalmanState.residual);
109  Matrix44 tmp44 = SMatrixIdentity();
110  tmp44 = (tmp44 - gain * kalmanState.h) * pred.covariance();
111  Matrix4S newError(tmp44.LowerBlock());
112 
113  // filtered residuals
114  Matrix22 tmp22 = SMatrixIdentity();
115  tmp22 = tmp22 - kalmanState.h * gain;
116  Vector2 filtRes = tmp22 * kalmanState.residual;
117  tmp22 *= kalmanState.measErr;
118  Matrix2S filtResErr(tmp22.LowerBlock());
119  if (!filtResErr.Invert())
120  return pred;
121 
122  ndof += state.weight() * 2.;
123  chi2 += state.weight() * Similarity(filtRes, filtResErr);
124 
125  return GhostTrackPrediction(newPred, newError);
126 }
127 
129  const GhostTrackPrediction &pred,
130  const GhostTrackState &state,
131  double &ndof, double &chi2,
132  bool withPredError) const
133 {
134  using namespace ROOT::Math;
135 
136  KalmanState kalmanState(pred, state);
137 
138  // this is called on the full predicted state,
139  // so the residual is already with respect to the filtered state
140 
141  // inverted error
142  Matrix2S invErr = kalmanState.measErr;
143  if (withPredError)
144  invErr += kalmanState.measPredErr;
145  if (!invErr.Invert()) {
146  ndof = 0.;
147  chi2 = 0.;
148  }
149 
150  ndof = 2.;
151  chi2 = Similarity(kalmanState.residual, invErr);
152 }
double weight() const
Sin< T >::type sin(const T &t)
Definition: Sin.h:22
GhostTrackPrediction update(const GhostTrackPrediction &pred, const GhostTrackState &state, double &ndof, double &chi2) const
A arg
Definition: Factorize.h:36
Cos< T >::type cos(const T &t)
Definition: Cos.h:22
The Signals That Services Can Subscribe To This is based on ActivityRegistry h
Helper function to determine trigger accepts.
Definition: Activities.doc:4
void contribution(const GhostTrackPrediction &pred, const GhostTrackState &state, double &ndof, double &chi2, bool withPredError=false) const
char state
Definition: procUtils.cc:75
const Vector & prediction() const
const Error & covariance() const
if(dp >Float(M_PI)) dp-
Square< F >::type sqr(const F &f)
Definition: Square.h:13
CovarianceMatrix cartesianCovariance() const
Definition: DDAxes.h:10
GlobalPoint globalPosition() const
*vegas h *****************************************************used in the default bin number in original ***version of VEGAS is ***a higher bin number might help to derive a more precise ***grade subtle point
Definition: invegas.h:5