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  void KalmanGain(const MPlexLS& A, const MPlex2S& B, MPlexL2& C) {
308  // C = A * B, C is 6x2, A is 6x6 sym , B is 2x2
309 
310  typedef float T;
311  const idx_t N = NN;
312 
313  const T* a = A.fArray;
314  ASSUME_ALIGNED(a, 64);
315  const T* b = B.fArray;
316  ASSUME_ALIGNED(b, 64);
317  T* c = C.fArray;
318  ASSUME_ALIGNED(c, 64);
319 
320 #include "KalmanGain62.ah"
321  }
322 
323  inline void KHMult(const MPlexLH& A, const MPlexQF& B00, const MPlexQF& B01, MPlexLL& C) {
324  // C = A * B, C is 6x6, A is 6x3 , B is 3x6
325  KHMult_imp(A, B00, B01, C, 0, NN);
326  }
327 
328  inline void KHC(const MPlexLL& A, const MPlexLS& B, MPlexLS& C) {
329  // C = A * B, C is 6x6, A is 6x6 , B is 6x6 sym
330 
331  typedef float T;
332  const idx_t N = NN;
333 
334  const T* a = A.fArray;
335  ASSUME_ALIGNED(a, 64);
336  const T* b = B.fArray;
337  ASSUME_ALIGNED(b, 64);
338  T* c = C.fArray;
339  ASSUME_ALIGNED(c, 64);
340 
341 #include "KHC.ah"
342  }
343 
344  inline void KHC(const MPlexL2& A, const MPlexLS& B, MPlexLS& C) {
345  // C = A * B, C is 6x6 sym, A is 6x2 , B is 6x6 sym
346 
347  typedef float T;
348  const idx_t N = NN;
349 
350  const T* a = A.fArray;
351  ASSUME_ALIGNED(a, 64);
352  const T* b = B.fArray;
353  ASSUME_ALIGNED(b, 64);
354  T* c = C.fArray;
355  ASSUME_ALIGNED(c, 64);
356 
357 #include "K62HC.ah"
358  }
359 
360  //Warning: MultFull is not vectorized!
361  template <typename T1, typename T2, typename T3>
362  void MultFull(const T1& A, int nia, int nja, const T2& B, int nib, int njb, T3& C, int nic, int njc) {
363 #ifdef DEBUG
364  assert(nja == nib);
365  assert(nia == nic);
366  assert(njb == njc);
367 #endif
368  for (int n = 0; n < NN; ++n) {
369  for (int i = 0; i < nia; ++i) {
370  for (int j = 0; j < njb; ++j) {
371  C(n, i, j) = 0.;
372  for (int k = 0; k < nja; ++k)
373  C(n, i, j) += A.constAt(n, i, k) * B.constAt(n, k, j);
374  }
375  }
376  }
377  }
378 
379  //Warning: MultTranspFull is not vectorized!
380  // (careful about which one is transposed, I think rows and cols are swapped and the one that is transposed is A)
381  template <typename T1, typename T2, typename T3>
382  void MultTranspFull(const T1& A, int nia, int nja, const T2& B, int nib, int njb, T3& C, int nic, int njc) {
383 #ifdef DEBUG
384  assert(nja == njb);
385  assert(nia == nic);
386  assert(nib == njc);
387 #endif
388  for (int n = 0; n < NN; ++n) {
389  for (int i = 0; i < nia; ++i) {
390  for (int j = 0; j < nib; ++j) {
391  C(n, i, j) = 0.;
392  for (int k = 0; k < nja; ++k)
393  C(n, i, j) += A.constAt(n, i, k) * B.constAt(n, j, k);
394  }
395  }
396  }
397  }
398 
399 } // namespace
400 
401 //==============================================================================
402 // Kalman operations - common dummy variables
403 //==============================================================================
404 
405 namespace {
406  // Dummy variables for parameter consistency to kalmanOperation.
407  // Through KalmanFilterOperation enum parameter it is guaranteed that
408  // those will never get accessed in the code (read from or written into).
409 
410  CMS_SA_ALLOW mkfit::MPlexLS dummy_err;
411  CMS_SA_ALLOW mkfit::MPlexLV dummy_par;
412  CMS_SA_ALLOW mkfit::MPlexQF dummy_chi2;
413 } // namespace
414 
415 namespace mkfit {
416 
417  //==============================================================================
418  // Kalman operations - Barrel
419  //==============================================================================
420 
421  void kalmanUpdate(const MPlexLS& psErr,
422  const MPlexLV& psPar,
423  const MPlexHS& msErr,
424  const MPlexHV& msPar,
425  MPlexLS& outErr,
426  MPlexLV& outPar,
427  const int N_proc) {
428  kalmanOperation(KFO_Update_Params, psErr, psPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
429  }
430 
431  void kalmanPropagateAndUpdate(const MPlexLS& psErr,
432  const MPlexLV& psPar,
433  MPlexQI& Chg,
434  const MPlexHS& msErr,
435  const MPlexHV& msPar,
436  MPlexLS& outErr,
437  MPlexLV& outPar,
438  const int N_proc,
439  const PropagationFlags propFlags,
440  const bool propToHit) {
441  if (propToHit) {
442  MPlexLS propErr;
443  MPlexLV propPar;
444  MPlexQF msRad;
445 #pragma omp simd
446  for (int n = 0; n < NN; ++n) {
447  msRad.At(n, 0, 0) = std::hypot(msPar.constAt(n, 0, 0), msPar.constAt(n, 1, 0));
448  }
449 
450  propagateHelixToRMPlex(psErr, psPar, Chg, msRad, propErr, propPar, N_proc, propFlags);
451 
452  kalmanOperation(KFO_Update_Params, propErr, propPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
453  } else {
454  kalmanOperation(KFO_Update_Params, psErr, psPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
455  }
456  for (int n = 0; n < NN; ++n) {
457  if (outPar.At(n, 3, 0) < 0) {
458  Chg.At(n, 0, 0) = -Chg.At(n, 0, 0);
459  outPar.At(n, 3, 0) = -outPar.At(n, 3, 0);
460  }
461  }
462  }
463 
464  //------------------------------------------------------------------------------
465 
466  void kalmanComputeChi2(const MPlexLS& psErr,
467  const MPlexLV& psPar,
468  const MPlexQI& inChg,
469  const MPlexHS& msErr,
470  const MPlexHV& msPar,
471  MPlexQF& outChi2,
472  const int N_proc) {
473  kalmanOperation(KFO_Calculate_Chi2, psErr, psPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
474  }
475 
476  void kalmanPropagateAndComputeChi2(const MPlexLS& psErr,
477  const MPlexLV& psPar,
478  const MPlexQI& inChg,
479  const MPlexHS& msErr,
480  const MPlexHV& msPar,
481  MPlexQF& outChi2,
482  MPlexLV& propPar,
483  const int N_proc,
484  const PropagationFlags propFlags,
485  const bool propToHit) {
486  propPar = psPar;
487  if (propToHit) {
488  MPlexLS propErr;
489  MPlexQF msRad;
490 #pragma omp simd
491  for (int n = 0; n < NN; ++n) {
492  msRad.At(n, 0, 0) = std::hypot(msPar.constAt(n, 0, 0), msPar.constAt(n, 1, 0));
493  }
494 
495  propagateHelixToRMPlex(psErr, psPar, inChg, msRad, propErr, propPar, N_proc, propFlags);
496 
497  kalmanOperation(KFO_Calculate_Chi2, propErr, propPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
498  } else {
499  kalmanOperation(KFO_Calculate_Chi2, psErr, psPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
500  }
501  }
502 
503  //------------------------------------------------------------------------------
504 
505  void kalmanOperation(const int kfOp,
506  const MPlexLS& psErr,
507  const MPlexLV& psPar,
508  const MPlexHS& msErr,
509  const MPlexHV& msPar,
510  MPlexLS& outErr,
511  MPlexLV& outPar,
512  MPlexQF& outChi2,
513  const int N_proc) {
514 #ifdef DEBUG
515  {
516  dmutex_guard;
517  printf("psPar:\n");
518  for (int i = 0; i < 6; ++i) {
519  printf("%8f ", psPar.constAt(0, 0, i));
520  printf("\n");
521  }
522  printf("\n");
523  printf("psErr:\n");
524  for (int i = 0; i < 6; ++i) {
525  for (int j = 0; j < 6; ++j)
526  printf("%8f ", psErr.constAt(0, i, j));
527  printf("\n");
528  }
529  printf("\n");
530  printf("msPar:\n");
531  for (int i = 0; i < 3; ++i) {
532  printf("%8f ", msPar.constAt(0, 0, i));
533  printf("\n");
534  }
535  printf("\n");
536  printf("msErr:\n");
537  for (int i = 0; i < 3; ++i) {
538  for (int j = 0; j < 3; ++j)
539  printf("%8f ", msErr.constAt(0, i, j));
540  printf("\n");
541  }
542  printf("\n");
543  }
544 #endif
545 
546  // Rotate global point on tangent plane to cylinder
547  // Tangent point is half way between hit and propagate position
548 
549  // Rotation matrix
550  // rotT00 0 rotT01
551  // rotT01 0 -rotT00
552  // 0 1 0
553  // Minimize temporaries: only two float are needed!
554 
555  MPlexQF rotT00;
556  MPlexQF rotT01;
557  for (int n = 0; n < NN; ++n) {
558  const float r = std::hypot(msPar.constAt(n, 0, 0), msPar.constAt(n, 1, 0));
559  rotT00.At(n, 0, 0) = -(msPar.constAt(n, 1, 0) + psPar.constAt(n, 1, 0)) / (2 * r);
560  rotT01.At(n, 0, 0) = (msPar.constAt(n, 0, 0) + psPar.constAt(n, 0, 0)) / (2 * r);
561  }
562 
563  MPlexHV res_glo; //position residual in global coordinates
564  SubtractFirst3(msPar, psPar, res_glo);
565 
566  MPlexHS resErr_glo; //covariance sum in global position coordinates
567  AddIntoUpperLeft3x3(psErr, msErr, resErr_glo);
568 
569  MPlex2V res_loc; //position residual in local coordinates
570  RotateResidualsOnTangentPlane(rotT00, rotT01, res_glo, res_loc);
571  MPlex2S resErr_loc; //covariance sum in local position coordinates
572  MPlexHH tempHH;
573  ProjectResErr(rotT00, rotT01, resErr_glo, tempHH);
574  ProjectResErrTransp(rotT00, rotT01, tempHH, resErr_loc);
575 
576 #ifdef DEBUG
577  {
578  dmutex_guard;
579  printf("resErr_loc:\n");
580  for (int i = 0; i < 2; ++i) {
581  for (int j = 0; j < 2; ++j)
582  printf("%8f ", resErr_loc.At(0, i, j));
583  printf("\n");
584  }
585  printf("\n");
586  }
587 #endif
588 
589  //invert the 2x2 matrix
590  Matriplex::invertCramerSym(resErr_loc);
591 
592  if (kfOp & KFO_Calculate_Chi2) {
593  Chi2Similarity(res_loc, resErr_loc, outChi2);
594 
595 #ifdef DEBUG
596  {
597  dmutex_guard;
598  printf("resErr_loc (Inv):\n");
599  for (int i = 0; i < 2; ++i) {
600  for (int j = 0; j < 2; ++j)
601  printf("%8f ", resErr_loc.At(0, i, j));
602  printf("\n");
603  }
604  printf("\n");
605  printf("chi2: %8f\n", outChi2.At(0, 0, 0));
606  }
607 #endif
608  }
609 
610  if (kfOp & KFO_Update_Params) {
611  MPlexLH K; // kalman gain, fixme should be L2
612  KalmanHTG(rotT00, rotT01, resErr_loc, tempHH); // intermediate term to get kalman gain (H^T*G)
613  KalmanGain(psErr, tempHH, K);
614 
615  MultResidualsAdd(K, psPar, res_loc, outPar);
616  MPlexLL tempLL;
617 
618  squashPhiMPlex(outPar, N_proc); // ensure phi is between |pi|
619 
620  KHMult(K, rotT00, rotT01, tempLL);
621  KHC(tempLL, psErr, outErr);
622  outErr.subtract(psErr, outErr);
623 
624 #ifdef DEBUG
625  {
626  dmutex_guard;
627  printf("res_glo:\n");
628  for (int i = 0; i < 3; ++i) {
629  printf("%8f ", res_glo.At(0, i, 0));
630  }
631  printf("\n");
632  printf("res_loc:\n");
633  for (int i = 0; i < 2; ++i) {
634  printf("%8f ", res_loc.At(0, i, 0));
635  }
636  printf("\n");
637  printf("resErr_loc (Inv):\n");
638  for (int i = 0; i < 2; ++i) {
639  for (int j = 0; j < 2; ++j)
640  printf("%8f ", resErr_loc.At(0, i, j));
641  printf("\n");
642  }
643  printf("\n");
644  printf("K:\n");
645  for (int i = 0; i < 6; ++i) {
646  for (int j = 0; j < 3; ++j)
647  printf("%8f ", K.At(0, i, j));
648  printf("\n");
649  }
650  printf("\n");
651  printf("outPar:\n");
652  for (int i = 0; i < 6; ++i) {
653  printf("%8f ", outPar.At(0, i, 0));
654  }
655  printf("\n");
656  printf("outErr:\n");
657  for (int i = 0; i < 6; ++i) {
658  for (int j = 0; j < 6; ++j)
659  printf("%8f ", outErr.At(0, i, j));
660  printf("\n");
661  }
662  printf("\n");
663  }
664 #endif
665  }
666  }
667 
668  //==============================================================================
669  // Kalman operations - Endcap
670  //==============================================================================
671 
672  void kalmanUpdateEndcap(const MPlexLS& psErr,
673  const MPlexLV& psPar,
674  const MPlexHS& msErr,
675  const MPlexHV& msPar,
676  MPlexLS& outErr,
677  MPlexLV& outPar,
678  const int N_proc) {
679  kalmanOperationEndcap(KFO_Update_Params, psErr, psPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
680  }
681 
682  void kalmanPropagateAndUpdateEndcap(const MPlexLS& psErr,
683  const MPlexLV& psPar,
684  MPlexQI& Chg,
685  const MPlexHS& msErr,
686  const MPlexHV& msPar,
687  MPlexLS& outErr,
688  MPlexLV& outPar,
689  const int N_proc,
690  const PropagationFlags propFlags,
691  const bool propToHit) {
692  if (propToHit) {
693  MPlexLS propErr;
694  MPlexLV propPar;
695  MPlexQF msZ;
696 #pragma omp simd
697  for (int n = 0; n < NN; ++n) {
698  msZ.At(n, 0, 0) = msPar.constAt(n, 2, 0);
699  }
700 
701  propagateHelixToZMPlex(psErr, psPar, Chg, msZ, propErr, propPar, N_proc, propFlags);
702 
703  kalmanOperationEndcap(KFO_Update_Params, propErr, propPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
704  } else {
705  kalmanOperationEndcap(KFO_Update_Params, psErr, psPar, msErr, msPar, outErr, outPar, dummy_chi2, N_proc);
706  }
707  for (int n = 0; n < NN; ++n) {
708  if (outPar.At(n, 3, 0) < 0) {
709  Chg.At(n, 0, 0) = -Chg.At(n, 0, 0);
710  outPar.At(n, 3, 0) = -outPar.At(n, 3, 0);
711  }
712  }
713  }
714 
715  //------------------------------------------------------------------------------
716 
717  void kalmanComputeChi2Endcap(const MPlexLS& psErr,
718  const MPlexLV& psPar,
719  const MPlexQI& inChg,
720  const MPlexHS& msErr,
721  const MPlexHV& msPar,
722  MPlexQF& outChi2,
723  const int N_proc) {
724  kalmanOperationEndcap(KFO_Calculate_Chi2, psErr, psPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
725  }
726 
727  void kalmanPropagateAndComputeChi2Endcap(const MPlexLS& psErr,
728  const MPlexLV& psPar,
729  const MPlexQI& inChg,
730  const MPlexHS& msErr,
731  const MPlexHV& msPar,
732  MPlexQF& outChi2,
733  MPlexLV& propPar,
734  const int N_proc,
735  const PropagationFlags propFlags,
736  const bool propToHit) {
737  propPar = psPar;
738  if (propToHit) {
739  MPlexLS propErr;
740  MPlexQF msZ;
741 #pragma omp simd
742  for (int n = 0; n < NN; ++n) {
743  msZ.At(n, 0, 0) = msPar.constAt(n, 2, 0);
744  }
745 
746  propagateHelixToZMPlex(psErr, psPar, inChg, msZ, propErr, propPar, N_proc, propFlags);
747 
748  kalmanOperationEndcap(KFO_Calculate_Chi2, propErr, propPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
749  } else {
750  kalmanOperationEndcap(KFO_Calculate_Chi2, psErr, psPar, msErr, msPar, dummy_err, dummy_par, outChi2, N_proc);
751  }
752  }
753 
754  //------------------------------------------------------------------------------
755 
756  void kalmanOperationEndcap(const int kfOp,
757  const MPlexLS& psErr,
758  const MPlexLV& psPar,
759  const MPlexHS& msErr,
760  const MPlexHV& msPar,
761  MPlexLS& outErr,
762  MPlexLV& outPar,
763  MPlexQF& outChi2,
764  const int N_proc) {
765 #ifdef DEBUG
766  {
767  dmutex_guard;
768  printf("updateParametersEndcapMPlex\n");
769  printf("psPar:\n");
770  for (int i = 0; i < 6; ++i) {
771  printf("%8f ", psPar.constAt(0, 0, i));
772  printf("\n");
773  }
774  printf("\n");
775  printf("msPar:\n");
776  for (int i = 0; i < 3; ++i) {
777  printf("%8f ", msPar.constAt(0, 0, i));
778  printf("\n");
779  }
780  printf("\n");
781  printf("psErr:\n");
782  for (int i = 0; i < 6; ++i) {
783  for (int j = 0; j < 6; ++j)
784  printf("%8f ", psErr.constAt(0, i, j));
785  printf("\n");
786  }
787  printf("\n");
788  printf("msErr:\n");
789  for (int i = 0; i < 3; ++i) {
790  for (int j = 0; j < 3; ++j)
791  printf("%8f ", msErr.constAt(0, i, j));
792  printf("\n");
793  }
794  printf("\n");
795  }
796 #endif
797 
798  MPlex2V res;
799  SubtractFirst2(msPar, psPar, res);
800 
801  MPlex2S resErr;
802  AddIntoUpperLeft2x2(psErr, msErr, resErr);
803 
804 #ifdef DEBUG
805  {
806  dmutex_guard;
807  printf("resErr:\n");
808  for (int i = 0; i < 2; ++i) {
809  for (int j = 0; j < 2; ++j)
810  printf("%8f ", resErr.At(0, i, j));
811  printf("\n");
812  }
813  printf("\n");
814  }
815 #endif
816 
817  //invert the 2x2 matrix
818  Matriplex::invertCramerSym(resErr);
819 
820  if (kfOp & KFO_Calculate_Chi2) {
821  Chi2Similarity(res, resErr, outChi2);
822 
823 #ifdef DEBUG
824  {
825  dmutex_guard;
826  printf("resErr_loc (Inv):\n");
827  for (int i = 0; i < 2; ++i) {
828  for (int j = 0; j < 2; ++j)
829  printf("%8f ", resErr.At(0, i, j));
830  printf("\n");
831  }
832  printf("\n");
833  printf("chi2: %8f\n", outChi2.At(0, 0, 0));
834  }
835 #endif
836  }
837 
838  if (kfOp & KFO_Update_Params) {
839  MPlexL2 K;
840  KalmanGain(psErr, resErr, K);
841 
842  MultResidualsAdd(K, psPar, res, outPar);
843 
844  squashPhiMPlex(outPar, N_proc); // ensure phi is between |pi|
845 
846  KHC(K, psErr, outErr);
847 
848 #ifdef DEBUG
849  {
850  dmutex_guard;
851  printf("outErr before subtract:\n");
852  for (int i = 0; i < 6; ++i) {
853  for (int j = 0; j < 6; ++j)
854  printf("%8f ", outErr.At(0, i, j));
855  printf("\n");
856  }
857  printf("\n");
858  }
859 #endif
860 
861  outErr.subtract(psErr, outErr);
862 
863 #ifdef DEBUG
864  {
865  dmutex_guard;
866  printf("res:\n");
867  for (int i = 0; i < 2; ++i) {
868  printf("%8f ", res.At(0, i, 0));
869  }
870  printf("\n");
871  printf("resErr (Inv):\n");
872  for (int i = 0; i < 2; ++i) {
873  for (int j = 0; j < 2; ++j)
874  printf("%8f ", resErr.At(0, i, j));
875  printf("\n");
876  }
877  printf("\n");
878  printf("K:\n");
879  for (int i = 0; i < 6; ++i) {
880  for (int j = 0; j < 2; ++j)
881  printf("%8f ", K.At(0, i, j));
882  printf("\n");
883  }
884  printf("\n");
885  printf("outPar:\n");
886  for (int i = 0; i < 6; ++i) {
887  printf("%8f ", outPar.At(0, i, 0));
888  }
889  printf("\n");
890  printf("outErr:\n");
891  for (int i = 0; i < 6; ++i) {
892  for (int j = 0; j < 6; ++j)
893  printf("%8f ", outErr.At(0, i, j));
894  printf("\n");
895  }
896  printf("\n");
897  }
898 #endif
899  }
900  }
901 
902 } // 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:431
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:428
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:756
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:421
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:727
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)
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:476
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:505
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:672
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:717
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:682
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:466
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