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  const int N_proc,
481  const PropagationFlags propFlags,
482  const bool propToHit) {
483  if (propToHit) {
484  MPlexLS propErr;
485  MPlexLV propPar;
486  MPlexQF msRad;
487 #pragma omp simd
488  for (int n = 0; n < NN; ++n) {
489  msRad.At(n, 0, 0) = std::hypot(msPar.constAt(n, 0, 0), msPar.constAt(n, 1, 0));
490  }
491 
492  propagateHelixToRMPlex(psErr, psPar, Chg, msRad, propErr, propPar, N_proc, propFlags);
493 
494  kalmanOperation(
495  KFO_Update_Params | KFO_Local_Cov, propErr, propPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
496  } else {
497  kalmanOperation(
498  KFO_Update_Params | KFO_Local_Cov, psErr, psPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
499  }
500  for (int n = 0; n < NN; ++n) {
501  if (outPar.At(n, 3, 0) < 0) {
502  Chg.At(n, 0, 0) = -Chg.At(n, 0, 0);
503  outPar.At(n, 3, 0) = -outPar.At(n, 3, 0);
504  }
505  }
506  }
507 
508  //------------------------------------------------------------------------------
509 
510  void kalmanComputeChi2(const MPlexLS& psErr,
511  const MPlexLV& psPar,
512  const MPlexQI& inChg,
513  const MPlexHS& msErr,
514  const MPlexHV& msPar,
515  MPlexQF& outChi2,
516  const int N_proc) {
517  kalmanOperation(KFO_Calculate_Chi2, psErr, psPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
518  }
519 
520  void kalmanPropagateAndComputeChi2(const MPlexLS& psErr,
521  const MPlexLV& psPar,
522  const MPlexQI& inChg,
523  const MPlexHS& msErr,
524  const MPlexHV& msPar,
525  MPlexQF& outChi2,
526  MPlexLV& propPar,
527  const int N_proc,
528  const PropagationFlags propFlags,
529  const bool propToHit) {
530  propPar = psPar;
531  if (propToHit) {
532  MPlexLS propErr;
533  MPlexQF msRad;
534 #pragma omp simd
535  for (int n = 0; n < NN; ++n) {
536  msRad.At(n, 0, 0) = std::hypot(msPar.constAt(n, 0, 0), msPar.constAt(n, 1, 0));
537  }
538 
539  propagateHelixToRMPlex(psErr, psPar, inChg, msRad, propErr, propPar, N_proc, propFlags);
540 
541  kalmanOperation(KFO_Calculate_Chi2, propErr, propPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
542  } else {
543  kalmanOperation(KFO_Calculate_Chi2, psErr, psPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
544  }
545  }
546 
547  //------------------------------------------------------------------------------
548 
549  void kalmanOperation(const int kfOp,
550  const MPlexLS& psErr,
551  const MPlexLV& psPar,
552  const MPlexHS& msErr,
553  const MPlexHV& msPar,
554  MPlexLS& outErr,
555  MPlexLV& outPar,
556  MPlexQF& outChi2,
557  const int N_proc) {
558 #ifdef DEBUG
559  {
560  dmutex_guard;
561  printf("psPar:\n");
562  for (int i = 0; i < 6; ++i) {
563  printf("%8f ", psPar.constAt(0, 0, i));
564  printf("\n");
565  }
566  printf("\n");
567  printf("psErr:\n");
568  for (int i = 0; i < 6; ++i) {
569  for (int j = 0; j < 6; ++j)
570  printf("%8f ", psErr.constAt(0, i, j));
571  printf("\n");
572  }
573  printf("\n");
574  printf("msPar:\n");
575  for (int i = 0; i < 3; ++i) {
576  printf("%8f ", msPar.constAt(0, 0, i));
577  printf("\n");
578  }
579  printf("\n");
580  printf("msErr:\n");
581  for (int i = 0; i < 3; ++i) {
582  for (int j = 0; j < 3; ++j)
583  printf("%8f ", msErr.constAt(0, i, j));
584  printf("\n");
585  }
586  printf("\n");
587  }
588 #endif
589 
590  // Rotate global point on tangent plane to cylinder
591  // Tangent point is half way between hit and propagate position
592 
593  // Rotation matrix
594  // rotT00 0 rotT01
595  // rotT01 0 -rotT00
596  // 0 1 0
597  // Minimize temporaries: only two float are needed!
598 
599  MPlexQF rotT00;
600  MPlexQF rotT01;
601  for (int n = 0; n < NN; ++n) {
602  const float r = std::hypot(msPar.constAt(n, 0, 0), msPar.constAt(n, 1, 0));
603  rotT00.At(n, 0, 0) = -(msPar.constAt(n, 1, 0) + psPar.constAt(n, 1, 0)) / (2 * r);
604  rotT01.At(n, 0, 0) = (msPar.constAt(n, 0, 0) + psPar.constAt(n, 0, 0)) / (2 * r);
605  }
606 
607  MPlexHV res_glo; //position residual in global coordinates
608  SubtractFirst3(msPar, psPar, res_glo);
609 
610  MPlexHS resErr_glo; //covariance sum in global position coordinates
611  AddIntoUpperLeft3x3(psErr, msErr, resErr_glo);
612 
613  MPlex2V res_loc; //position residual in local coordinates
614  RotateResidualsOnTangentPlane(rotT00, rotT01, res_glo, res_loc);
615  MPlex2S resErr_loc; //covariance sum in local position coordinates
616  MPlexHH tempHH;
617  ProjectResErr(rotT00, rotT01, resErr_glo, tempHH);
618  ProjectResErrTransp(rotT00, rotT01, tempHH, resErr_loc);
619 
620 #ifdef DEBUG
621  {
622  dmutex_guard;
623  printf("resErr_loc:\n");
624  for (int i = 0; i < 2; ++i) {
625  for (int j = 0; j < 2; ++j)
626  printf("%8f ", resErr_loc.At(0, i, j));
627  printf("\n");
628  }
629  printf("\n");
630  }
631 #endif
632 
633  //invert the 2x2 matrix
634  Matriplex::invertCramerSym(resErr_loc);
635 
636  if (kfOp & KFO_Calculate_Chi2) {
637  Chi2Similarity(res_loc, resErr_loc, outChi2);
638 
639 #ifdef DEBUG
640  {
641  dmutex_guard;
642  printf("resErr_loc (Inv):\n");
643  for (int i = 0; i < 2; ++i) {
644  for (int j = 0; j < 2; ++j)
645  printf("%8f ", resErr_loc.At(0, i, j));
646  printf("\n");
647  }
648  printf("\n");
649  printf("chi2: %8f\n", outChi2.At(0, 0, 0));
650  }
651 #endif
652  }
653 
654  if (kfOp & KFO_Update_Params) {
655  MPlexLS psErrLoc = psErr;
656  if (kfOp & KFO_Local_Cov)
657  CovXYconstrain(rotT00, rotT01, psErr, psErrLoc);
658 
659  MPlexLH K; // kalman gain, fixme should be L2
660  KalmanHTG(rotT00, rotT01, resErr_loc, tempHH); // intermediate term to get kalman gain (H^T*G)
661  KalmanGain(psErrLoc, tempHH, K);
662 
663  MultResidualsAdd(K, psPar, res_loc, outPar);
664  MPlexLL tempLL;
665 
666  squashPhiMPlex(outPar, N_proc); // ensure phi is between |pi|
667 
668  KHMult(K, rotT00, rotT01, tempLL);
669  KHC(tempLL, psErrLoc, outErr);
670  outErr.subtract(psErrLoc, outErr);
671 
672 #ifdef DEBUG
673  {
674  dmutex_guard;
675  if (kfOp & KFO_Local_Cov) {
676  printf("psErrLoc:\n");
677  for (int i = 0; i < 6; ++i) {
678  for (int j = 0; j < 6; ++j)
679  printf("% 8e ", psErrLoc.At(0, i, j));
680  printf("\n");
681  }
682  printf("\n");
683  }
684  printf("res_glo:\n");
685  for (int i = 0; i < 3; ++i) {
686  printf("%8f ", res_glo.At(0, i, 0));
687  }
688  printf("\n");
689  printf("res_loc:\n");
690  for (int i = 0; i < 2; ++i) {
691  printf("%8f ", res_loc.At(0, i, 0));
692  }
693  printf("\n");
694  printf("resErr_loc (Inv):\n");
695  for (int i = 0; i < 2; ++i) {
696  for (int j = 0; j < 2; ++j)
697  printf("%8f ", resErr_loc.At(0, i, j));
698  printf("\n");
699  }
700  printf("\n");
701  printf("K:\n");
702  for (int i = 0; i < 6; ++i) {
703  for (int j = 0; j < 3; ++j)
704  printf("%8f ", K.At(0, i, j));
705  printf("\n");
706  }
707  printf("\n");
708  printf("outPar:\n");
709  for (int i = 0; i < 6; ++i) {
710  printf("%8f ", outPar.At(0, i, 0));
711  }
712  printf("\n");
713  printf("outErr:\n");
714  for (int i = 0; i < 6; ++i) {
715  for (int j = 0; j < 6; ++j)
716  printf("%8f ", outErr.At(0, i, j));
717  printf("\n");
718  }
719  printf("\n");
720  }
721 #endif
722  }
723  }
724 
725  //==============================================================================
726  // Kalman operations - Endcap
727  //==============================================================================
728 
729  void kalmanUpdateEndcap(const MPlexLS& psErr,
730  const MPlexLV& psPar,
731  const MPlexHS& msErr,
732  const MPlexHV& msPar,
733  MPlexLS& outErr,
734  MPlexLV& outPar,
735  const int N_proc) {
736  kalmanOperationEndcap(KFO_Update_Params, psErr, psPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
737  }
738 
739  void kalmanPropagateAndUpdateEndcap(const MPlexLS& psErr,
740  const MPlexLV& psPar,
741  MPlexQI& Chg,
742  const MPlexHS& msErr,
743  const MPlexHV& msPar,
744  MPlexLS& outErr,
745  MPlexLV& outPar,
746  const int N_proc,
747  const PropagationFlags propFlags,
748  const bool propToHit) {
749  if (propToHit) {
750  MPlexLS propErr;
751  MPlexLV propPar;
752  MPlexQF msZ;
753 #pragma omp simd
754  for (int n = 0; n < NN; ++n) {
755  msZ.At(n, 0, 0) = msPar.constAt(n, 2, 0);
756  }
757 
758  propagateHelixToZMPlex(psErr, psPar, Chg, msZ, propErr, propPar, N_proc, propFlags);
759 
760  kalmanOperationEndcap(KFO_Update_Params, propErr, propPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
761  } else {
762  kalmanOperationEndcap(KFO_Update_Params, psErr, psPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
763  }
764  for (int n = 0; n < NN; ++n) {
765  if (outPar.At(n, 3, 0) < 0) {
766  Chg.At(n, 0, 0) = -Chg.At(n, 0, 0);
767  outPar.At(n, 3, 0) = -outPar.At(n, 3, 0);
768  }
769  }
770  }
771 
772  //------------------------------------------------------------------------------
773 
774  void kalmanComputeChi2Endcap(const MPlexLS& psErr,
775  const MPlexLV& psPar,
776  const MPlexQI& inChg,
777  const MPlexHS& msErr,
778  const MPlexHV& msPar,
779  MPlexQF& outChi2,
780  const int N_proc) {
781  kalmanOperationEndcap(KFO_Calculate_Chi2, psErr, psPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
782  }
783 
784  void kalmanPropagateAndComputeChi2Endcap(const MPlexLS& psErr,
785  const MPlexLV& psPar,
786  const MPlexQI& inChg,
787  const MPlexHS& msErr,
788  const MPlexHV& msPar,
789  MPlexQF& outChi2,
790  MPlexLV& propPar,
791  const int N_proc,
792  const PropagationFlags propFlags,
793  const bool propToHit) {
794  propPar = psPar;
795  if (propToHit) {
796  MPlexLS propErr;
797  MPlexQF msZ;
798 #pragma omp simd
799  for (int n = 0; n < NN; ++n) {
800  msZ.At(n, 0, 0) = msPar.constAt(n, 2, 0);
801  }
802 
803  propagateHelixToZMPlex(psErr, psPar, inChg, msZ, propErr, propPar, N_proc, propFlags);
804 
805  kalmanOperationEndcap(KFO_Calculate_Chi2, propErr, propPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
806  } else {
807  kalmanOperationEndcap(KFO_Calculate_Chi2, psErr, psPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
808  }
809  }
810 
811  //------------------------------------------------------------------------------
812 
813  void kalmanOperationEndcap(const int kfOp,
814  const MPlexLS& psErr,
815  const MPlexLV& psPar,
816  const MPlexHS& msErr,
817  const MPlexHV& msPar,
818  MPlexLS& outErr,
819  MPlexLV& outPar,
820  MPlexQF& outChi2,
821  const int N_proc) {
822 #ifdef DEBUG
823  {
824  dmutex_guard;
825  printf("updateParametersEndcapMPlex\n");
826  printf("psPar:\n");
827  for (int i = 0; i < 6; ++i) {
828  printf("%8f ", psPar.constAt(0, 0, i));
829  printf("\n");
830  }
831  printf("\n");
832  printf("msPar:\n");
833  for (int i = 0; i < 3; ++i) {
834  printf("%8f ", msPar.constAt(0, 0, i));
835  printf("\n");
836  }
837  printf("\n");
838  printf("psErr:\n");
839  for (int i = 0; i < 6; ++i) {
840  for (int j = 0; j < 6; ++j)
841  printf("%8f ", psErr.constAt(0, i, j));
842  printf("\n");
843  }
844  printf("\n");
845  printf("msErr:\n");
846  for (int i = 0; i < 3; ++i) {
847  for (int j = 0; j < 3; ++j)
848  printf("%8f ", msErr.constAt(0, i, j));
849  printf("\n");
850  }
851  printf("\n");
852  }
853 #endif
854 
855  MPlex2V res;
856  SubtractFirst2(msPar, psPar, res);
857 
858  MPlex2S resErr;
859  AddIntoUpperLeft2x2(psErr, msErr, resErr);
860 
861 #ifdef DEBUG
862  {
863  dmutex_guard;
864  printf("resErr:\n");
865  for (int i = 0; i < 2; ++i) {
866  for (int j = 0; j < 2; ++j)
867  printf("%8f ", resErr.At(0, i, j));
868  printf("\n");
869  }
870  printf("\n");
871  }
872 #endif
873 
874  //invert the 2x2 matrix
875  Matriplex::invertCramerSym(resErr);
876 
877  if (kfOp & KFO_Calculate_Chi2) {
878  Chi2Similarity(res, resErr, outChi2);
879 
880 #ifdef DEBUG
881  {
882  dmutex_guard;
883  printf("resErr_loc (Inv):\n");
884  for (int i = 0; i < 2; ++i) {
885  for (int j = 0; j < 2; ++j)
886  printf("%8f ", resErr.At(0, i, j));
887  printf("\n");
888  }
889  printf("\n");
890  printf("chi2: %8f\n", outChi2.At(0, 0, 0));
891  }
892 #endif
893  }
894 
895  if (kfOp & KFO_Update_Params) {
896  MPlexL2 K;
897  KalmanGain(psErr, resErr, K);
898 
899  MultResidualsAdd(K, psPar, res, outPar);
900 
901  squashPhiMPlex(outPar, N_proc); // ensure phi is between |pi|
902 
903  KHC(K, psErr, outErr);
904 
905 #ifdef DEBUG
906  {
907  dmutex_guard;
908  printf("outErr before subtract:\n");
909  for (int i = 0; i < 6; ++i) {
910  for (int j = 0; j < 6; ++j)
911  printf("%8f ", outErr.At(0, i, j));
912  printf("\n");
913  }
914  printf("\n");
915  }
916 #endif
917 
918  outErr.subtract(psErr, outErr);
919 
920 #ifdef DEBUG
921  {
922  dmutex_guard;
923  printf("res:\n");
924  for (int i = 0; i < 2; ++i) {
925  printf("%8f ", res.At(0, i, 0));
926  }
927  printf("\n");
928  printf("resErr (Inv):\n");
929  for (int i = 0; i < 2; ++i) {
930  for (int j = 0; j < 2; ++j)
931  printf("%8f ", resErr.At(0, i, j));
932  printf("\n");
933  }
934  printf("\n");
935  printf("K:\n");
936  for (int i = 0; i < 6; ++i) {
937  for (int j = 0; j < 2; ++j)
938  printf("%8f ", K.At(0, i, j));
939  printf("\n");
940  }
941  printf("\n");
942  printf("outPar:\n");
943  for (int i = 0; i < 6; ++i) {
944  printf("%8f ", outPar.At(0, i, 0));
945  }
946  printf("\n");
947  printf("outErr:\n");
948  for (int i = 0; i < 6; ++i) {
949  for (int j = 0; j < 6; ++j)
950  printf("%8f ", outErr.At(0, i, j));
951  printf("\n");
952  }
953  printf("\n");
954  }
955 #endif
956  }
957  }
958 
959 } // end namespace mkfit
mkfit::kalmanPropagateAndUpdate
void kalmanPropagateAndUpdate(const MPlexLS &psErr, const MPlexLV &psPar, MPlexQI &Chg, const MPlexHS &msErr, const MPlexHV &msPar, MPlexLS &outErr, MPlexLV &outPar, const int N_proc, const PropagationFlags propFlags, const bool propToHit)
Definition: KalmanUtilsMPlex.cc:473
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:813
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::kalmanPropagateAndComputeChi2Endcap
void kalmanPropagateAndComputeChi2Endcap(const MPlexLS &psErr, const MPlexLV &psPar, const MPlexQI &inChg, const MPlexHS &msErr, const MPlexHV &msPar, MPlexQF &outChi2, MPlexLV &propPar, const int N_proc, const PropagationFlags propFlags, const bool propToHit)
Definition: KalmanUtilsMPlex.cc:784
mkfit::propagateHelixToRMPlex
void propagateHelixToRMPlex(const MPlexLS &inErr, const MPlexLV &inPar, const MPlexQI &inChg, const MPlexQF &msRad, MPlexLS &outErr, MPlexLV &outPar, 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
res
Definition: Electron.h:6
mkfit::squashPhiMPlex
void squashPhiMPlex(MPlexLV &par, const int N_proc)
Definition: PropagationMPlex.h:8
mkfit::kalmanPropagateAndComputeChi2
void kalmanPropagateAndComputeChi2(const MPlexLS &psErr, const MPlexLV &psPar, const MPlexQI &inChg, const MPlexHS &msErr, const MPlexHV &msPar, MPlexQF &outChi2, MPlexLV &propPar, const int N_proc, const PropagationFlags propFlags, const bool propToHit)
Definition: KalmanUtilsMPlex.cc:520
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:549
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:729
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:774
Matriplex::MatriplexSym::constAt
const T & constAt(idx_t n, idx_t i, idx_t j) const
Definition: MatriplexSym.h:69
mkfit::NN
constexpr Matriplex::idx_t NN
Definition: Matrix.h:43
mkfit::kalmanPropagateAndUpdateEndcap
void kalmanPropagateAndUpdateEndcap(const MPlexLS &psErr, const MPlexLV &psPar, MPlexQI &Chg, const MPlexHS &msErr, const MPlexHV &msPar, MPlexLS &outErr, MPlexLV &outPar, const int N_proc, const PropagationFlags propFlags, const bool propToHit)
Definition: KalmanUtilsMPlex.cc:739
correctionTermsCaloMet_cff.C
C
Definition: correctionTermsCaloMet_cff.py:34
ztail.d
d
Definition: ztail.py:151
mkfit::propagateHelixToZMPlex
void propagateHelixToZMPlex(const MPlexLS &inErr, const MPlexLV &inPar, const MPlexQI &inChg, const MPlexQF &msZ, MPlexLS &outErr, MPlexLV &outPar, const int N_proc, const PropagationFlags pflags, const MPlexQI *noMatEffPtr)
Definition: PropagationMPlex.cc:595
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:510
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