CMS 3D CMS Logo

KFUpdator.cc
Go to the documentation of this file.
9 
10 
11 // test of joseph form
12 #ifdef KU_JF_TEST
13 
14 #include<atomic>
15 #include<iostream>
16 namespace {
17  struct Stat {
18  Stat(): tot(0),
19  nopd(0), jnopd(0), fnopd(0),
20  inopd(0), ijnopd(0), ifnopd(0),dtot(0)
21  {for (int i=0;i<5;++i) idmm[i]=0; }
22 
23  std::atomic<long long> tot;
24  std::atomic<long long> nopd;
25  std::atomic<long long> jnopd;
26  std::atomic<long long> fnopd;
27  std::atomic<long long> inopd;
28  std::atomic<long long> ijnopd;
29  std::atomic<long long> ifnopd;
30 
31  std::atomic<unsigned long long> dtot;
32  std::atomic<unsigned long long> idmm[5];
33  double mm = 0;
34  double dmm[5] = {0};
35  bool ok=true;
36  ~Stat() {
37  double n = 1.e-3/double(dtot);
38  std::cout << "KF " << tot*1.e-9 << "G " << mm <<" "
39  <<dmm[0]<<'/'<<dmm[1]<<'/'<<dmm[2]<<'/'<<dmm[3]<<'/'<<dmm[4]<< '\n'
40  <<dtot << ' ' <<idmm[0]<<'/'<<idmm[1]<<'/'<<idmm[2]<<'/'<<idmm[3]<<'/'<<idmm[4]<< '\n'
41  <<std::sqrt(idmm[0]*n)<<'/'<<std::sqrt(idmm[1]*n)<<'/'<<std::sqrt(idmm[2]*n)<<'/'<<std::sqrt(idmm[3]*n)<<'/'<<std::sqrt(idmm[4]*n)
42  << " " << nopd << " " << jnopd << " " << fnopd
43  << " " << inopd << " " << ijnopd << " " << ifnopd
44  << std::endl;
45  }
46  };
47 
48  Stat stat;
49 
50  bool isNopd(AlgebraicSymMatrix55 const & m) {
51  return m(0,0)<0 || m(1,1)<0 || m(2,2)<0 || m(3,3)<0 || m(4,4)<0;
52  }
53 }
54 #endif
55 
56 
57 namespace {
58 
59 template <unsigned int D>
61  const TrackingRecHit& aRecHit) {
62 
63  typedef typename AlgebraicROOTObject<D,5>::Matrix MatD5;
64  typedef typename AlgebraicROOTObject<5,D>::Matrix Mat5D;
65  typedef typename AlgebraicROOTObject<D,D>::SymMatrix SMatDD;
66  typedef typename AlgebraicROOTObject<D>::Vector VecD;
67  using ROOT::Math::SMatrixNoInit;
68  double pzSign = tsos.localParameters().pzSign();
69 
70  auto && x = tsos.localParameters().vector();
71  auto && C = tsos.localError().matrix();
72 
73  // projection matrix (assume element of "H" to be just 0 or 1)
75 
76  // Measurement matrix
77  VecD r, rMeas;
78  SMatDD V(SMatrixNoInit{}), VMeas(SMatrixNoInit{});
79 
80  KfComponentsHolder holder;
81  holder.template setup<D>(&r, &V, &pf, &rMeas, &VMeas, x, C);
82  aRecHit.getKfComponents(holder);
83 
84  r -= rMeas;
85 
86  // and covariance matrix of residuals
87  SMatDD R = V + VMeas;
88  bool ok = invertPosDefMatrix(R);
89 
90  // Compute Kalman gain matrix
92  Mat5D K = C*pf.project(R);
93  pf.projectAndSubtractFrom(M,K);
94 
95 
96  // Compute local filtered state vector
97  AlgebraicVector5 fsv = x + K * r;
98 
99  // Compute covariance matrix of local filtered state vector
100 #define KU_JosephForm
101 #ifdef KU_JosephForm
102  AlgebraicSymMatrix55 fse = ROOT::Math::Similarity(M, C) + ROOT::Math::Similarity(K, V);
103 #else
104  AlgebraicSymMatrix55 fse; ROOT::Math::AssignSym::Evaluate(fse, M*C);
105 #endif
106 
107 // test of joseph form
108 #ifdef KU_JF_TEST
109 
110  AlgebraicSymMatrix55 fse2; ROOT::Math::AssignSym::Evaluate(fse2, M*C);
111 
112  // std::cout << "Joseph Form \n" << fse << std::endl;
113  // std::cout << "Fast Form \n" << fse2 << std::endl;
114 
115 
116  stat.tot++;
117  auto n1 = isNopd(fse);
118  auto n2 = isNopd(fse2);
119  if (n1&&n2) stat.nopd++;
120  if (n1) stat.jnopd++;
121  if (n2) stat.fnopd++;
122 
123  AlgebraicSymMatrix55 dd = fse2-fse;
124  auto dda = dd.Array();
125  auto fsa = fse.Array();
126  double ddd[15];
127  for (int i=0; i<15; ++i) ddd[i] = std::abs(dda[i])/std::abs(fsa[i]);
128  auto mm = *std::max_element(ddd,ddd+15);
129  stat.mm = std::max(stat.mm,mm);
130  if (stat.ok && !(n1||n2)) {
131  stat.dtot++;
132  for (int i=0; i<5; ++i) {
133  auto dmm = std::sqrt(fse2(i,i)) - std::sqrt(fse(i,i));
134  stat.dmm[i] = std::max(stat.dmm[i],std::abs(dmm));
135  stat.idmm[i] += (unsigned long long)(1000.*dmm*dmm);
136  if (stat.idmm[i] > std::numeric_limits<long long>::max() ) stat.ok=false;
137  }
138  }
139 
140  AlgebraicSymMatrix55 ifse = fse; invertPosDefMatrix(ifse);
141  AlgebraicSymMatrix55 ifse2 = fse2; invertPosDefMatrix(ifse2);
142  n1 = isNopd(ifse);
143  n2 = isNopd(ifse2);
144  if (n1&&n2) stat.inopd++;
145  if (n1) stat.ijnopd++;
146  if (n2) stat.ifnopd++;
147 #endif
148 
149  /*
150  // expanded similariy
151  AlgebraicSymMatrix55 fse;
152  ROOT::Math::AssignSym::Evaluate(fse, (M* C) * ( ROOT::Math::Transpose(M)));
153  AlgebraicSymMatrix55 tmp;
154  ROOT::Math::AssignSym::Evaluate(tmp, (K*V) * (ROOT::Math::Transpose(K)));
155  fse += tmp;
156  */
157 
158  if (ok) {
160  LocalTrajectoryError(fse), tsos.surface(),&(tsos.globalParameters().magneticField()), tsos.surfaceSide() );
161  }else {
162  edm::LogError("KFUpdator")<<" could not invert martix:\n"<< (V+VMeas);
163  return TrajectoryStateOnSurface();
164  }
165 
166 }
167 }
168 
170  const TrackingRecHit& aRecHit) const {
171  switch (aRecHit.dimension()) {
172  case 1: return lupdate<1>(tsos,aRecHit);
173  case 2: return lupdate<2>(tsos,aRecHit);
174  case 3: return lupdate<3>(tsos,aRecHit);
175  case 4: return lupdate<4>(tsos,aRecHit);
176  case 5: return lupdate<5>(tsos,aRecHit);
177  }
178  throw cms::Exception("Rec hit of invalid dimension (not 1,2,3,4,5)") <<
179  "The value was " << aRecHit.dimension() <<
180  ", type is " << typeid(aRecHit).name() << "\n";
181 }
182 
virtual void getKfComponents(KfComponentsHolder &holder) const
ROOT::Math::SMatrix< double, D1, D1, ROOT::Math::MatRepSym< double, D1 > > SymMatrix
const LocalTrajectoryParameters & localParameters() const
ROOT::Math::SMatrixIdentity AlgebraicMatrixID
ROOT::Math::SMatrix< double, 5, 5, ROOT::Math::MatRepSym< double, 5 > > AlgebraicSymMatrix55
AlgebraicVector5 vector() const
ROOT::Math::SMatrix< double, D1, D2, ROOT::Math::MatRepStd< double, D1, D2 > > Matrix
TrajectoryStateOnSurface update(const TrajectoryStateOnSurface &, const TrackingRecHit &) const override
Definition: KFUpdator.cc:169
const SurfaceType & surface() const
T sqrt(T t)
Definition: SSEVec.h:18
virtual int dimension() const =0
SurfaceSide surfaceSide() const
Position relative to material, defined relative to momentum vector.
bool invertPosDefMatrix(ROOT::Math::SMatrix< T, N, N, ROOT::Math::MatRepSym< T, N > > &m)
Abs< T >::type abs(const T &t)
Definition: Abs.h:22
const AlgebraicSymMatrix55 & matrix() const
const LocalTrajectoryError & localError() const
ROOT::Math::SVector< double, D1 > Vector
ROOT::Math::SVector< double, 5 > AlgebraicVector5
const GlobalTrajectoryParameters & globalParameters() const
const MagneticField & magneticField() const
Definition: DDUnits.h:8
float pzSign() const
Sign of the z-component of the momentum in the local frame.
ROOT::Math::SMatrix< double, 5, 5, ROOT::Math::MatRepStd< double, 5, 5 > > AlgebraicMatrix55