EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
CylinderKalman.cpp
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file CylinderKalman.cpp
1 #include "CylinderKalman.h"
2 
3 #include "HelixKalmanState.h"
4 #include "SimpleHit3D.h"
5 
6 #include <Eigen/Core>
7 #include <Eigen/LU>
8 
9 #include <algorithm>
10 #include <cmath>
11 
12 using namespace std;
13 using namespace Eigen;
14 
15 static inline float sign(float x) {
16  return ((float)(x > 0.)) - ((float)(x < 0.));
17 }
18 
19 CylinderKalman::CylinderKalman(vector<float>& detector_radii,
20  vector<float>& detector_material, float B)
21  : HelixKalman(B), nlayers(detector_radii.size()), signk_store(0.) {
22  det_rad = detector_radii;
23  det_scatter_variance = detector_material;
24  for (unsigned int i = 0; i < det_scatter_variance.size(); ++i) {
25  det_scatter_variance[i] = det_scatter_variance[i] * 0.0136 * 0.0136;
26  }
27 }
28 
29 CylinderKalman::~CylinderKalman() {}
30 
31 void CylinderKalman::calculate_dxda(SimpleHit3D& hit, HelixKalmanState& state,
32  Matrix<float, 3, 5>& dxda, float& x,
33  float& y, float& z) {
34  float phi = state.phi;
35  float d = state.d;
36  float k = state.kappa;
37  float z0 = state.z0;
38  float dzdl = state.dzdl;
39 
40  float rad_det = det_rad[hit.get_layer()];
41 
42  float cosphi = cos(phi);
43  float sinphi = sin(phi);
44 
45  // float signk = (float)(sign(k));
46  k = fabs(k);
47 
48  float kd = (d * k + 1.);
49  float kcx = kd * cosphi;
50  float kcy = kd * sinphi;
51  float kd_inv = 1. / kd;
52  float R2 = rad_det * rad_det;
53  float a = 0.5 * (k * R2 + (d * d * k + 2. * d)) * kd_inv;
54  float tmp1 = a * kd_inv;
55  float P2x = kcx * tmp1;
56  float P2y = kcy * tmp1;
57 
58  float h = sqrt(R2 - a * a);
59 
60  float ux = -kcy * kd_inv;
61  float uy = kcx * kd_inv;
62 
63  float x1 = P2x + ux * h;
64  float y1 = P2y + uy * h;
65  float x2 = P2x - ux * h;
66  float y2 = P2y - uy * h;
67  float diff1 = (x1 - hit.get_x()) * (x1 - hit.get_x()) +
68  (y1 - hit.get_y()) * (y1 - hit.get_y());
69  float diff2 = (x2 - hit.get_x()) * (x2 - hit.get_x()) +
70  (y2 - hit.get_y()) * (y2 - hit.get_y());
71  float signk = 0.;
72  if (diff1 < diff2) {
73  signk = 1.;
74  } else {
75  signk = -1.;
76  }
77  signk_store = signk;
78  x = P2x + signk * ux * h;
79  y = P2y + signk * uy * h;
80 
81  // dx/dphi
82 
83  // (d/dA)[ (d*k +1.)*cos(phi) ]
84  float dkcxdA = -kd * sinphi;
85  // (d/dA)[ (d*k +1.)*sin(phi) ]
86  float dkcydA = kd * cosphi;
87  // (d/dA)[ 1/(k*d + 1) ]
88  float dkd_invdA = 0.;
89  // 0.5*[ (k*R2 + ( d*d*k + 2.*d ))*dkd_inv/dA + kd_inv*(R2*dk/dA +
90  // 2*dd/dA*(k*d + 1) + (d^2)*dk/dA) ]
91  float dadA = 0.;
92  float dtmp1dA = dadA * kd_inv + a * dkd_invdA;
93  float dP2xdA = tmp1 * dkcxdA + kcx * dtmp1dA;
94  float duxdA = -kd_inv * dkcydA - kcy * dkd_invdA;
95  float dhdA = (0.5 / sqrt(R2 - a * a)) * (-2. * a * dadA);
96  dxda(0, 0) = dP2xdA + signk * (duxdA * h + ux * dhdA);
97 
98  // dy/dphi
99 
100  float duydA = kd_inv * dkcxdA + kcx * dkd_invdA;
101  float dP2ydA = tmp1 * dkcydA + kcy * dtmp1dA;
102  dxda(1, 0) = dP2ydA + signk * (duydA * h + uy * dhdA);
103 
104  // dx/d
105 
106  // (d/dA)[ (d*k +1.)*cos(phi) ]
107  dkcxdA = cosphi * k;
108  // (d/dA)[ (d*k +1.)*sin(phi) ]
109  dkcydA = sinphi * k;
110  // (d/dA)[ 1/(k*d + 1) ]
111  dkd_invdA = -kd_inv * kd_inv * k;
112  // 0.5*[ (k*R2 + ( d*d*k + 2.*d ))*dkd_inv/dA + kd_inv*(R2*dk/dA +
113  // 2*dd/dA*(k*d + 1) + (d^2)*dk/dA) ]
114  dadA =
115  0.5 * ((k * R2 + (d * d * k + 2. * d)) * dkd_invdA + kd_inv * (2. * kd));
116  dtmp1dA = dadA * kd_inv + a * dkd_invdA;
117  dP2xdA = tmp1 * dkcxdA + kcx * dtmp1dA;
118  duxdA = -kd_inv * dkcydA - kcy * dkd_invdA;
119  dhdA = (0.5 / sqrt(R2 - a * a)) * (-2. * a * dadA);
120  dxda(0, 1) = dP2xdA + signk * (duxdA * h + ux * dhdA);
121 
122  // dy/d
123 
124  duydA = kd_inv * dkcxdA + kcx * dkd_invdA;
125  dP2ydA = tmp1 * dkcydA + kcy * dtmp1dA;
126  dxda(1, 1) = dP2ydA + signk * (duydA * h + uy * dhdA);
127 
128  // dx/dk
129 
130  // (d/dA)[ (d*k +1.)*cos(phi) ]
131  dkcxdA = cosphi * d;
132  // (d/dA)[ (d*k +1.)*sin(phi) ]
133  dkcydA = sinphi * d;
134  // (d/dA)[ 1/(k*d + 1) ]
135  dkd_invdA = -kd_inv * kd_inv * d;
136  // 0.5*[ (k*R2 + ( d*d*k + 2.*d ))*dkd_inv/dA + kd_inv*(R2*dk/dA +
137  // 2*dd/dA*(k*d + 1) + (d^2)*dk/dA) ]
138  dadA = 0.5 *
139  ((k * R2 + (d * d * k + 2. * d)) * dkd_invdA + kd_inv * (R2 + d * d));
140  dtmp1dA = dadA * kd_inv + a * dkd_invdA;
141  dP2xdA = tmp1 * dkcxdA + kcx * dtmp1dA;
142  duxdA = -kd_inv * dkcydA - kcy * dkd_invdA;
143  dhdA = (0.5 / sqrt(R2 - a * a)) * (-2. * a * dadA);
144  dxda(0, 2) = dP2xdA + signk * (duxdA * h + ux * dhdA);
145 
146  // dy/k
147 
148  duydA = kd_inv * dkcxdA + kcx * dkd_invdA;
149  dP2ydA = tmp1 * dkcydA + kcy * dtmp1dA;
150  dxda(1, 2) = dP2ydA + signk * (duydA * h + uy * dhdA);
151 
152  // now for the z direction
153 
154  float sign_dzdl = sign(dzdl);
155  float onedzdl2_inv = 1. / (1. - dzdl * dzdl);
156  float startx = d * cosphi;
157  float starty = d * sinphi;
158  float D = sqrt((startx - x) * (startx - x) + (starty - y) * (starty - y));
159  float D_inv = 1. / D;
160  float v = 0.5 * k * D;
161  z = 0.;
162  if (v > 0.1) {
163  if (v >= 0.999999) {
164  v = 0.999999;
165  }
166  float s = 2. * asin(v) / k;
167  float s_inv = 1. / s;
168  float sqrtvv = sqrt(1 - v * v);
169  float dz = sqrt(s * s * dzdl * dzdl / (1. - dzdl * dzdl));
170  z = z0 + sign_dzdl * dz;
171  float dz_inv = 1. / dz;
172  float dz2 = dz * dz;
173 
174  // phi
175  float dstartxdA = -d * sinphi;
176  float dstartydA = d * cosphi;
177  float dkdA = 0.;
178  float ddzdldA = 0.;
179  float dz0dA = 0.;
180 
181  float dDdA = 0.5 * D_inv * (2. * (startx - x) * dstartxdA +
182  2. * (starty - y) * dstartydA);
183  float dvdA = 0.5 * (k * dDdA + D * dkdA);
184  float dsdA = (2. / (k * sqrtvv)) * dvdA - (s / k) * dkdA;
185  float ddzdA = 0.5 * dz_inv *
186  (2. * (dsdA)*dz2 * s_inv +
187  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
188  dxda(2, 0) = dz0dA + sign_dzdl * ddzdA;
189 
190  // d
191  dstartxdA = cosphi;
192  dstartydA = sinphi;
193  dkdA = 0.;
194  ddzdldA = 0.;
195  dz0dA = 0.;
196 
197  dDdA = 0.5 * D_inv *
198  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
199  dvdA = 0.5 * (k * dDdA + D * dkdA);
200  dsdA = (2. / (k * sqrtvv)) * dvdA - (s / k) * dkdA;
201  ddzdA = 0.5 * dz_inv *
202  (2. * (dsdA)*dz2 * s_inv +
203  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
204  dxda(2, 1) = dz0dA + sign_dzdl * ddzdA;
205 
206  // k
207  dstartxdA = 0.;
208  dstartydA = 0.;
209  dkdA = 1.;
210  ddzdldA = 0.;
211  dz0dA = 0.;
212 
213  dDdA = 0.5 * D_inv *
214  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
215  dvdA = 0.5 * (k * dDdA + D * dkdA);
216  dsdA = (2. / (k * sqrtvv)) * dvdA - (s / k) * dkdA;
217  ddzdA = 0.5 * dz_inv *
218  (2. * (dsdA)*dz2 * s_inv +
219  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
220  dxda(2, 2) = dz0dA + sign_dzdl * ddzdA;
221 
222  // z0
223  dstartxdA = 0.;
224  dstartydA = 0.;
225  dkdA = 0.;
226  ddzdldA = 0.;
227  dz0dA = 1.;
228 
229  dDdA = 0.5 * D_inv *
230  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
231  dvdA = 0.5 * (k * dDdA + D * dkdA);
232  dsdA = (2. / (k * sqrtvv)) * dvdA - (s / k) * dkdA;
233  ddzdA = 0.5 * dz_inv *
234  (2. * (dsdA)*dz2 * s_inv +
235  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
236  dxda(2, 3) = dz0dA + sign_dzdl * ddzdA;
237 
238  // dzdl
239  dstartxdA = 0.;
240  dstartydA = 0.;
241  dkdA = 0.;
242  ddzdldA = 1.;
243  dz0dA = 0.;
244 
245  dDdA = 0.5 * D_inv *
246  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
247  dvdA = 0.5 * (k * dDdA + D * dkdA);
248  dsdA = (2. / (k * sqrtvv)) * dvdA - (s / k) * dkdA;
249  ddzdA = 0.5 * dz_inv *
250  (2. * (dsdA)*dz2 * s_inv +
251  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
252  dxda(2, 4) = dz0dA + sign_dzdl * ddzdA;
253  } else {
254  float s = 0.;
255  float temp1 = k * D * 0.5;
256  temp1 *= temp1;
257  float temp2 = D * 0.5;
258  s += 2. * temp2;
259  temp2 *= temp1;
260  s += temp2 / 3.;
261  temp2 *= temp1;
262  s += (3. / 20.) * temp2;
263  temp2 *= temp1;
264  s += (5. / 56.) * temp2;
265  float s_inv = 1. / s;
266  float dz = sqrt(s * s * dzdl * dzdl / (1. - dzdl * dzdl));
267  z = z0 + sign_dzdl * dz;
268  float dz_inv = 1. / dz;
269  float dz2 = dz * dz;
270 
271  float k2 = k * k;
272  float k3 = k2 * k;
273  float k4 = k3 * k;
274  float k5 = k4 * k;
275  float k6 = k5 * k;
276  float D2 = D * D;
277  float D3 = D2 * D;
278  float D4 = D3 * D;
279  float D5 = D4 * D;
280  float D6 = D5 * D;
281  float D7 = D6 * D;
282 
283  // phi
284  float dstartxdA = -d * sinphi;
285  float dstartydA = d * cosphi;
286  float dkdA = 0.;
287  float ddzdldA = 0.;
288  float dz0dA = 0.;
289 
290  float dDdA = 0.5 * D_inv * (2. * (startx - x) * dstartxdA +
291  2. * (starty - y) * dstartydA);
292  float dsdA = dDdA;
293  dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
294  dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
295  dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
296  float ddzdA = 0.5 * dz_inv *
297  (2. * (dsdA)*dz2 * s_inv +
298  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
299  dxda(2, 0) = dz0dA + sign_dzdl * ddzdA;
300 
301  // d
302  dstartxdA = cosphi;
303  dstartydA = sinphi;
304  dkdA = 0.;
305  ddzdldA = 0.;
306  dz0dA = 0.;
307 
308  dDdA = 0.5 * D_inv *
309  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
310  dsdA = dDdA;
311  dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
312  dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
313  dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
314  ddzdA = 0.5 * dz_inv *
315  (2. * (dsdA)*dz2 * s_inv +
316  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
317  dxda(2, 1) = dz0dA + sign_dzdl * ddzdA;
318 
319  // k
320  dstartxdA = 0.;
321  dstartydA = 0.;
322  dkdA = 1.;
323  ddzdldA = 0.;
324  dz0dA = 0.;
325 
326  dDdA = 0.5 * D_inv *
327  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
328  dsdA = dDdA;
329  dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
330  dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
331  dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
332  ddzdA = 0.5 * dz_inv *
333  (2. * (dsdA)*dz2 * s_inv +
334  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
335  dxda(2, 2) = dz0dA + sign_dzdl * ddzdA;
336 
337  // z0
338  dstartxdA = 0.;
339  dstartydA = 0.;
340  dkdA = 0.;
341  ddzdldA = 0.;
342  dz0dA = 1.;
343 
344  dDdA = 0.5 * D_inv *
345  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
346  dsdA = dDdA;
347  dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
348  dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
349  dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
350  ddzdA = 0.5 * dz_inv *
351  (2. * (dsdA)*dz2 * s_inv +
352  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
353  dxda(2, 3) = dz0dA + sign_dzdl * ddzdA;
354 
355  // dzdl
356  dstartxdA = 0.;
357  dstartydA = 0.;
358  dkdA = 0.;
359  ddzdldA = 1.;
360  dz0dA = 0.;
361 
362  dDdA = 0.5 * D_inv *
363  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
364  dsdA = dDdA;
365  dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
366  dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
367  dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
368  ddzdA = 0.5 * dz_inv *
369  (2. * (dsdA)*dz2 * s_inv +
370  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
371  dxda(2, 4) = dz0dA + sign_dzdl * ddzdA;
372  }
373 
374  // we want dx/dnu instead of dx/dk
375  // nu = sqrt(k)
376  // dx/dnu = dx/dk * dk/dnu
377  // k = nu^2 , dk/dnu = 2.*nu
378  for (int i = 0; i < 3; ++i) {
379  dxda(i, 2) *= 2. * state.nu;
380  }
381 }
382 
383 void CylinderKalman::subtractProjections(Matrix<float, 2, 1>& m,
384  Matrix<float, 2, 1>& ha,
385  Matrix<float, 2, 1>& diff) {
386  diff = m - ha;
387  if (diff(0, 0) > M_PI) {
388  diff(0, 0) -= (2. * M_PI);
389  }
390  if (diff(0, 0) < (-M_PI)) {
391  diff(0, 0) += (2. * M_PI);
392  }
393 }
394 
395 void CylinderKalman::calculateProjections(SimpleHit3D& hit,
396  HelixKalmanState& state,
397  Matrix<float, 2, 5>& H,
398  Matrix<float, 2, 1>& ha) {
399  det_rad[hit.get_layer()] =
400  sqrt(hit.get_x() * hit.get_x() + hit.get_y() * hit.get_y());
401 
402  // calculate dx/dA, A are the helix params, x is (x,y,z)
403  // first calculate for x-y plane
404  Matrix<float, 3, 5> dxda = Matrix<float, 3, 5>::Zero(3, 5);
405  float x = 0.;
406  float y = 0.;
407  float z = 0.;
408  calculate_dxda(hit, state, dxda, x, y, z);
409 
410  // calculate dm/dx , m is (phi, z)
411  Matrix<float, 2, 3> dmdx = Matrix<float, 2, 3>::Zero(2, 3);
412  // phi = atan2(y, x);
413  float r2_inv = 1. / (x * x + y * y);
414  dmdx(0, 0) = -y * r2_inv;
415  dmdx(0, 1) = x * r2_inv;
416  dmdx(1, 2) = 1.;
417 
418  H = dmdx * dxda;
419 
420  ha(0, 0) = atan2(y, x);
421  if (ha(0, 0) < 0.) {
422  ha(0, 0) += 2. * M_PI;
423  }
424  ha(1, 0) = z;
425 }
426 
427 void CylinderKalman::calculateMeasurements(SimpleHit3D& hit,
428  Matrix<float, 2, 1>& m,
429  Matrix<float, 2, 2>& G) {
430  Matrix<float, 2, 2> V = Matrix<float, 2, 2>::Zero(2, 2);
431 
432  V(0, 0) = 3.33333333333333426e-01 *
433  (
434  (2.0*sqrt(hit.get_size(0,0))) *
435  (2.0*sqrt(hit.get_size(0,0))) +
436  (2.0*sqrt(hit.get_size(1,1))) *
437  (2.0*sqrt(hit.get_size(1,1)))
438  ) /
439  ((hit.get_x()) * (hit.get_x()) + (hit.get_y()) * (hit.get_y()));
440 
441  V(1, 1) = 3.33333333333333426e-01 *
442  (2.0*sqrt(hit.get_size(2,2))) *
443  (2.0*sqrt(hit.get_size(2,2)));
444 
445  G = V.fullPivLu().inverse();
446 
447  m = Matrix<float, 2, 1>::Zero(2, 1);
448  m(0) = atan2(hit.get_y(), hit.get_x());
449  if (m(0) < 0.) {
450  m(0) += 2. * M_PI;
451  }
452  m(1) = hit.get_z();
453 }
454 
455 void CylinderKalman::updateIntersection(HelixKalmanState& state, int layer) {
456  float phi = state.phi;
457  float d = state.d;
458  float k = state.kappa;
459  float z0 = state.z0;
460  float dzdl = state.dzdl;
461 
462  float rad_det = det_rad[layer];
463 
464  float cosphi = cos(phi);
465  float sinphi = sin(phi);
466 
467  // float signk = (float)(sign(k));
468  k = fabs(k);
469 
470  float kd = (d * k + 1.);
471  float kcx = kd * cosphi;
472  float kcy = kd * sinphi;
473  float kd_inv = 1. / kd;
474  float R2 = rad_det * rad_det;
475  float a = 0.5 * (k * R2 + (d * d * k + 2. * d)) * kd_inv;
476  float tmp1 = a * kd_inv;
477  float P2x = kcx * tmp1;
478  float P2y = kcy * tmp1;
479 
480  float h = sqrt(R2 - a * a);
481 
482  float ux = -kcy * kd_inv;
483  float uy = kcx * kd_inv;
484 
485  float signk = signk_store;
486  state.x_int = P2x + signk * ux * h;
487  state.y_int = P2y + signk * uy * h;
488 
489  float sign_dzdl = sign(dzdl);
490  float startx = d * cosphi;
491  float starty = d * sinphi;
492  float D = sqrt((startx - state.x_int) * (startx - state.x_int) +
493  (starty - state.y_int) * (starty - state.y_int));
494  float v = 0.5 * k * D;
495  if (v > 0.1) {
496  if (v >= 0.999999) {
497  v = 0.999999;
498  }
499  float s = 2. * asin(v) / k;
500  float dz = sqrt(s * s * dzdl * dzdl / (1. - dzdl * dzdl));
501  state.z_int = z0 + sign_dzdl * dz;
502  } else {
503  float s = 0.;
504  float temp1 = k * D * 0.5;
505  temp1 *= temp1;
506  float temp2 = D * 0.5;
507  s += 2. * temp2;
508  temp2 *= temp1;
509  s += temp2 / 3.;
510  temp2 *= temp1;
511  s += (3. / 20.) * temp2;
512  temp2 *= temp1;
513  s += (5. / 56.) * temp2;
514  float dz = sqrt(s * s * dzdl * dzdl / (1. - dzdl * dzdl));
515  state.z_int = z0 + sign_dzdl * dz;
516  }
517 
518  state.position = (layer + 1);
519 }
520 
521 bool CylinderKalman::calculateScatteringVariance(HelixKalmanState& state,
522  float& var) {
523  if ((state.position == 0) || (state.position > nlayers)) {
524  var = 0.;
525  return false;
526  } else {
527  float k = state.kappa;
528  float p_inv = 3.33333333333333314e+02 * k * Bfield_inv *
529  sqrt(1. - state.dzdl * state.dzdl);
530  var = 2. * p_inv * p_inv * det_scatter_variance[state.position - 1];
531  return true;
532  }
533 }