ECCE @ EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
HelixKalman.cpp
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file HelixKalman.cpp
1 #include "HelixKalman.h"
2 #include "HelixKalmanState.h"
3 #include "SimpleHit3D.h"
4 
5 #include <Eigen/Core>
6 #include <Eigen/Dense>
7 #include <Eigen/LU>
8 
9 #include <cmath>
10 
11 using namespace std;
12 using namespace Eigen;
13 
14 static inline float sign(float x) {
15  return ((float)(x > 0.)) - ((float)(x < 0.));
16 }
17 
18 HelixKalman::HelixKalman(float B) : Bfield(B), Bfield_inv(1. / B) {}
19 
20 HelixKalman::~HelixKalman() {}
21 
22 void HelixKalman::subtractProjections(Matrix<float, 2, 1>& m,
23  Matrix<float, 2, 1>& ha,
24  Matrix<float, 2, 1>& diff) {
25  diff = m - ha;
26 }
27 
28 void HelixKalman::addHit(SimpleHit3D& hit, HelixKalmanState& state) {
29  Matrix<float, 2, 5> H = Matrix<float, 2, 5>::Zero(2, 5);
30  Matrix<float, 2, 1> ha = Matrix<float, 2, 1>::Zero(2, 1);
31  calculateProjections(hit, state, H, ha);
32 
33  Matrix<float, 2, 1> m = Matrix<float, 2, 1>::Zero(2, 1);
34  Matrix<float, 2, 2> G = Matrix<float, 2, 2>::Zero(2, 2);
35  calculateMeasurements(hit, m, G);
36 
37  Matrix<float, 5, 5> Q = Matrix<float, 5, 5>::Zero(5, 5);
38  calculateMSCovariance(state, Q);
39 
40  Matrix<float, 5, 2> Ht = H.transpose();
41 
42  Matrix<float, 5, 5> C_proj = state.C + Q;
43 
44  Matrix<float, 5, 5> C_proj_inv = C_proj.fullPivLu().inverse();
45 
46  Matrix<float, 5, 5> Cadd = Ht * G * H;
47 
48  Matrix<float, 5, 5> Cinv = (C_proj_inv + Cadd);
49 
50  state.C = Cinv.fullPivLu().inverse();
51 
52  Matrix<float, 5, 2> K = state.C * Ht * G;
53 
54  Matrix<float, 2, 1> proj_diff = Matrix<float, 2, 1>::Zero(2, 1);
55  subtractProjections(m, ha, proj_diff);
56 
57  Matrix<float, 5, 1> state_add = K * proj_diff;
58 
59  Matrix<float, 5, 1> old_state = Matrix<float, 5, 1>::Zero(5, 1);
60  old_state(0, 0) = state.phi;
61  old_state(1, 0) = state.d;
62  // old_state(2,0) = state.kappa;
63  old_state(2, 0) = state.nu;
64  old_state(3, 0) = state.z0;
65  old_state(4, 0) = state.dzdl;
66 
67  Matrix<float, 5, 1> new_state = old_state + state_add;
68 
69  state.phi = new_state(0, 0);
70  state.d = new_state(1, 0);
71  state.nu = new_state(2, 0);
72  state.kappa = state.nu * state.nu;
73  state.z0 = new_state(3, 0);
74  state.dzdl = new_state(4, 0);
75 
76  int layer = hit.get_layer();
77  updateIntersection(state, layer);
78  Matrix<float, 5, 1> state_diff = (new_state - old_state);
79  Matrix<float, 1, 5> state_diff_T = state_diff.transpose();
80 
81  Matrix<float, 1, 1> chi2_add = (state_diff_T * C_proj_inv * state_diff);
82  state.chi2 += chi2_add(0, 0);
83 }
84 
85 // dA/dA' , where A are the global helix parameters with pivot (0,0,0), and A'
86 // are the 3 helix parameters phi,k,dzdl with the pivot being the intersection
87 // at the current layer
88 void HelixKalman::calculate_dAdAp(HelixKalmanState& state,
89  Matrix<float, 5, 3>& dAdAp, float& phi_p,
90  float& cosphi_p, float& sinphi_p) {
91  float phi = state.phi;
92  float d = state.d;
93  float k = state.kappa;
94  // float z0 = state.z0;
95  float dzdl = state.dzdl;
96 
97  float cosphi = cos(phi);
98  float sinphi = sin(phi);
99 
100  // float signk = (float)(sign(k));
101  k = fabs(k);
102 
103  // calculate primed variables after translating (x0,y0,z0) -> (0,0,0)
104 
105  float x0 = state.x_int;
106  float y0 = state.y_int;
107 
108  phi_p = atan2((1. + k * d) * sinphi - k * y0, (1. + k * d) * cosphi - k * x0);
109  cosphi_p = cos(phi_p);
110  sinphi_p = sin(phi_p);
111 
112  // dA/dA' :
113 
114  // tx = cosphi' + k*x0;
115  // ty = sinphi' + k*y0;
116  // phi = atan2(ty, tx);
117  //
118  // dphi/dtx = -ty/(tx^2+ty^2)
119  // dphi/dty = tx/(tx^2+ty^2)
120 
121  // d = (1/k)*(sqrt( tx*tx + ty*ty ) - 1.)
122  // dd/dtx = (1/k)*( (tx*tx + ty*ty)^(-1/2) )*( tx )
123  // dd/dty = (1/k)*( (tx*tx + ty*ty)^(-1/2) )*( ty )
124 
125  // The A params are phi,d,k,z0,dzdl
126  // The A' params are phi',k',dzdl'
127 
128  // first the x-y plane
129 
130  float tx = cosphi_p + k * x0;
131  float ty = sinphi_p + k * y0;
132  float tx2ty2_inv = 1. / (tx * tx + ty * ty);
133  float dphidtx = -ty * tx2ty2_inv;
134  float dphidty = tx * tx2ty2_inv;
135  float dtxdA_p = -sinphi_p;
136  float dtydA_p = cosphi_p;
137  dAdAp(0, 0) = dphidtx * dtxdA_p + dphidty * dtydA_p;
138 
139  dtxdA_p = x0;
140  dtydA_p = y0;
141  dAdAp(0, 1) = dphidtx * dtxdA_p + dphidty * dtydA_p;
142 
143  if (k != 0.) {
144  float k_inv = 1. / k;
145  float tx2ty2sqrtinv = sqrt(tx2ty2_inv);
146  float dddtx = tx2ty2sqrtinv * tx * k_inv;
147  float dddty = tx2ty2sqrtinv * ty * k_inv;
148 
149  dtxdA_p = -sinphi_p;
150  dtydA_p = cosphi_p;
151  dAdAp(1, 0) = dddtx * dtxdA_p + dddty * dtydA_p;
152 
153  dtxdA_p = x0;
154  dtydA_p = y0;
155  dAdAp(1, 1) = dphidtx * dtxdA_p + dphidty * dtydA_p -
156  (sqrt(tx * tx + ty * ty) - 1.) * k_inv * k_inv;
157  } else {
158  // d = x0*cosphi_p + y0*sinphi_p
159  dAdAp(1, 0) = y0 * cosphi_p - x0 * sinphi_p;
160  dAdAp(1, 1) = (dAdAp(1, 0)) * (dAdAp(1, 0)) * 0.5;
161  }
162 
163  dAdAp(2, 1) = 1.;
164 
165  // now the z direction
166 
167  float sign_dzdl = sign(dzdl);
168 
169  float dx = d * cosphi;
170  float dy = d * sinphi;
171  float D = sqrt((x0 - dx) * (x0 - dx) + (y0 - dy) * (y0 - dy));
172  float D_inv = 1. / D;
173  float v = 0.5 * k * D;
174  if (v > 0.1) {
175  float k_inv = 1. / k;
176  float s = 2. * asin(v) * k_inv;
177  float s_inv = 1. / s;
178  float dsdv = 2. / (k * sqrt(1 - v * v));
179  float dsdk = -s * k_inv;
180  float tmp1 = s * s * dzdl * dzdl;
181  float tmp2 = dzdl * tmp1;
182  float tmp3 = 1. / tmp2;
183  float dz = sqrt(tmp1 / (1. - dzdl * dzdl));
184  float dz3 = dz * dz * dz;
185 
186  // phi'
187  float ddxdA = -d * sinphi * dAdAp(0, 0) + cosphi * dAdAp(1, 0);
188  float ddydA = d * cosphi * dAdAp(0, 0) + sinphi * dAdAp(1, 0);
189  float dkdA = 0.;
190  float ddzdldA = 0.;
191 
192  float dDdA =
193  0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
194  float dvdA = 0.5 * (k * dDdA + D * dkdA);
195  float dsdA = dsdv * dvdA + dsdk * dkdA;
196  float ddzdA = (dz * s_inv) * dsdA + (dz3 * tmp3) * ddzdldA;
197  dAdAp(3, 0) = -sign_dzdl * ddzdA;
198 
199  // k'
200  ddxdA = 0.;
201  ddydA = 0.;
202  dkdA = 1.;
203  ddzdldA = 0.;
204 
205  dDdA = 0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
206  dvdA = 0.5 * (k * dDdA + D * dkdA);
207  dsdA = dsdv * dvdA + dsdk * dkdA;
208  ddzdA = (dz * s_inv) * dsdA + (dz3 * tmp3) * ddzdldA;
209  dAdAp(3, 1) = -sign_dzdl * ddzdA;
210 
211  // dzdl'
212  ddxdA = 0.;
213  ddydA = 0.;
214  dkdA = 0.;
215  ddzdldA = 1.;
216 
217  dDdA = 0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
218  dvdA = 0.5 * (k * dDdA + D * dkdA);
219  dsdA = dsdv * dvdA + dsdk * dkdA;
220  ddzdA = (dz * s_inv) * dsdA + (dz3 * tmp3) * ddzdldA;
221  dAdAp(3, 2) = -sign_dzdl * ddzdA;
222  } else {
223  float s = 0.;
224  float temp1 = k * D * 0.5;
225  temp1 *= temp1;
226  float temp2 = D * 0.5;
227  s += 2. * temp2;
228  temp2 *= temp1;
229  s += temp2 / 3.;
230  temp2 *= temp1;
231  s += (3. / 20.) * temp2;
232  temp2 *= temp1;
233  s += (5. / 56.) * temp2;
234  float s_inv = 1. / s;
235  float onedzdl2_inv = 1. / (1. - dzdl * dzdl);
236  float dz = sqrt(s * s * dzdl * dzdl * onedzdl2_inv);
237  float dz_inv = 1. / dz;
238  float dz2 = dz * dz;
239 
240  float k2 = k * k;
241  float k3 = k2 * k;
242  float k4 = k3 * k;
243  float k5 = k4 * k;
244  float k6 = k5 * k;
245  float D2 = D * D;
246  float D3 = D2 * D;
247  float D4 = D3 * D;
248  float D5 = D4 * D;
249  float D6 = D5 * D;
250  float D7 = D6 * D;
251 
252  // phi'
253  float ddxdA = -d * sinphi * dAdAp(0, 0) + cosphi * dAdAp(1, 0);
254  float ddydA = d * cosphi * dAdAp(0, 0) + sinphi * dAdAp(1, 0);
255  float dkdA = 0.;
256  float ddzdldA = 0.;
257 
258  float dDdA =
259  0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
260  float dsdA = dDdA;
261  dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
262  dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
263  dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
264  float ddzdA = 0.5 * dz_inv *
265  (2. * (dsdA)*dz2 * s_inv +
266  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
267  dAdAp(3, 0) = -sign_dzdl * ddzdA;
268 
269  // k'
270  ddxdA = 0.;
271  ddydA = 0.;
272  dkdA = 1.;
273  ddzdldA = 0.;
274 
275  dDdA = 0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
276  dsdA = dDdA;
277  dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
278  dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
279  dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
280  ddzdA = 0.5 * dz_inv *
281  (2. * (dsdA)*dz2 * s_inv +
282  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
283  dAdAp(3, 1) = -sign_dzdl * ddzdA;
284 
285  // dzdl'
286  ddxdA = 0.;
287  ddydA = 0.;
288  dkdA = 0.;
289  ddzdldA = 1.;
290 
291  dDdA = 0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
292  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  ddzdA = 0.5 * dz_inv *
297  (2. * (dsdA)*dz2 * s_inv +
298  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
299  dAdAp(3, 2) = -sign_dzdl * ddzdA;
300  }
301 
302  dAdAp(4, 2) = 1.;
303 
304  // we want to substitute nu for k
305  // dnu/dnu' = 1 , just as for kappa
306  // for the rest of the variables, dx/dnu = dx/dk * dk/dnu
307  dAdAp(0, 1) *= 2. * state.nu;
308  dAdAp(1, 1) *= 2. * state.nu;
309  dAdAp(3, 1) *= 2. * state.nu;
310  dAdAp(4, 1) *= 2. * state.nu;
311 }
312 
313 // dA'/dp , where p is the momentum vector
314 void HelixKalman::calculate_dApdp(HelixKalmanState& state,
315  Matrix<float, 3, 3>& dApdp, Vector3f& p,
316  float /*phi*/, float cosphi, float sinphi) {
317  float k = state.kappa;
318  float dzdl = state.dzdl;
319 
320  // kappa = 0.003*B/sqrt(px*px + py*py)
321  // phi = atan2(-px, py)
322  // dzdl' = pz/sqrt(px^2 + py^2 + pz^2)
323 
324  float pT = 0.003 * Bfield / k;
325  float pT_inv = 1. / pT;
326  float pT_inv2 = pT_inv * pT_inv;
327 
328  float px = -sinphi * pT;
329  float py = cosphi * pT;
330  float pz = dzdl * pT / sqrt(1. - dzdl * dzdl);
331 
332  // dphi/dpx
333  dApdp(0, 0) = -py * pT_inv2;
334  // dphi/dpy
335  dApdp(0, 1) = px * pT_inv2;
336 
337  // dk/dpx = -0.003*B*(px*px+py*py)^(-3/2)*px
338  float pTneg3 = (pT_inv * pT_inv2);
339  dApdp(1, 0) = -0.003 * Bfield * px * pTneg3;
340  // dk/dpy
341  dApdp(1, 1) = -0.003 * Bfield * py * pTneg3;
342 
343  float pmag = sqrt(pT * pT + pz * pz);
344  float pmag_inv = 1. / pmag;
345  float pmag_inv_3 = pmag_inv * pmag_inv * pmag_inv;
346  // ddzdl/dpx = -pz*(px^2 + py^2 + pz^2)^(-3/2)*px
347  dApdp(2, 0) = -pz * pmag_inv_3 * px;
348  dApdp(2, 1) = -pz * pmag_inv_3 * py;
349  dApdp(2, 2) = -pz * pmag_inv_3 * pz + pmag_inv;
350 
351  p(0) = px;
352  p(1) = py;
353  p(2) = pz;
354 
355  // dnu/dx = dk/dx * dnu/dk
356  // TODO put this directly into the calculation above
357  dApdp(1, 0) *= 0.5 / state.nu;
358  dApdp(1, 1) *= 0.5 / state.nu;
359 }
360 
361 // dp/db : b is defined by p = R*s*b , with R being a rotation matrix rotating
362 // the unit vector in the z-direction to the direction of p, and S is a scalar
363 // with a value of the magnitude of the current value of p
364 void HelixKalman::calculate_dpdb(Vector3f& p, Matrix<float, 3, 3>& dpdb) {
365  Vector3f b = Vector3f::Zero(3);
366  b(2) = 1.;
367 
368  float p_mag = sqrt(p.dot(p));
369 
370  Vector3f p_unit = p / p_mag;
371 
372  Vector3f axis = b.cross(p_unit);
373  float angle = acos(p_unit.dot(b));
374 
375  axis /= sqrt(axis.dot(axis));
376 
377  Matrix<float, 3, 3> rot_p = Matrix<float, 3, 3>::Zero(3, 3);
378  float cos_p = cos(angle);
379  float sin_p = sin(angle);
380  rot_p(0, 0) = cos_p + axis(0) * axis(0) * (1. - cos_p);
381  rot_p(0, 1) = axis(0) * axis(1) * (1. - cos_p) - axis(2) * sin_p;
382  rot_p(0, 2) = axis(0) * axis(2) * (1. - cos_p) + axis(1) * sin_p;
383  rot_p(1, 0) = axis(1) * axis(0) * (1. - cos_p) + axis(2) * sin_p;
384  rot_p(1, 1) = cos_p + axis(1) * axis(1) * (1. - cos_p);
385  rot_p(1, 2) = axis(1) * axis(2) * (1. - cos_p) - axis(0) * sin_p;
386  rot_p(2, 0) = axis(2) * axis(0) * (1. - cos_p) - axis(1) * sin_p;
387  rot_p(2, 1) = axis(2) * axis(1) * (1. - cos_p) + axis(0) * sin_p;
388  rot_p(2, 2) = cos_p + axis(2) * axis(2) * (1. - cos_p);
389 
390  dpdb = rot_p * p_mag;
391 }
392 
393 // db/dt , with b a vector, and t a being the two rotation angles, one around
394 // the x axis and one around the y axis. The derivative db/dt is calculated at
395 // the value of b being the unit vector in the z direction
396 void HelixKalman::calculate_dbdt(Matrix<float, 3, 2>& dbdt_out) {
397  // bz = (1 + [tan(t1)]^2 + [tan(t2)]^2)^(-1/2)
398  // bx = bz*tan(t1)
399  // by = bz*tan(t2)
400 
401  // dbz/dt1 = -[(1 + [tan(t1)]^2 + [tan(t2)]^2)^(-3/2)]*( tan(t1)*( 1 +
402  // [tan(t1)]^2 ) )
403 
404  // dbx/dt1 = tan(t1)*(dbz/dt1) + bz*( 1 + [tan(t1)]^2 )
405  // dbx/dt2 = tan(t1)*(dbz/dt2)
406 
407  // dby/dt1 = tan(t2)*(dbz/dt1)
408  // dby/dt2 = tan(t2)*(dbz/dt2) + bz*( 1 + [tan(t2)]^2 )
409 
410  // t1 = t2 = 0
411 
412  // float tant1 = tan(t1);
413  // float tant1_2 = tant1*tant1;
414  // float tant2 = tan(t2);
415  // float tant2_2 = tant2*tant2;
416  // float bz = 1./sqrt(1. + tant1_2 + tant2_2);
417  // dbdt_out(2,0) = -bz*bz*bz*tant1*(1. + tant1_2);
418  // dbdt_out(2,1) = -bz*bz*bz*tant2*(1. + tant2_2);
419  //
420  // dbdt_out(0,0) = tant1*dbdt_out(2,0) + bz*(1. + tant1_2);
421  // dbdt_out(0,1) = tant1*dbdt_out(2,1);
422  //
423  // dbdt_out(1,0) = tant2*dbdt_out(2,0);
424  // dbdt_out(1,1) = tant2*dbdt_out(2,1) + bz*(1. + tant2_2);
425 
426  dbdt_out(2, 0) = 0.;
427  dbdt_out(2, 1) = 0.;
428 
429  dbdt_out(0, 0) = 1.;
430  dbdt_out(0, 1) = 0.;
431 
432  dbdt_out(1, 0) = 0.;
433  dbdt_out(1, 1) = 1.;
434 }
435 
436 void HelixKalman::calculateMSCovariance(HelixKalmanState& state,
437  Matrix<float, 5, 5>& Q) {
438  Matrix<float, 5, 3> dAdAp = Matrix<float, 5, 3>::Zero(5, 3);
439  float phi_p = 0.;
440  float cosphi_p = 0.;
441  float sinphi_p = 0.;
442 
443  float var = 0.;
444  if (calculateScatteringVariance(state, var) == false) {
445  return;
446  }
447 
448  calculate_dAdAp(state, dAdAp, phi_p, cosphi_p, sinphi_p);
449 
450  Matrix<float, 3, 3> dApdp = Matrix<float, 3, 3>::Zero(3, 3);
451  Vector3f p = Vector3f::Zero(3);
452  calculate_dApdp(state, dApdp, p, phi_p, cosphi_p, sinphi_p);
453 
454  Matrix<float, 3, 3> dpdb = Matrix<float, 3, 3>::Zero(3, 3);
455  calculate_dpdb(p, dpdb);
456 
457  Matrix<float, 3, 2> dbdt = Matrix<float, 3, 2>::Zero(3, 2);
458  calculate_dbdt(dbdt);
459 
460  Matrix<float, 5, 2> dAdt = dAdAp * dApdp * dpdb * dbdt;
461 
462  for (unsigned int j = 0; j < 5; ++j) {
463  for (unsigned int i = 0; i < 5; ++i) {
464  Q(i, j) = (dAdt(i, 0) * dAdt(j, 0) + dAdt(i, 1) * dAdt(j, 1)) * var;
465  }
466  }
467 }