CMS 3D CMS Logo
KalmanUtilsMPlex.cc
Go to the documentation of this file.
1 #include "KalmanUtilsMPlex.h"
2 #include "PropagationMPlex.h"
3 
4 //#define DEBUG
5 #include "Debug.h"
6 
7 #include "KalmanUtilsMPlex.icc"
8 
9 #include "RecoTracker/MkFitCore/interface/cms_common_macros.h"
10 
11 namespace {
12  using namespace mkfit;
13  using idx_t = Matriplex::idx_t;
14 
15  inline void MultResidualsAdd(const MPlexLH& A, const MPlexLV& B, const MPlex2V& C, MPlexLV& D) {
16  // outPar = psPar + kalmanGain*(dPar)
17  // D = B A C
18  // where right half of kalman gain is 0
19 
20  // XXX Regenerate with a script.
21 
22  MultResidualsAdd_imp(A, B, C, D, 0, NN);
23  }
24 
25  inline void MultResidualsAdd(const MPlexL2& A, const MPlexLV& B, const MPlex2V& C, MPlexLV& D) {
26  // outPar = psPar + kalmanGain*(dPar)
27  // D = B A C
28  // where right half of kalman gain is 0
29 
30  // XXX Regenerate with a script.
31 
32  typedef float T;
33  const idx_t N = NN;
34 
35  const T* a = A.fArray;
36  ASSUME_ALIGNED(a, 64);
37  const T* b = B.fArray;
38  ASSUME_ALIGNED(b, 64);
39  const T* c = C.fArray;
40  ASSUME_ALIGNED(c, 64);
41  T* d = D.fArray;
42  ASSUME_ALIGNED(d, 64);
43 
44 #pragma omp simd
45  for (idx_t n = 0; n < N; ++n) {
46  // generate loop (can also write it manually this time, it's not much)
47  d[0 * N + n] = b[0 * N + n] + a[0 * N + n] * c[0 * N + n] + a[1 * N + n] * c[1 * N + n];
48  d[1 * N + n] = b[1 * N + n] + a[2 * N + n] * c[0 * N + n] + a[3 * N + n] * c[1 * N + n];
49  d[2 * N + n] = b[2 * N + n] + a[4 * N + n] * c[0 * N + n] + a[5 * N + n] * c[1 * N + n];
50  d[3 * N + n] = b[3 * N + n] + a[6 * N + n] * c[0 * N + n] + a[7 * N + n] * c[1 * N + n];
51  d[4 * N + n] = b[4 * N + n] + a[8 * N + n] * c[0 * N + n] + a[9 * N + n] * c[1 * N + n];
52  d[5 * N + n] = b[5 * N + n] + a[10 * N + n] * c[0 * N + n] + a[11 * N + n] * c[1 * N + n];
53  }
54  }
55 
56  //------------------------------------------------------------------------------
57 
58  inline void Chi2Similarity(const MPlex2V& A, //resPar
59  const MPlex2S& C, //resErr
60  MPlexQF& D) //outChi2
61  {
62  // outChi2 = (resPar) * resErr * (resPar)
63  // D = A * C * A
64 
65  typedef float T;
66  const idx_t N = NN;
67 
68  const T* a = A.fArray;
69  ASSUME_ALIGNED(a, 64);
70  const T* c = C.fArray;
71  ASSUME_ALIGNED(c, 64);
72  T* d = D.fArray;
73  ASSUME_ALIGNED(d, 64);
74 
75 #pragma omp simd
76  for (idx_t n = 0; n < N; ++n) {
77  // generate loop (can also write it manually this time, it's not much)
78  d[0 * N + n] = c[0 * N + n] * a[0 * N + n] * a[0 * N + n] + c[2 * N + n] * a[1 * N + n] * a[1 * N + n] +
79  2 * (c[1 * N + n] * a[1 * N + n] * a[0 * N + n]);
80  }
81  }
82 
83  //------------------------------------------------------------------------------
84 
85  inline void AddIntoUpperLeft3x3(const MPlexLS& A, const MPlexHS& B, MPlexHS& C) {
86  // The rest of matrix is left untouched.
87 
88  typedef float T;
89  const idx_t N = NN;
90 
91  const T* a = A.fArray;
92  ASSUME_ALIGNED(a, 64);
93  const T* b = B.fArray;
94  ASSUME_ALIGNED(b, 64);
95  T* c = C.fArray;
96  ASSUME_ALIGNED(c, 64);
97 
98 #pragma omp simd
99  for (idx_t n = 0; n < N; ++n) {
100  c[0 * N + n] = a[0 * N + n] + b[0 * N + n];
101  c[1 * N + n] = a[1 * N + n] + b[1 * N + n];
102  c[2 * N + n] = a[2 * N + n] + b[2 * N + n];
103  c[3 * N + n] = a[3 * N + n] + b[3 * N + n];
104  c[4 * N + n] = a[4 * N + n] + b[4 * N + n];
105  c[5 * N + n] = a[5 * N + n] + b[5 * N + n];
106  }
107  }
108 
109  inline void AddIntoUpperLeft2x2(const MPlexLS& A, const MPlexHS& B, MPlex2S& C) {
110  // The rest of matrix is left untouched.
111 
112  typedef float T;
113  const idx_t N = NN;
114 
115  const T* a = A.fArray;
116  ASSUME_ALIGNED(a, 64);
117  const T* b = B.fArray;
118  ASSUME_ALIGNED(b, 64);
119  T* c = C.fArray;
120  ASSUME_ALIGNED(c, 64);
121 
122 #pragma omp simd
123  for (idx_t n = 0; n < N; ++n) {
124  c[0 * N + n] = a[0 * N + n] + b[0 * N + n];
125  c[1 * N + n] = a[1 * N + n] + b[1 * N + n];
126  c[2 * N + n] = a[2 * N + n] + b[2 * N + n];
127  }
128  }
129 
130  //------------------------------------------------------------------------------
131 
132  inline void SubtractFirst3(const MPlexHV& A, const MPlexLV& B, MPlexHV& C) {
133  // The rest of matrix is left untouched.
134 
135  typedef float T;
136  const idx_t N = NN;
137 
138  const T* a = A.fArray;
139  ASSUME_ALIGNED(a, 64);
140  const T* b = B.fArray;
141  ASSUME_ALIGNED(b, 64);
142  T* c = C.fArray;
143  ASSUME_ALIGNED(c, 64);
144 
145 #pragma omp simd
146  for (idx_t n = 0; n < N; ++n) {
147  c[0 * N + n] = a[0 * N + n] - b[0 * N + n];
148  c[1 * N + n] = a[1 * N + n] - b[1 * N + n];
149  c[2 * N + n] = a[2 * N + n] - b[2 * N + n];
150  }
151  }
152 
153  inline void SubtractFirst2(const MPlexHV& A, const MPlexLV& B, MPlex2V& C) {
154  // The rest of matrix is left untouched.
155 
156  typedef float T;
157  const idx_t N = NN;
158 
159  const T* a = A.fArray;
160  ASSUME_ALIGNED(a, 64);
161  const T* b = B.fArray;
162  ASSUME_ALIGNED(b, 64);
163  T* c = C.fArray;
164  ASSUME_ALIGNED(c, 64);
165 
166 #pragma omp simd
167  for (idx_t n = 0; n < N; ++n) {
168  c[0 * N + n] = a[0 * N + n] - b[0 * N + n];
169  c[1 * N + n] = a[1 * N + n] - b[1 * N + n];
170  }
171  }
172 
173  //==============================================================================
174 
175  inline void ProjectResErr(const MPlexQF& A00, const MPlexQF& A01, const MPlexHS& B, MPlexHH& C) {
176  // C = A * B, C is 3x3, A is 3x3 , B is 3x3 sym
177 
178  // Based on script generation and adapted to custom sizes.
179 
180  typedef float T;
181  const idx_t N = NN;
182 
183  const T* a00 = A00.fArray;
184  ASSUME_ALIGNED(a00, 64);
185  const T* a01 = A01.fArray;
186  ASSUME_ALIGNED(a01, 64);
187  const T* b = B.fArray;
188  ASSUME_ALIGNED(b, 64);
189  T* c = C.fArray;
190  ASSUME_ALIGNED(c, 64);
191 
192 #pragma omp simd
193  for (int n = 0; n < N; ++n) {
194  c[0 * N + n] = a00[n] * b[0 * N + n] + a01[n] * b[1 * N + n];
195  c[1 * N + n] = a00[n] * b[1 * N + n] + a01[n] * b[2 * N + n];
196  c[2 * N + n] = a00[n] * b[3 * N + n] + a01[n] * b[4 * N + n];
197  c[3 * N + n] = b[3 * N + n];
198  c[4 * N + n] = b[4 * N + n];
199  c[5 * N + n] = b[5 * N + n];
200  c[6 * N + n] = a01[n] * b[0 * N + n] - a00[n] * b[1 * N + n];
201  c[7 * N + n] = a01[n] * b[1 * N + n] - a00[n] * b[2 * N + n];
202  c[8 * N + n] = a01[n] * b[3 * N + n] - a00[n] * b[4 * N + n];
203  }
204  }
205 
206  inline void ProjectResErrTransp(const MPlexQF& A00, const MPlexQF& A01, const MPlexHH& B, MPlex2S& C) {
207  // C = A * B, C is 3x3 sym, A is 3x3 , B is 3x3
208 
209  // Based on script generation and adapted to custom sizes.
210 
211  typedef float T;
212  const idx_t N = NN;
213 
214  const T* a00 = A00.fArray;
215  ASSUME_ALIGNED(a00, 64);
216  const T* a01 = A01.fArray;
217  ASSUME_ALIGNED(a01, 64);
218  const T* b = B.fArray;
219  ASSUME_ALIGNED(b, 64);
220  T* c = C.fArray;
221  ASSUME_ALIGNED(c, 64);
222 
223 #pragma omp simd
224  for (int n = 0; n < N; ++n) {
225  c[0 * N + n] = b[0 * N + n] * a00[n] + b[1 * N + n] * a01[n];
226  c[1 * N + n] = b[3 * N + n] * a00[n] + b[4 * N + n] * a01[n];
227  c[2 * N + n] = b[5 * N + n];
228  }
229  }
230 
231  inline void RotateResidualsOnTangentPlane(const MPlexQF& R00, //r00
232  const MPlexQF& R01, //r01
233  const MPlexHV& A, //res_glo
234  MPlex2V& B) //res_loc
235  {
236  RotateResidualsOnTangentPlane_impl(R00, R01, A, B, 0, NN);
237  }
238 
239  inline void KalmanHTG(const MPlexQF& A00, const MPlexQF& A01, const MPlex2S& B, MPlexHH& C) {
240  // HTG = rot * res_loc
241  // C = A * B
242 
243  // Based on script generation and adapted to custom sizes.
244 
245  typedef float T;
246  const idx_t N = NN;
247 
248  const T* a00 = A00.fArray;
249  ASSUME_ALIGNED(a00, 64);
250  const T* a01 = A01.fArray;
251  ASSUME_ALIGNED(a01, 64);
252  const T* b = B.fArray;
253  ASSUME_ALIGNED(b, 64);
254  T* c = C.fArray;
255  ASSUME_ALIGNED(c, 64);
256 
257 #pragma omp simd
258  for (int n = 0; n < N; ++n) {
259  c[0 * N + n] = a00[n] * b[0 * N + n];
260  c[1 * N + n] = a00[n] * b[1 * N + n];
261  c[2 * N + n] = 0.;
262  c[3 * N + n] = a01[n] * b[0 * N + n];
263  c[4 * N + n] = a01[n] * b[1 * N + n];
264  c[5 * N + n] = 0.;
265  c[6 * N + n] = b[1 * N + n];
266  c[7 * N + n] = b[2 * N + n];
267  c[8 * N + n] = 0.;
268  }
269  }
270 
271  inline void KalmanGain(const MPlexLS& A, const MPlexHH& B, MPlexLH& C) {
272  // C = A * B, C is 6x3, A is 6x6 sym , B is 3x3
273 
274  typedef float T;
275  const idx_t N = NN;
276 
277  const T* a = A.fArray;
278  ASSUME_ALIGNED(a, 64);
279  const T* b = B.fArray;
280  ASSUME_ALIGNED(b, 64);
281  T* c = C.fArray;
282  ASSUME_ALIGNED(c, 64);
283 
284 #pragma omp simd
285  for (int n = 0; n < N; ++n) {
286  c[0 * N + n] = a[0 * N + n] * b[0 * N + n] + a[1 * N + n] * b[3 * N + n] + a[3 * N + n] * b[6 * N + n];
287  c[1 * N + n] = a[0 * N + n] * b[1 * N + n] + a[1 * N + n] * b[4 * N + n] + a[3 * N + n] * b[7 * N + n];
288  c[2 * N + n] = 0;
289  c[3 * N + n] = a[1 * N + n] * b[0 * N + n] + a[2 * N + n] * b[3 * N + n] + a[4 * N + n] * b[6 * N + n];
290  c[4 * N + n] = a[1 * N + n] * b[1 * N + n] + a[2 * N + n] * b[4 * N + n] + a[4 * N + n] * b[7 * N + n];
291  c[5 * N + n] = 0;
292  c[6 * N + n] = a[3 * N + n] * b[0 * N + n] + a[4 * N + n] * b[3 * N + n] + a[5 * N + n] * b[6 * N + n];
293  c[7 * N + n] = a[3 * N + n] * b[1 * N + n] + a[4 * N + n] * b[4 * N + n] + a[5 * N + n] * b[7 * N + n];
294  c[8 * N + n] = 0;
295  c[9 * N + n] = a[6 * N + n] * b[0 * N + n] + a[7 * N + n] * b[3 * N + n] + a[8 * N + n] * b[6 * N + n];
296  c[10 * N + n] = a[6 * N + n] * b[1 * N + n] + a[7 * N + n] * b[4 * N + n] + a[8 * N + n] * b[7 * N + n];
297  c[11 * N + n] = 0;
298  c[12 * N + n] = a[10 * N + n] * b[0 * N + n] + a[11 * N + n] * b[3 * N + n] + a[12 * N + n] * b[6 * N + n];
299  c[13 * N + n] = a[10 * N + n] * b[1 * N + n] + a[11 * N + n] * b[4 * N + n] + a[12 * N + n] * b[7 * N + n];
300  c[14 * N + n] = 0;
301  c[15 * N + n] = a[15 * N + n] * b[0 * N + n] + a[16 * N + n] * b[3 * N + n] + a[17 * N + n] * b[6 * N + n];
302  c[16 * N + n] = a[15 * N + n] * b[1 * N + n] + a[16 * N + n] * b[4 * N + n] + a[17 * N + n] * b[7 * N + n];
303  c[17 * N + n] = 0;
304  }
305  }
306 
307  inline void CovXYconstrain(const MPlexQF& R00, const MPlexQF& R01, const MPlexLS& Ci, MPlexLS& Co) {
308  // C is transformed to align along y after rotation and rotated back
309 
310  typedef float T;
311  const idx_t N = NN;
312 
313  const T* r00 = R00.fArray;
314  ASSUME_ALIGNED(r00, 64);
315  const T* r01 = R01.fArray;
316  ASSUME_ALIGNED(r01, 64);
317  const T* ci = Ci.fArray;
318  ASSUME_ALIGNED(ci, 64);
319  T* co = Co.fArray;
320  ASSUME_ALIGNED(co, 64);
321 
322 #pragma omp simd
323  for (int n = 0; n < N; ++n) {
324  // a bit loopy to avoid temporaries
325  co[0 * N + n] =
326  r00[n] * r00[n] * ci[0 * N + n] + 2 * r00[n] * r01[n] * ci[1 * N + n] + r01[n] * r01[n] * ci[2 * N + n];
327  co[1 * N + n] = r00[n] * r01[n] * co[0 * N + n];
328  co[2 * N + n] = r01[n] * r01[n] * co[0 * N + n];
329  co[0 * N + n] = r00[n] * r00[n] * co[0 * N + n];
330 
331  co[3 * N + n] = r00[n] * ci[3 * N + n] + r01[n] * ci[4 * N + n];
332  co[4 * N + n] = r01[n] * co[3 * N + n];
333  co[3 * N + n] = r00[n] * co[3 * N + n];
334 
335  co[6 * N + n] = r00[n] * ci[6 * N + n] + r01[n] * ci[7 * N + n];
336  co[7 * N + n] = r01[n] * co[6 * N + n];
337  co[6 * N + n] = r00[n] * co[6 * N + n];
338 
339  co[10 * N + n] = r00[n] * ci[10 * N + n] + r01[n] * ci[11 * N + n];
340  co[11 * N + n] = r01[n] * co[10 * N + n];
341  co[10 * N + n] = r00[n] * co[10 * N + n];
342 
343  co[15 * N + n] = r00[n] * ci[15 * N + n] + r01[n] * ci[16 * N + n];
344  co[16 * N + n] = r01[n] * co[15 * N + n];
345  co[15 * N + n] = r00[n] * co[15 * N + n];
346  }
347  }
348 
349  void KalmanGain(const MPlexLS& A, const MPlex2S& B, MPlexL2& C) {
350  // C = A * B, C is 6x2, A is 6x6 sym , B is 2x2
351 
352  typedef float T;
353  const idx_t N = NN;
354 
355  const T* a = A.fArray;
356  ASSUME_ALIGNED(a, 64);
357  const T* b = B.fArray;
358  ASSUME_ALIGNED(b, 64);
359  T* c = C.fArray;
360  ASSUME_ALIGNED(c, 64);
361 
362 #include "KalmanGain62.ah"
363  }
364 
365  inline void KHMult(const MPlexLH& A, const MPlexQF& B00, const MPlexQF& B01, MPlexLL& C) {
366  // C = A * B, C is 6x6, A is 6x3 , B is 3x6
367  KHMult_imp(A, B00, B01, C, 0, NN);
368  }
369 
370  inline void KHC(const MPlexLL& A, const MPlexLS& B, MPlexLS& C) {
371  // C = A * B, C is 6x6, A is 6x6 , B is 6x6 sym
372 
373  typedef float T;
374  const idx_t N = NN;
375 
376  const T* a = A.fArray;
377  ASSUME_ALIGNED(a, 64);
378  const T* b = B.fArray;
379  ASSUME_ALIGNED(b, 64);
380  T* c = C.fArray;
381  ASSUME_ALIGNED(c, 64);
382 
383 #include "KHC.ah"
384  }
385 
386  inline void KHC(const MPlexL2& A, const MPlexLS& B, MPlexLS& C) {
387  // C = A * B, C is 6x6 sym, A is 6x2 , B is 6x6 sym
388 
389  typedef float T;
390  const idx_t N = NN;
391 
392  const T* a = A.fArray;
393  ASSUME_ALIGNED(a, 64);
394  const T* b = B.fArray;
395  ASSUME_ALIGNED(b, 64);
396  T* c = C.fArray;
397  ASSUME_ALIGNED(c, 64);
398 
399 #include "K62HC.ah"
400  }
401 
402  //Warning: MultFull is not vectorized!
403  template <typename T1, typename T2, typename T3>
404  void MultFull(const T1& A, int nia, int nja, const T2& B, int nib, int njb, T3& C, int nic, int njc) {
405 #ifdef DEBUG
406  assert(nja == nib);
407  assert(nia == nic);
408  assert(njb == njc);
409 #endif
410  for (int n = 0; n < NN; ++n) {
411  for (int i = 0; i < nia; ++i) {
412  for (int j = 0; j < njb; ++j) {
413  C(n, i, j) = 0.;
414  for (int k = 0; k < nja; ++k)
415  C(n, i, j) += A.constAt(n, i, k) * B.constAt(n, k, j);
416  }
417  }
418  }
419  }
420 
421  //Warning: MultTranspFull is not vectorized!
422  // (careful about which one is transposed, I think rows and cols are swapped and the one that is transposed is A)
423  template <typename T1, typename T2, typename T3>
424  void MultTranspFull(const T1& A, int nia, int nja, const T2& B, int nib, int njb, T3& C, int nic, int njc) {
425 #ifdef DEBUG
426  assert(nja == njb);
427  assert(nia == nic);
428  assert(nib == njc);
429 #endif
430  for (int n = 0; n < NN; ++n) {
431  for (int i = 0; i < nia; ++i) {
432  for (int j = 0; j < nib; ++j) {
433  C(n, i, j) = 0.;
434  for (int k = 0; k < nja; ++k)
435  C(n, i, j) += A.constAt(n, i, k) * B.constAt(n, j, k);
436  }
437  }
438  }
439  }
440 
441 } // namespace
442 
443 //==============================================================================
444 // Kalman operations - common dummy variables
445 //==============================================================================
446 
447 namespace {
448  // Dummy variables for parameter consistency to kalmanOperation.
449  // Through KalmanFilterOperation enum parameter it is guaranteed that
450  // those will never get accessed in the code (read from or written into).
451 
452  CMS_SA_ALLOW mkfit::MPlexLS dummy_err;
453  CMS_SA_ALLOW mkfit::MPlexLV dummy_par;
454  CMS_SA_ALLOW mkfit::MPlexQF dummy_chi2;
455 } // namespace
456 
457 namespace mkfit {
458 
459  //==============================================================================
460  // Kalman operations - Barrel
461  //==============================================================================
462 
463  void kalmanUpdate(const MPlexLS& psErr,
464  const MPlexLV& psPar,
465  const MPlexHS& msErr,
466  const MPlexHV& msPar,
467  MPlexLS& outErr,
468  MPlexLV& outPar,
469  const int N_proc) {
470  kalmanOperation(KFO_Update_Params | KFO_Local_Cov, psErr, psPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
471  }
472 
473  void kalmanPropagateAndUpdate(const MPlexLS& psErr,
474  const MPlexLV& psPar,
475  MPlexQI& Chg,
476  const MPlexHS& msErr,
477  const MPlexHV& msPar,
478  MPlexLS& outErr,
479  MPlexLV& outPar,
480  MPlexQI& outFailFlag,
481  const int N_proc,
482  const PropagationFlags propFlags,
483  const bool propToHit) {
484  if (propToHit) {
485  MPlexLS propErr;
486  MPlexLV propPar;
487  MPlexQF msRad;
488 #pragma omp simd
489  for (int n = 0; n < NN; ++n) {
490  msRad.At(n, 0, 0) = std::hypot(msPar.constAt(n, 0, 0), msPar.constAt(n, 1, 0));
491  }
492 
493  propagateHelixToRMPlex(psErr, psPar, Chg, msRad, propErr, propPar, outFailFlag, N_proc, propFlags);
494 
495  kalmanOperation(
496  KFO_Update_Params | KFO_Local_Cov, propErr, propPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
497  } else {
498  kalmanOperation(
499  KFO_Update_Params | KFO_Local_Cov, psErr, psPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
500  }
501  for (int n = 0; n < NN; ++n) {
502  if (outPar.At(n, 3, 0) < 0) {
503  Chg.At(n, 0, 0) = -Chg.At(n, 0, 0);
504  outPar.At(n, 3, 0) = -outPar.At(n, 3, 0);
505  }
506  }
507  }
508 
509  //------------------------------------------------------------------------------
510 
511  void kalmanComputeChi2(const MPlexLS& psErr,
512  const MPlexLV& psPar,
513  const MPlexQI& inChg,
514  const MPlexHS& msErr,
515  const MPlexHV& msPar,
516  MPlexQF& outChi2,
517  const int N_proc) {
518  kalmanOperation(KFO_Calculate_Chi2, psErr, psPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
519  }
520 
521  void kalmanPropagateAndComputeChi2(const MPlexLS& psErr,
522  const MPlexLV& psPar,
523  const MPlexQI& inChg,
524  const MPlexHS& msErr,
525  const MPlexHV& msPar,
526  MPlexQF& outChi2,
527  MPlexLV& propPar,
528  MPlexQI& outFailFlag,
529  const int N_proc,
530  const PropagationFlags propFlags,
531  const bool propToHit) {
532  propPar = psPar;
533  if (propToHit) {
534  MPlexLS propErr;
535  MPlexQF msRad;
536 #pragma omp simd
537  for (int n = 0; n < NN; ++n) {
538  msRad.At(n, 0, 0) = std::hypot(msPar.constAt(n, 0, 0), msPar.constAt(n, 1, 0));
539  }
540 
541  propagateHelixToRMPlex(psErr, psPar, inChg, msRad, propErr, propPar, outFailFlag, N_proc, propFlags);
542 
543  kalmanOperation(KFO_Calculate_Chi2, propErr, propPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
544  } else {
545  kalmanOperation(KFO_Calculate_Chi2, psErr, psPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
546  }
547  }
548 
549  //------------------------------------------------------------------------------
550 
551  void kalmanOperation(const int kfOp,
552  const MPlexLS& psErr,
553  const MPlexLV& psPar,
554  const MPlexHS& msErr,
555  const MPlexHV& msPar,
556  MPlexLS& outErr,
557  MPlexLV& outPar,
558  MPlexQF& outChi2,
559  const int N_proc) {
560 #ifdef DEBUG
561  {
562  dmutex_guard;
563  printf("psPar:\n");
564  for (int i = 0; i < 6; ++i) {
565  printf("%8f ", psPar.constAt(0, 0, i));
566  printf("\n");
567  }
568  printf("\n");
569  printf("psErr:\n");
570  for (int i = 0; i < 6; ++i) {
571  for (int j = 0; j < 6; ++j)
572  printf("%8f ", psErr.constAt(0, i, j));
573  printf("\n");
574  }
575  printf("\n");
576  printf("msPar:\n");
577  for (int i = 0; i < 3; ++i) {
578  printf("%8f ", msPar.constAt(0, 0, i));
579  printf("\n");
580  }
581  printf("\n");
582  printf("msErr:\n");
583  for (int i = 0; i < 3; ++i) {
584  for (int j = 0; j < 3; ++j)
585  printf("%8f ", msErr.constAt(0, i, j));
586  printf("\n");
587  }
588  printf("\n");
589  }
590 #endif
591 
592  // Rotate global point on tangent plane to cylinder
593  // Tangent point is half way between hit and propagate position
594 
595  // Rotation matrix
596  // rotT00 0 rotT01
597  // rotT01 0 -rotT00
598  // 0 1 0
599  // Minimize temporaries: only two float are needed!
600 
601  MPlexQF rotT00;
602  MPlexQF rotT01;
603  for (int n = 0; n < NN; ++n) {
604  const float r = std::hypot(msPar.constAt(n, 0, 0), msPar.constAt(n, 1, 0));
605  rotT00.At(n, 0, 0) = -(msPar.constAt(n, 1, 0) + psPar.constAt(n, 1, 0)) / (2 * r);
606  rotT01.At(n, 0, 0) = (msPar.constAt(n, 0, 0) + psPar.constAt(n, 0, 0)) / (2 * r);
607  }
608 
609  MPlexHV res_glo; //position residual in global coordinates
610  SubtractFirst3(msPar, psPar, res_glo);
611 
612  MPlexHS resErr_glo; //covariance sum in global position coordinates
613  AddIntoUpperLeft3x3(psErr, msErr, resErr_glo);
614 
615  MPlex2V res_loc; //position residual in local coordinates
616  RotateResidualsOnTangentPlane(rotT00, rotT01, res_glo, res_loc);
617  MPlex2S resErr_loc; //covariance sum in local position coordinates
618  MPlexHH tempHH;
619  ProjectResErr(rotT00, rotT01, resErr_glo, tempHH);
620  ProjectResErrTransp(rotT00, rotT01, tempHH, resErr_loc);
621 
622 #ifdef DEBUG
623  {
624  dmutex_guard;
625  printf("resErr_loc:\n");
626  for (int i = 0; i < 2; ++i) {
627  for (int j = 0; j < 2; ++j)
628  printf("%8f ", resErr_loc.At(0, i, j));
629  printf("\n");
630  }
631  printf("\n");
632  }
633 #endif
634 
635  //invert the 2x2 matrix
636  Matriplex::invertCramerSym(resErr_loc);
637 
638  if (kfOp & KFO_Calculate_Chi2) {
639  Chi2Similarity(res_loc, resErr_loc, outChi2);
640 
641 #ifdef DEBUG
642  {
643  dmutex_guard;
644  printf("resErr_loc (Inv):\n");
645  for (int i = 0; i < 2; ++i) {
646  for (int j = 0; j < 2; ++j)
647  printf("%8f ", resErr_loc.At(0, i, j));
648  printf("\n");
649  }
650  printf("\n");
651  printf("chi2: %8f\n", outChi2.At(0, 0, 0));
652  }
653 #endif
654  }
655 
656  if (kfOp & KFO_Update_Params) {
657  MPlexLS psErrLoc = psErr;
658  if (kfOp & KFO_Local_Cov)
659  CovXYconstrain(rotT00, rotT01, psErr, psErrLoc);
660 
661  MPlexLH K; // kalman gain, fixme should be L2
662  KalmanHTG(rotT00, rotT01, resErr_loc, tempHH); // intermediate term to get kalman gain (H^T*G)
663  KalmanGain(psErrLoc, tempHH, K);
664 
665  MultResidualsAdd(K, psPar, res_loc, outPar);
666  MPlexLL tempLL;
667 
668  squashPhiMPlex(outPar, N_proc); // ensure phi is between |pi|
669 
670  KHMult(K, rotT00, rotT01, tempLL);
671  KHC(tempLL, psErrLoc, outErr);
672  outErr.subtract(psErrLoc, outErr);
673 
674 #ifdef DEBUG
675  {
676  dmutex_guard;
677  if (kfOp & KFO_Local_Cov) {
678  printf("psErrLoc:\n");
679  for (int i = 0; i < 6; ++i) {
680  for (int j = 0; j < 6; ++j)
681  printf("% 8e ", psErrLoc.At(0, i, j));
682  printf("\n");
683  }
684  printf("\n");
685  }
686  printf("res_glo:\n");
687  for (int i = 0; i < 3; ++i) {
688  printf("%8f ", res_glo.At(0, i, 0));
689  }
690  printf("\n");
691  printf("res_loc:\n");
692  for (int i = 0; i < 2; ++i) {
693  printf("%8f ", res_loc.At(0, i, 0));
694  }
695  printf("\n");
696  printf("resErr_loc (Inv):\n");
697  for (int i = 0; i < 2; ++i) {
698  for (int j = 0; j < 2; ++j)
699  printf("%8f ", resErr_loc.At(0, i, j));
700  printf("\n");
701  }
702  printf("\n");
703  printf("K:\n");
704  for (int i = 0; i < 6; ++i) {
705  for (int j = 0; j < 3; ++j)
706  printf("%8f ", K.At(0, i, j));
707  printf("\n");
708  }
709  printf("\n");
710  printf("outPar:\n");
711  for (int i = 0; i < 6; ++i) {
712  printf("%8f ", outPar.At(0, i, 0));
713  }
714  printf("\n");
715  printf("outErr:\n");
716  for (int i = 0; i < 6; ++i) {
717  for (int j = 0; j < 6; ++j)
718  printf("%8f ", outErr.At(0, i, j));
719  printf("\n");
720  }
721  printf("\n");
722  }
723 #endif
724  }
725  }
726 
727  //==============================================================================
728  // Kalman operations - Endcap
729  //==============================================================================
730 
731  void kalmanUpdateEndcap(const MPlexLS& psErr,
732  const MPlexLV& psPar,
733  const MPlexHS& msErr,
734  const MPlexHV& msPar,
735  MPlexLS& outErr,
736  MPlexLV& outPar,
737  const int N_proc) {
738  kalmanOperationEndcap(KFO_Update_Params, psErr, psPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
739  }
740 
741  void kalmanPropagateAndUpdateEndcap(const MPlexLS& psErr,
742  const MPlexLV& psPar,
743  MPlexQI& Chg,
744  const MPlexHS& msErr,
745  const MPlexHV& msPar,
746  MPlexLS& outErr,
747  MPlexLV& outPar,
748  MPlexQI& outFailFlag,
749  const int N_proc,
750  const PropagationFlags propFlags,
751  const bool propToHit) {
752  if (propToHit) {
753  MPlexLS propErr;
754  MPlexLV propPar;
755  MPlexQF msZ;
756 #pragma omp simd
757  for (int n = 0; n < NN; ++n) {
758  msZ.At(n, 0, 0) = msPar.constAt(n, 2, 0);
759  }
760 
761  propagateHelixToZMPlex(psErr, psPar, Chg, msZ, propErr, propPar, outFailFlag, N_proc, propFlags);
762 
763  kalmanOperationEndcap(KFO_Update_Params, propErr, propPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
764  } else {
765  kalmanOperationEndcap(KFO_Update_Params, psErr, psPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
766  }
767  for (int n = 0; n < NN; ++n) {
768  if (outPar.At(n, 3, 0) < 0) {
769  Chg.At(n, 0, 0) = -Chg.At(n, 0, 0);
770  outPar.At(n, 3, 0) = -outPar.At(n, 3, 0);
771  }
772  }
773  }
774 
775  //------------------------------------------------------------------------------
776 
777  void kalmanComputeChi2Endcap(const MPlexLS& psErr,
778  const MPlexLV& psPar,
779  const MPlexQI& inChg,
780  const MPlexHS& msErr,
781  const MPlexHV& msPar,
782  MPlexQF& outChi2,
783  const int N_proc) {
784  kalmanOperationEndcap(KFO_Calculate_Chi2, psErr, psPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
785  }
786 
787  void kalmanPropagateAndComputeChi2Endcap(const MPlexLS& psErr,
788  const MPlexLV& psPar,
789  const MPlexQI& inChg,
790  const MPlexHS& msErr,
791  const MPlexHV& msPar,
792  MPlexQF& outChi2,
793  MPlexLV& propPar,
794  MPlexQI& outFailFlag,
795  const int N_proc,
796  const PropagationFlags propFlags,
797  const bool propToHit) {
798  propPar = psPar;
799  if (propToHit) {
800  MPlexLS propErr;
801  MPlexQF msZ;
802 #pragma omp simd
803  for (int n = 0; n < NN; ++n) {
804  msZ.At(n, 0, 0) = msPar.constAt(n, 2, 0);
805  }
806 
807  propagateHelixToZMPlex(psErr, psPar, inChg, msZ, propErr, propPar, outFailFlag, N_proc, propFlags);
808 
809  kalmanOperationEndcap(KFO_Calculate_Chi2, propErr, propPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
810  } else {
811  kalmanOperationEndcap(KFO_Calculate_Chi2, psErr, psPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
812  }
813  }
814 
815  //------------------------------------------------------------------------------
816 
817  void kalmanOperationEndcap(const int kfOp,
818  const MPlexLS& psErr,
819  const MPlexLV& psPar,
820  const MPlexHS& msErr,
821  const MPlexHV& msPar,
822  MPlexLS& outErr,
823  MPlexLV& outPar,
824  MPlexQF& outChi2,
825  const int N_proc) {
826 #ifdef DEBUG
827  {
828  dmutex_guard;
829  printf("updateParametersEndcapMPlex\n");
830  printf("psPar:\n");
831  for (int i = 0; i < 6; ++i) {
832  printf("%8f ", psPar.constAt(0, 0, i));
833  printf("\n");
834  }
835  printf("\n");
836  printf("msPar:\n");
837  for (int i = 0; i < 3; ++i) {
838  printf("%8f ", msPar.constAt(0, 0, i));
839  printf("\n");
840  }
841  printf("\n");
842  printf("psErr:\n");
843  for (int i = 0; i < 6; ++i) {
844  for (int j = 0; j < 6; ++j)
845  printf("%8f ", psErr.constAt(0, i, j));
846  printf("\n");
847  }
848  printf("\n");
849  printf("msErr:\n");
850  for (int i = 0; i < 3; ++i) {
851  for (int j = 0; j < 3; ++j)
852  printf("%8f ", msErr.constAt(0, i, j));
853  printf("\n");
854  }
855  printf("\n");
856  }
857 #endif
858 
859  MPlex2V res;
860  SubtractFirst2(msPar, psPar, res);
861 
862  MPlex2S resErr;
863  AddIntoUpperLeft2x2(psErr, msErr, resErr);
864 
865 #ifdef DEBUG
866  {
867  dmutex_guard;
868  printf("resErr:\n");
869  for (int i = 0; i < 2; ++i) {
870  for (int j = 0; j < 2; ++j)
871  printf("%8f ", resErr.At(0, i, j));
872  printf("\n");
873  }
874  printf("\n");
875  }
876 #endif
877 
878  //invert the 2x2 matrix
879  Matriplex::invertCramerSym(resErr);
880 
881  if (kfOp & KFO_Calculate_Chi2) {
882  Chi2Similarity(res, resErr, outChi2);
883 
884 #ifdef DEBUG
885  {
886  dmutex_guard;
887  printf("resErr_loc (Inv):\n");
888  for (int i = 0; i < 2; ++i) {
889  for (int j = 0; j < 2; ++j)
890  printf("%8f ", resErr.At(0, i, j));
891  printf("\n");
892  }
893  printf("\n");
894  printf("chi2: %8f\n", outChi2.At(0, 0, 0));
895  }
896 #endif
897  }
898 
899  if (kfOp & KFO_Update_Params) {
900  MPlexL2 K;
901  KalmanGain(psErr, resErr, K);
902 
903  MultResidualsAdd(K, psPar, res, outPar);
904 
905  squashPhiMPlex(outPar, N_proc); // ensure phi is between |pi|
906 
907  KHC(K, psErr, outErr);
908 
909 #ifdef DEBUG
910  {
911  dmutex_guard;
912  printf("outErr before subtract:\n");
913  for (int i = 0; i < 6; ++i) {
914  for (int j = 0; j < 6; ++j)
915  printf("%8f ", outErr.At(0, i, j));
916  printf("\n");
917  }
918  printf("\n");
919  }
920 #endif
921 
922  outErr.subtract(psErr, outErr);
923 
924 #ifdef DEBUG
925  {
926  dmutex_guard;
927  printf("res:\n");
928  for (int i = 0; i < 2; ++i) {
929  printf("%8f ", res.At(0, i, 0));
930  }
931  printf("\n");
932  printf("resErr (Inv):\n");
933  for (int i = 0; i < 2; ++i) {
934  for (int j = 0; j < 2; ++j)
935  printf("%8f ", resErr.At(0, i, j));
936  printf("\n");
937  }
938  printf("\n");
939  printf("K:\n");
940  for (int i = 0; i < 6; ++i) {
941  for (int j = 0; j < 2; ++j)
942  printf("%8f ", K.At(0, i, j));
943  printf("\n");
944  }
945  printf("\n");
946  printf("outPar:\n");
947  for (int i = 0; i < 6; ++i) {
948  printf("%8f ", outPar.At(0, i, 0));
949  }
950  printf("\n");
951  printf("outErr:\n");
952  for (int i = 0; i < 6; ++i) {
953  for (int j = 0; j < 6; ++j)
954  printf("%8f ", outErr.At(0, i, j));
955  printf("\n");
956  }
957  printf("\n");
958  }
959 #endif
960  }
961  }
962 
963 } // end namespace mkfit
Matriplex::MatriplexSym::subtract
MatriplexSym & subtract(const MatriplexSym &a, const MatriplexSym &b)
Definition: MatriplexSym.h:212
CMS_SA_ALLOW
#define CMS_SA_ALLOW
mps_fire.i
i
Definition: mps_fire.py:429
B
Definition: APVGainStruct.h:7
mkfit::kalmanOperationEndcap
void kalmanOperationEndcap(const int kfOp, const MPlexLS &psErr, const MPlexLV &psPar, const MPlexHS &msErr, const MPlexHV &msPar, MPlexLS &outErr, MPlexLV &outPar, MPlexQF &outChi2, const int N_proc)
Definition: KalmanUtilsMPlex.cc:817
mkfit::propagateHelixToZMPlex
void propagateHelixToZMPlex(const MPlexLS &inErr, const MPlexLV &inPar, const MPlexQI &inChg, const MPlexQF &msZ, MPlexLS &outErr, MPlexLV &outPar, MPlexQI &outFailFlag, const int N_proc, const PropagationFlags pflags, const MPlexQI *noMatEffPtr)
Definition: PropagationMPlex.cc:594
Matriplex::Matriplex::constAt
const T & constAt(idx_t n, idx_t i, idx_t j) const
Definition: Matriplex.h:52
mkfit::kalmanUpdate
void kalmanUpdate(const MPlexLS &psErr, const MPlexLV &psPar, const MPlexHS &msErr, const MPlexHV &msPar, MPlexLS &outErr, MPlexLV &outPar, const int N_proc)
Definition: KalmanUtilsMPlex.cc:463
Matriplex::MatriplexSym::At
T & At(idx_t n, idx_t i, idx_t j)
Definition: MatriplexSym.h:71
mkfit::propagateHelixToRMPlex
void propagateHelixToRMPlex(const MPlexLS &inErr, const MPlexLV &inPar, const MPlexQI &inChg, const MPlexQF &msRad, MPlexLS &outErr, MPlexLV &outPar, MPlexQI &outFailFlag, const int N_proc, const PropagationFlags pflags, const MPlexQI *noMatEffPtr)
Definition: PropagationMPlex.cc:487
cms::cuda::assert
assert(be >=bs)
cms::cuda::co
__host__ __device__ VT * co
Definition: prefixScan.h:47
HltBtagPostValidation_cff.c
c
Definition: HltBtagPostValidation_cff.py:31
res
Definition: Electron.h:6
mkfit::squashPhiMPlex
void squashPhiMPlex(MPlexLV &par, const int N_proc)
Definition: PropagationMPlex.h:8
mkfit::kalmanOperation
void kalmanOperation(const int kfOp, const MPlexLS &psErr, const MPlexLV &psPar, const MPlexHS &msErr, const MPlexHV &msPar, MPlexLS &outErr, MPlexLV &outPar, MPlexQF &outChi2, const int N_proc)
Definition: KalmanUtilsMPlex.cc:551
Matriplex::invertCramerSym
void invertCramerSym(MPlexSym< T, D, N > &A, double *determ=nullptr)
Definition: MatriplexSym.h:410
cms_common_macros.h
mkfit::kalmanUpdateEndcap
void kalmanUpdateEndcap(const MPlexLS &psErr, const MPlexLV &psPar, const MPlexHS &msErr, const MPlexHV &msPar, MPlexLS &outErr, MPlexLV &outPar, const int N_proc)
Definition: KalmanUtilsMPlex.cc:731
mkfit::kalmanPropagateAndComputeChi2
void kalmanPropagateAndComputeChi2(const MPlexLS &psErr, const MPlexLV &psPar, const MPlexQI &inChg, const MPlexHS &msErr, const MPlexHV &msPar, MPlexQF &outChi2, MPlexLV &propPar, MPlexQI &outFailFlag, const int N_proc, const PropagationFlags propFlags, const bool propToHit)
Definition: KalmanUtilsMPlex.cc:521
mkfit::kalmanComputeChi2Endcap
void kalmanComputeChi2Endcap(const MPlexLS &psErr, const MPlexLV &psPar, const MPlexQI &inChg, const MPlexHS &msErr, const MPlexHV &msPar, MPlexQF &outChi2, const int N_proc)
Definition: KalmanUtilsMPlex.cc:777
Matriplex::MatriplexSym::constAt
const T & constAt(idx_t n, idx_t i, idx_t j) const
Definition: MatriplexSym.h:69
mkfit::kalmanPropagateAndUpdate
void kalmanPropagateAndUpdate(const MPlexLS &psErr, const MPlexLV &psPar, MPlexQI &Chg, const MPlexHS &msErr, const MPlexHV &msPar, MPlexLS &outErr, MPlexLV &outPar, MPlexQI &outFailFlag, const int N_proc, const PropagationFlags propFlags, const bool propToHit)
Definition: KalmanUtilsMPlex.cc:473
mkfit::NN
constexpr Matriplex::idx_t NN
Definition: Matrix.h:43
mkfit::kalmanPropagateAndComputeChi2Endcap
void kalmanPropagateAndComputeChi2Endcap(const MPlexLS &psErr, const MPlexLV &psPar, const MPlexQI &inChg, const MPlexHS &msErr, const MPlexHV &msPar, MPlexQF &outChi2, MPlexLV &propPar, MPlexQI &outFailFlag, const int N_proc, const PropagationFlags propFlags, const bool propToHit)
Definition: KalmanUtilsMPlex.cc:787
mkfit::kalmanPropagateAndUpdateEndcap
void kalmanPropagateAndUpdateEndcap(const MPlexLS &psErr, const MPlexLV &psPar, MPlexQI &Chg, const MPlexHS &msErr, const MPlexHV &msPar, MPlexLS &outErr, MPlexLV &outPar, MPlexQI &outFailFlag, const int N_proc, const PropagationFlags propFlags, const bool propToHit)
Definition: KalmanUtilsMPlex.cc:741
correctionTermsCaloMet_cff.C
C
Definition: correctionTermsCaloMet_cff.py:34
ztail.d
d
Definition: ztail.py:151
N
#define N
Definition: blowfish.cc:9
funct::D
DecomposeProduct< arg, typename Div::arg > D
Definition: Factorize.h:141
b
double b
Definition: hdecay.h:118
a
double a
Definition: hdecay.h:119
mkfit::kalmanComputeChi2
void kalmanComputeChi2(const MPlexLS &psErr, const MPlexLV &psPar, const MPlexQI &inChg, const MPlexHS &msErr, const MPlexHV &msPar, MPlexQF &outChi2, const int N_proc)
Definition: KalmanUtilsMPlex.cc:511
A
Definition: APVGainStruct.h:7
T
long double T
Definition: Basic3DVectorLD.h:48
ASSUME_ALIGNED
#define ASSUME_ALIGNED(a, b)
Definition: MatriplexCommon.h:82
Matriplex::Matriplex::At
T & At(idx_t n, idx_t i, idx_t j)
Definition: Matriplex.h:54
Generated for CMSSW Reference Manual by doxygen 1.8.14