ECCE @ EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
HelixKalmanFilter.cc
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file HelixKalmanFilter.cc
1 #include "HelixKalmanFilter.h"
2 
3 #include <HelixHough/SimpleHit3D.h>
4 #include <HelixHough/HelixKalmanState.h>
5 
6 #include <Eigen/Dense>
7 #include <Eigen/LU>
8 
9 #include <algorithm> // for copy
10 #include <cmath> // for sqrt, M_PI, atan2
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 HelixKalmanFilter::HelixKalmanFilter(std::vector<float>& detector_radii,
20  std::vector<float>& detector_material, float B) :
21  det_radii(detector_radii),
22  det_scatter_variance(detector_material),
23  nlayers(detector_radii.size()),
24  signk_store(0.),
25  Bfield(B),
26  Bfield_inv(1. / B)
27 {
28  for (unsigned int i = 0; i < det_scatter_variance.size(); ++i) {
29  det_scatter_variance[i] = det_scatter_variance[i] * 0.0136 * 0.0136;
30  }
31 }
32 
33 
35 
36  // H : observation(projector) matrix (with 2 observables in this case, phi and z)
37  Matrix<float, 2, 5> H = Matrix<float, 2, 5>::Zero(2, 5);
38  // ha : projected measurements
39  Matrix<float, 2, 1> ha = Matrix<float, 2, 1>::Zero(2, 1);
40  calculateProjections(hit, state, H, ha);
41 
42  // m : measurement
43  Matrix<float, 2, 1> m = Matrix<float, 2, 1>::Zero(2, 1);
44  // G : V^-1, where V : error coavariance for measurement noise
45  Matrix<float, 2, 2> G = Matrix<float, 2, 2>::Zero(2, 2);
46  calculateMeasurements(hit, m, G);
47 
48  // Q : error covariance for Multiple-Scattering process noise
49  Matrix<float, 5, 5> Q = Matrix<float, 5, 5>::Zero(5, 5);
50  calculateMSCovariance(state, Q);
51 
52  Matrix<float, 5, 2> Ht = H.transpose();
53 
54  Matrix<float, 5, 5> C_proj = state.C + Q;
55 
56  Matrix<float, 5, 5> C_proj_inv = C_proj.fullPivLu().inverse();
57 
58  Matrix<float, 5, 5> Cadd = Ht * G * H;
59 
60  Matrix<float, 5, 5> Cinv = (C_proj_inv + Cadd);
61 
62  state.C = Cinv.fullPivLu().inverse();
63 
64  // K : Kalman gain
65  Matrix<float, 5, 2> K = state.C * Ht * G;
66 
67  Matrix<float, 2, 1> proj_diff = Matrix<float, 2, 1>::Zero(2, 1);
68  subtractProjections(m, ha, proj_diff);
69 
70  Matrix<float, 5, 1> state_add = K * proj_diff;
71 
72  Matrix<float, 5, 1> old_state = Matrix<float, 5, 1>::Zero(5, 1);
73  old_state(0, 0) = state.phi;
74  old_state(1, 0) = state.d;
75  // old_state(2,0) = state.kappa;
76  old_state(2, 0) = state.nu;
77  old_state(3, 0) = state.z0;
78  old_state(4, 0) = state.dzdl;
79 
80  Matrix<float, 5, 1> new_state = old_state + state_add;
81 
82  state.phi = new_state(0, 0);
83  state.d = new_state(1, 0);
84  state.nu = new_state(2, 0);
85  state.kappa = state.nu * state.nu;
86  state.z0 = new_state(3, 0);
87  state.dzdl = new_state(4, 0);
88 
89  int layer = hit.get_layer();
90  updateIntersection(state, layer);
91  Matrix<float, 5, 1> state_diff = (new_state - old_state);
92  Matrix<float, 1, 5> state_diff_T = state_diff.transpose();
93 
94  Matrix<float, 1, 1> chi2_add = (state_diff_T * C_proj_inv * state_diff);
95  state.chi2 += chi2_add(0, 0);
96 }
97 
99  HelixKalmanState& state,
100  Matrix<float, 2, 5>& H,
101  Matrix<float, 2, 1>& ha) {
102  det_radii[hit.get_layer()] =
103  sqrt(hit.get_x() * hit.get_x() + hit.get_y() * hit.get_y());
104 
105  // calculate dx/da, a are the helix params, x is (x,y,z)
106  // first calculate for x-y plane
107  Matrix<float, 3, 5> dxda = Matrix<float, 3, 5>::Zero(3, 5);
108  float x = 0.;
109  float y = 0.;
110  float z = 0.;
111  calculate_dxda(hit, state, dxda, x, y, z);
112 
113  // calculate dm/dx , m is (phi, z)
114  Matrix<float, 2, 3> dmdx = Matrix<float, 2, 3>::Zero(2, 3);
115  // phi = atan2(y, x);
116  float r2_inv = 1. / (x * x + y * y);
117  dmdx(0, 0) = -y * r2_inv;
118  dmdx(0, 1) = x * r2_inv;
119  dmdx(1, 2) = 1.;
120 
121  H = dmdx * dxda;
122 
123  ha(0, 0) = atan2(y, x);
124  if (ha(0, 0) < 0.) {
125  ha(0, 0) += 2. * M_PI;
126  }
127  ha(1, 0) = z;
128 }
129 
131  Matrix<float, 2, 1>& m,
132  Matrix<float, 2, 2>& G) {
133  Matrix<float, 2, 2> V = Matrix<float, 2, 2>::Zero(2, 2);
134 
135  // <(r*dphi)(r*dphi)>/(r*r)
136  V(0, 0) = 3.33333333333333426e-01 *
137  (
138  (2.0*sqrt(hit.get_size(0,0))) *
139  (2.0*sqrt(hit.get_size(0,0))) +
140  (2.0*sqrt(hit.get_size(1,1))) *
141  (2.0*sqrt(hit.get_size(1,1)))
142  ) /
143  ((hit.get_x()) * (hit.get_x()) + (hit.get_y()) * (hit.get_y()));
144 
145  V(1, 1) = 3.33333333333333426e-01 *
146  (2.0*sqrt(hit.get_size(2,2))) *
147  (2.0*sqrt(hit.get_size(2,2)));
148 
149  G = V.fullPivLu().inverse();
150 
151  m = Matrix<float, 2, 1>::Zero(2, 1);
152  m(0) = atan2(hit.get_y(), hit.get_x());
153  if (m(0) < 0.) {
154  m(0) += 2. * M_PI;
155  }
156  m(1) = hit.get_z();
157 }
158 
160  Matrix<float, 5, 5>& Q) {
161  Matrix<float, 5, 3> dAdAp = Matrix<float, 5, 3>::Zero(5, 3);
162  float phi_p = 0.;
163  float cosphi_p = 0.;
164  float sinphi_p = 0.;
165 
166  float var = 0.;
167  if (calculateScatteringVariance(state, var) == false) {
168  return;
169  }
170 
171  calculate_dAdAp(state, dAdAp, phi_p, cosphi_p, sinphi_p);
172 
173  Matrix<float, 3, 3> dApdp = Matrix<float, 3, 3>::Zero(3, 3);
174  Vector3f p = Vector3f::Zero(3);
175  calculate_dApdp(state, dApdp, p, phi_p, cosphi_p, sinphi_p);
176 
177  Matrix<float, 3, 3> dpdb = Matrix<float, 3, 3>::Zero(3, 3);
178  calculate_dpdb(p, dpdb);
179 
180  Matrix<float, 3, 2> dbdt = Matrix<float, 3, 2>::Zero(3, 2);
181  calculate_dbdt(dbdt);
182 
183  Matrix<float, 5, 2> dAdt = dAdAp * dApdp * dpdb * dbdt;
184 
185  for (unsigned int j = 0; j < 5; ++j) {
186  for (unsigned int i = 0; i < 5; ++i) {
187  Q(i, j) = (dAdt(i, 0) * dAdt(j, 0) + dAdt(i, 1) * dAdt(j, 1)) * var;
188  }
189  }
190 }
191 
192 void HelixKalmanFilter::subtractProjections(Matrix<float, 2, 1>& m,
193  Matrix<float, 2, 1>& ha,
194  Matrix<float, 2, 1>& diff) {
195  diff = m - ha;
196  if (diff(0, 0) > M_PI) {
197  diff(0, 0) -= (2. * M_PI);
198  }
199  if (diff(0, 0) < (-M_PI)) {
200  diff(0, 0) += (2. * M_PI);
201  }
202 
203 }
204 
206  float phi = state.phi;
207  float d = state.d;
208  float k = state.kappa;
209  float z0 = state.z0;
210  float dzdl = state.dzdl;
211 
212  float rad_det = det_radii[layer];
213 
214  float cosphi = cos(phi);
215  float sinphi = sin(phi);
216 
217  // float signk = (float)(sign(k));
218  k = fabs(k);
219 
220  float kd = (d * k + 1.);
221  float kcx = kd * cosphi;
222  float kcy = kd * sinphi;
223  float kd_inv = 1. / kd;
224  float R2 = rad_det * rad_det;
225  float a = 0.5 * (k * R2 + (d * d * k + 2. * d)) * kd_inv;
226  float tmp1 = a * kd_inv;
227  float P2x = kcx * tmp1;
228  float P2y = kcy * tmp1;
229 
230  float h = sqrt(R2 - a * a);
231 
232  float ux = -kcy * kd_inv;
233  float uy = kcx * kd_inv;
234 
235  float signk = signk_store;
236  state.x_int = P2x + signk * ux * h;
237  state.y_int = P2y + signk * uy * h;
238 
239  float sign_dzdl = sign(dzdl);
240  float startx = d * cosphi;
241  float starty = d * sinphi;
242  float D = sqrt((startx - state.x_int) * (startx - state.x_int) +
243  (starty - state.y_int) * (starty - state.y_int));
244  float v = 0.5 * k * D;
245  if (v > 0.1) {
246  if (v >= 0.999999) {
247  v = 0.999999;
248  }
249  float s = 2. * asin(v) / k;
250  float dz = sqrt(s * s * dzdl * dzdl / (1. - dzdl * dzdl));
251  state.z_int = z0 + sign_dzdl * dz;
252  } else {
253  float s = 0.;
254  float temp1 = k * D * 0.5;
255  temp1 *= temp1;
256  float temp2 = D * 0.5;
257  s += 2. * temp2;
258  temp2 *= temp1;
259  s += temp2 / 3.;
260  temp2 *= temp1;
261  s += (3. / 20.) * temp2;
262  temp2 *= temp1;
263  s += (5. / 56.) * temp2;
264  float dz = sqrt(s * s * dzdl * dzdl / (1. - dzdl * dzdl));
265  state.z_int = z0 + sign_dzdl * dz;
266  }
267 
268  state.position = (layer + 1);
269 }
270 
272  float& var) {
273  if ((state.position == 0) || (state.position > nlayers)) {
274  var = 0.;
275  return false;
276  } else {
277  float k = state.kappa;
278  float p_inv = 3.33333333333333314e+02 * k * Bfield_inv *
279  sqrt(1. - state.dzdl * state.dzdl);
280  var = 2. * p_inv * p_inv * det_scatter_variance[state.position - 1];
281  return true;
282  }
283 }
284 
285 // dA/dA' , where A are the global helix parameters with pivot (0,0,0), and A'
286 // are the 3 helix parameters phi,k,dzdl with the pivot being the intersection
287 // at the current layer
289  Matrix<float, 5, 3>& dAdAp, float& phi_p,
290  float& cosphi_p, float& sinphi_p) {
291  float phi = state.phi;
292  float d = state.d;
293  float k = state.kappa;
294  // float z0 = state.z0;
295  float dzdl = state.dzdl;
296 
297  float cosphi = cos(phi);
298  float sinphi = sin(phi);
299 
300  // float signk = (float)(sign(k));
301  k = fabs(k);
302 
303  // calculate primed variables after translating (x0,y0,z0) -> (0,0,0)
304 
305  float x0 = state.x_int;
306  float y0 = state.y_int;
307 
308  phi_p = atan2((1. + k * d) * sinphi - k * y0, (1. + k * d) * cosphi - k * x0);
309  cosphi_p = cos(phi_p);
310  sinphi_p = sin(phi_p);
311 
312  // dA/dA' :
313 
314  // tx = cosphi' + k*x0;
315  // ty = sinphi' + k*y0;
316  // phi = atan2(ty, tx);
317  //
318  // dphi/dtx = -ty/(tx^2+ty^2)
319  // dphi/dty = tx/(tx^2+ty^2)
320 
321  // d = (1/k)*(sqrt( tx*tx + ty*ty ) - 1.)
322  // dd/dtx = (1/k)*( (tx*tx + ty*ty)^(-1/2) )*( tx )
323  // dd/dty = (1/k)*( (tx*tx + ty*ty)^(-1/2) )*( ty )
324 
325  // The A params are phi,d,k,z0,dzdl
326  // The A' params are phi',k',dzdl'
327 
328  // first the x-y plane
329 
330  float tx = cosphi_p + k * x0;
331  float ty = sinphi_p + k * y0;
332  float tx2ty2_inv = 1. / (tx * tx + ty * ty);
333  float dphidtx = -ty * tx2ty2_inv;
334  float dphidty = tx * tx2ty2_inv;
335  float dtxdA_p = -sinphi_p;
336  float dtydA_p = cosphi_p;
337  dAdAp(0, 0) = dphidtx * dtxdA_p + dphidty * dtydA_p;
338 
339  dtxdA_p = x0;
340  dtydA_p = y0;
341  dAdAp(0, 1) = dphidtx * dtxdA_p + dphidty * dtydA_p;
342 
343  if (k != 0.) {
344  float k_inv = 1. / k;
345  float tx2ty2sqrtinv = sqrt(tx2ty2_inv);
346  float dddtx = tx2ty2sqrtinv * tx * k_inv;
347  float dddty = tx2ty2sqrtinv * ty * k_inv;
348 
349  dtxdA_p = -sinphi_p;
350  dtydA_p = cosphi_p;
351  dAdAp(1, 0) = dddtx * dtxdA_p + dddty * dtydA_p;
352 
353  dtxdA_p = x0;
354  dtydA_p = y0;
355  dAdAp(1, 1) = dphidtx * dtxdA_p + dphidty * dtydA_p -
356  (sqrt(tx * tx + ty * ty) - 1.) * k_inv * k_inv;
357  } else {
358  // d = x0*cosphi_p + y0*sinphi_p
359  dAdAp(1, 0) = y0 * cosphi_p - x0 * sinphi_p;
360  dAdAp(1, 1) = (dAdAp(1, 0)) * (dAdAp(1, 0)) * 0.5;
361  }
362 
363  dAdAp(2, 1) = 1.;
364 
365  // now the z direction
366 
367  float sign_dzdl = sign(dzdl);
368 
369  float dx = d * cosphi;
370  float dy = d * sinphi;
371  float D = sqrt((x0 - dx) * (x0 - dx) + (y0 - dy) * (y0 - dy));
372  float D_inv = 1. / D;
373  float v = 0.5 * k * D;
374  if (v > 0.1) {
375  float k_inv = 1. / k;
376  float s = 2. * asin(v) * k_inv;
377  float s_inv = 1. / s;
378  float dsdv = 2. / (k * sqrt(1 - v * v));
379  float dsdk = -s * k_inv;
380  float tmp1 = s * s * dzdl * dzdl;
381  float tmp2 = dzdl * tmp1;
382  float tmp3 = 1. / tmp2;
383  float dz = sqrt(tmp1 / (1. - dzdl * dzdl));
384  float dz3 = dz * dz * dz;
385 
386  // phi'
387  float ddxdA = -d * sinphi * dAdAp(0, 0) + cosphi * dAdAp(1, 0);
388  float ddydA = d * cosphi * dAdAp(0, 0) + sinphi * dAdAp(1, 0);
389  float dkdA = 0.;
390  float ddzdldA = 0.;
391 
392  float dDdA =
393  0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
394  float dvdA = 0.5 * (k * dDdA + D * dkdA);
395  float dsdA = dsdv * dvdA + dsdk * dkdA;
396  float ddzdA = (dz * s_inv) * dsdA + (dz3 * tmp3) * ddzdldA;
397  dAdAp(3, 0) = -sign_dzdl * ddzdA;
398 
399  // k'
400  ddxdA = 0.;
401  ddydA = 0.;
402  dkdA = 1.;
403  ddzdldA = 0.;
404 
405  dDdA = 0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
406  dvdA = 0.5 * (k * dDdA + D * dkdA);
407  dsdA = dsdv * dvdA + dsdk * dkdA;
408  ddzdA = (dz * s_inv) * dsdA + (dz3 * tmp3) * ddzdldA;
409  dAdAp(3, 1) = -sign_dzdl * ddzdA;
410 
411  // dzdl'
412  ddxdA = 0.;
413  ddydA = 0.;
414  dkdA = 0.;
415  ddzdldA = 1.;
416 
417  dDdA = 0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
418  dvdA = 0.5 * (k * dDdA + D * dkdA);
419  dsdA = dsdv * dvdA + dsdk * dkdA;
420  ddzdA = (dz * s_inv) * dsdA + (dz3 * tmp3) * ddzdldA;
421  dAdAp(3, 2) = -sign_dzdl * ddzdA;
422  } else {
423  float s = 0.;
424  float temp1 = k * D * 0.5;
425  temp1 *= temp1;
426  float temp2 = D * 0.5;
427  s += 2. * temp2;
428  temp2 *= temp1;
429  s += temp2 / 3.;
430  temp2 *= temp1;
431  s += (3. / 20.) * temp2;
432  temp2 *= temp1;
433  s += (5. / 56.) * temp2;
434  float s_inv = 1. / s;
435  float onedzdl2_inv = 1. / (1. - dzdl * dzdl);
436  float dz = sqrt(s * s * dzdl * dzdl * onedzdl2_inv);
437  float dz_inv = 1. / dz;
438  float dz2 = dz * dz;
439 
440  float k2 = k * k;
441  float k3 = k2 * k;
442  float k4 = k3 * k;
443  float k5 = k4 * k;
444  float k6 = k5 * k;
445  float D2 = D * D;
446  float D3 = D2 * D;
447  float D4 = D3 * D;
448  float D5 = D4 * D;
449  float D6 = D5 * D;
450  float D7 = D6 * D;
451 
452  // phi'
453  float ddxdA = -d * sinphi * dAdAp(0, 0) + cosphi * dAdAp(1, 0);
454  float ddydA = d * cosphi * dAdAp(0, 0) + sinphi * dAdAp(1, 0);
455  float dkdA = 0.;
456  float ddzdldA = 0.;
457 
458  float dDdA =
459  0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
460  float dsdA = dDdA;
461  dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
462  dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
463  dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
464  float ddzdA = 0.5 * dz_inv *
465  (2. * (dsdA)*dz2 * s_inv +
466  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
467  dAdAp(3, 0) = -sign_dzdl * ddzdA;
468 
469  // k'
470  ddxdA = 0.;
471  ddydA = 0.;
472  dkdA = 1.;
473  ddzdldA = 0.;
474 
475  dDdA = 0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
476  dsdA = dDdA;
477  dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
478  dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
479  dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
480  ddzdA = 0.5 * dz_inv *
481  (2. * (dsdA)*dz2 * s_inv +
482  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
483  dAdAp(3, 1) = -sign_dzdl * ddzdA;
484 
485  // dzdl'
486  ddxdA = 0.;
487  ddydA = 0.;
488  dkdA = 0.;
489  ddzdldA = 1.;
490 
491  dDdA = 0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
492  dsdA = dDdA;
493  dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
494  dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
495  dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
496  ddzdA = 0.5 * dz_inv *
497  (2. * (dsdA)*dz2 * s_inv +
498  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
499  dAdAp(3, 2) = -sign_dzdl * ddzdA;
500  }
501 
502  dAdAp(4, 2) = 1.;
503 
504  // we want to substitute nu for k
505  // dnu/dnu' = 1 , just as for kappa
506  // for the rest of the variables, dx/dnu = dx/dk * dk/dnu
507  dAdAp(0, 1) *= 2. * state.nu;
508  dAdAp(1, 1) *= 2. * state.nu;
509  dAdAp(3, 1) *= 2. * state.nu;
510  dAdAp(4, 1) *= 2. * state.nu;
511 }
512 
513 // dA'/dp , where p is the momentum vector
515  Matrix<float, 3, 3>& dApdp, Vector3f& p,
516  float /*phi*/, float cosphi, float sinphi) {
517  float k = state.kappa;
518  float dzdl = state.dzdl;
519 
520  // kappa = 0.003*B/sqrt(px*px + py*py)
521  // phi = atan2(-px, py)
522  // dzdl' = pz/sqrt(px^2 + py^2 + pz^2)
523 
524  float pT = 0.003 * Bfield / k;
525  float pT_inv = 1. / pT;
526  float pT_inv2 = pT_inv * pT_inv;
527 
528  float px = -sinphi * pT;
529  float py = cosphi * pT;
530  float pz = dzdl * pT / sqrt(1. - dzdl * dzdl);
531 
532  // dphi/dpx
533  dApdp(0, 0) = -py * pT_inv2;
534  // dphi/dpy
535  dApdp(0, 1) = px * pT_inv2;
536 
537  // dk/dpx = -0.003*B*(px*px+py*py)^(-3/2)*px
538  float pTneg3 = (pT_inv * pT_inv2);
539  dApdp(1, 0) = -0.003 * Bfield * px * pTneg3;
540  // dk/dpy
541  dApdp(1, 1) = -0.003 * Bfield * py * pTneg3;
542 
543  float pmag = sqrt(pT * pT + pz * pz);
544  float pmag_inv = 1. / pmag;
545  float pmag_inv_3 = pmag_inv * pmag_inv * pmag_inv;
546  // ddzdl/dpx = -pz*(px^2 + py^2 + pz^2)^(-3/2)*px
547  dApdp(2, 0) = -pz * pmag_inv_3 * px;
548  dApdp(2, 1) = -pz * pmag_inv_3 * py;
549  dApdp(2, 2) = -pz * pmag_inv_3 * pz + pmag_inv;
550 
551  p(0) = px;
552  p(1) = py;
553  p(2) = pz;
554 
555  // dnu/dx = dk/dx * dnu/dk
556  // TODO put this directly into the calculation above
557  dApdp(1, 0) *= 0.5 / state.nu;
558  dApdp(1, 1) *= 0.5 / state.nu;
559 }
560 
561 // dp/db : b is defined by p = R*s*b , with R being a rotation matrix rotating
562 // the unit vector in the z-direction to the direction of p, and S is a scalar
563 // with a value of the magnitude of the current value of p
564 void HelixKalmanFilter::calculate_dpdb(Vector3f& p, Matrix<float, 3, 3>& dpdb) {
565  Vector3f b = Vector3f::Zero(3);
566  b(2) = 1.;
567 
568  float p_mag = sqrt(p.dot(p));
569 
570  Vector3f p_unit = p / p_mag;
571 
572  Vector3f axis = b.cross(p_unit);
573  float angle = acos(p_unit.dot(b));
574 
575  axis /= sqrt(axis.dot(axis));
576 
577  Matrix<float, 3, 3> rot_p = Matrix<float, 3, 3>::Zero(3, 3);
578  float cos_p = cos(angle);
579  float sin_p = sin(angle);
580  rot_p(0, 0) = cos_p + axis(0) * axis(0) * (1. - cos_p);
581  rot_p(0, 1) = axis(0) * axis(1) * (1. - cos_p) - axis(2) * sin_p;
582  rot_p(0, 2) = axis(0) * axis(2) * (1. - cos_p) + axis(1) * sin_p;
583  rot_p(1, 0) = axis(1) * axis(0) * (1. - cos_p) + axis(2) * sin_p;
584  rot_p(1, 1) = cos_p + axis(1) * axis(1) * (1. - cos_p);
585  rot_p(1, 2) = axis(1) * axis(2) * (1. - cos_p) - axis(0) * sin_p;
586  rot_p(2, 0) = axis(2) * axis(0) * (1. - cos_p) - axis(1) * sin_p;
587  rot_p(2, 1) = axis(2) * axis(1) * (1. - cos_p) + axis(0) * sin_p;
588  rot_p(2, 2) = cos_p + axis(2) * axis(2) * (1. - cos_p);
589 
590  dpdb = rot_p * p_mag;
591 }
592 
593 // db/dt , with b a vector, and t a being the two rotation angles, one around
594 // the x axis and one around the y axis. The derivative db/dt is calculated at
595 // the value of b being the unit vector in the z direction
596 void HelixKalmanFilter::calculate_dbdt(Matrix<float, 3, 2>& dbdt_out) {
597  // bz = (1 + [tan(t1)]^2 + [tan(t2)]^2)^(-1/2)
598  // bx = bz*tan(t1)
599  // by = bz*tan(t2)
600 
601  // dbz/dt1 = -[(1 + [tan(t1)]^2 + [tan(t2)]^2)^(-3/2)]*( tan(t1)*( 1 +
602  // [tan(t1)]^2 ) )
603 
604  // dbx/dt1 = tan(t1)*(dbz/dt1) + bz*( 1 + [tan(t1)]^2 )
605  // dbx/dt2 = tan(t1)*(dbz/dt2)
606 
607  // dby/dt1 = tan(t2)*(dbz/dt1)
608  // dby/dt2 = tan(t2)*(dbz/dt2) + bz*( 1 + [tan(t2)]^2 )
609 
610  // t1 = t2 = 0
611 
612  // float tant1 = tan(t1);
613  // float tant1_2 = tant1*tant1;
614  // float tant2 = tan(t2);
615  // float tant2_2 = tant2*tant2;
616  // float bz = 1./sqrt(1. + tant1_2 + tant2_2);
617  // dbdt_out(2,0) = -bz*bz*bz*tant1*(1. + tant1_2);
618  // dbdt_out(2,1) = -bz*bz*bz*tant2*(1. + tant2_2);
619  //
620  // dbdt_out(0,0) = tant1*dbdt_out(2,0) + bz*(1. + tant1_2);
621  // dbdt_out(0,1) = tant1*dbdt_out(2,1);
622  //
623  // dbdt_out(1,0) = tant2*dbdt_out(2,0);
624  // dbdt_out(1,1) = tant2*dbdt_out(2,1) + bz*(1. + tant2_2);
625 
626  dbdt_out(2, 0) = 0.;
627  dbdt_out(2, 1) = 0.;
628 
629  dbdt_out(0, 0) = 1.;
630  dbdt_out(0, 1) = 0.;
631 
632  dbdt_out(1, 0) = 0.;
633  dbdt_out(1, 1) = 1.;
634 }
635 
637  Matrix<float, 3, 5>& dxda, float& x,
638  float& y, float& z) {
639  float phi = state.phi;
640  float d = state.d;
641  float k = state.kappa;
642  float z0 = state.z0;
643  float dzdl = state.dzdl;
644 
645  float rad_det = det_radii[hit.get_layer()];
646 
647  float cosphi = cos(phi);
648  float sinphi = sin(phi);
649 
650  // float signk = (float)(sign(k));
651  k = fabs(k);
652 
653  float kd = (d * k + 1.);
654  float kcx = kd * cosphi;
655  float kcy = kd * sinphi;
656  float kd_inv = 1. / kd;
657  float R2 = rad_det * rad_det;
658  float a = 0.5 * (k * R2 + (d * d * k + 2. * d)) * kd_inv;
659  float tmp1 = a * kd_inv;
660  float P2x = kcx * tmp1;
661  float P2y = kcy * tmp1;
662 
663  float h = sqrt(R2 - a * a);
664 
665  float ux = -kcy * kd_inv;
666  float uy = kcx * kd_inv;
667 
668  float x1 = P2x + ux * h;
669  float y1 = P2y + uy * h;
670  float x2 = P2x - ux * h;
671  float y2 = P2y - uy * h;
672  float diff1 = (x1 - hit.get_x()) * (x1 - hit.get_x()) +
673  (y1 - hit.get_y()) * (y1 - hit.get_y());
674  float diff2 = (x2 - hit.get_x()) * (x2 - hit.get_x()) +
675  (y2 - hit.get_y()) * (y2 - hit.get_y());
676  float signk = 0.;
677  if (diff1 < diff2) {
678  signk = 1.;
679  } else {
680  signk = -1.;
681  }
682  signk_store = signk;
683  x = P2x + signk * ux * h;
684  y = P2y + signk * uy * h;
685 
686  // dx/dphi
687 
688  // (d/dA)[ (d*k +1.)*cos(phi) ]
689  float dkcxdA = -kd * sinphi;
690  // (d/dA)[ (d*k +1.)*sin(phi) ]
691  float dkcydA = kd * cosphi;
692  // (d/dA)[ 1/(k*d + 1) ]
693  float dkd_invdA = 0.;
694  // 0.5*[ (k*R2 + ( d*d*k + 2.*d ))*dkd_inv/dA + kd_inv*(R2*dk/dA +
695  // 2*dd/dA*(k*d + 1) + (d^2)*dk/dA) ]
696  float dadA = 0.;
697  float dtmp1dA = dadA * kd_inv + a * dkd_invdA;
698  float dP2xdA = tmp1 * dkcxdA + kcx * dtmp1dA;
699  float duxdA = -kd_inv * dkcydA - kcy * dkd_invdA;
700  float dhdA = (0.5 / sqrt(R2 - a * a)) * (-2. * a * dadA);
701  dxda(0, 0) = dP2xdA + signk * (duxdA * h + ux * dhdA);
702 
703  // dy/dphi
704 
705  float duydA = kd_inv * dkcxdA + kcx * dkd_invdA;
706  float dP2ydA = tmp1 * dkcydA + kcy * dtmp1dA;
707  dxda(1, 0) = dP2ydA + signk * (duydA * h + uy * dhdA);
708 
709  // dx/dd
710 
711  // (d/dA)[ (d*k +1.)*cos(phi) ]
712  dkcxdA = cosphi * k;
713  // (d/dA)[ (d*k +1.)*sin(phi) ]
714  dkcydA = sinphi * k;
715  // (d/dA)[ 1/(k*d + 1) ]
716  dkd_invdA = -kd_inv * kd_inv * k;
717  // 0.5*[ (k*R2 + ( d*d*k + 2.*d ))*dkd_inv/dA + kd_inv*(R2*dk/dA +
718  // 2*dd/dA*(k*d + 1) + (d^2)*dk/dA) ]
719  dadA =
720  0.5 * ((k * R2 + (d * d * k + 2. * d)) * dkd_invdA + kd_inv * (2. * kd));
721  dtmp1dA = dadA * kd_inv + a * dkd_invdA;
722  dP2xdA = tmp1 * dkcxdA + kcx * dtmp1dA;
723  duxdA = -kd_inv * dkcydA - kcy * dkd_invdA;
724  dhdA = (0.5 / sqrt(R2 - a * a)) * (-2. * a * dadA);
725  dxda(0, 1) = dP2xdA + signk * (duxdA * h + ux * dhdA);
726 
727  // dy/dd
728 
729  duydA = kd_inv * dkcxdA + kcx * dkd_invdA;
730  dP2ydA = tmp1 * dkcydA + kcy * dtmp1dA;
731  dxda(1, 1) = dP2ydA + signk * (duydA * h + uy * dhdA);
732 
733  // dx/dk
734 
735  // (d/dA)[ (d*k +1.)*cos(phi) ]
736  dkcxdA = cosphi * d;
737  // (d/dA)[ (d*k +1.)*sin(phi) ]
738  dkcydA = sinphi * d;
739  // (d/dA)[ 1/(k*d + 1) ]
740  dkd_invdA = -kd_inv * kd_inv * d;
741  // 0.5*[ (k*R2 + ( d*d*k + 2.*d ))*dkd_inv/dA + kd_inv*(R2*dk/dA +
742  // 2*dd/dA*(k*d + 1) + (d^2)*dk/dA) ]
743  dadA = 0.5 *
744  ((k * R2 + (d * d * k + 2. * d)) * dkd_invdA + kd_inv * (R2 + d * d));
745  dtmp1dA = dadA * kd_inv + a * dkd_invdA;
746  dP2xdA = tmp1 * dkcxdA + kcx * dtmp1dA;
747  duxdA = -kd_inv * dkcydA - kcy * dkd_invdA;
748  dhdA = (0.5 / sqrt(R2 - a * a)) * (-2. * a * dadA);
749  dxda(0, 2) = dP2xdA + signk * (duxdA * h + ux * dhdA);
750 
751  // dy/dk
752 
753  duydA = kd_inv * dkcxdA + kcx * dkd_invdA;
754  dP2ydA = tmp1 * dkcydA + kcy * dtmp1dA;
755  dxda(1, 2) = dP2ydA + signk * (duydA * h + uy * dhdA);
756 
757  // now for the z direction
758 
759  float sign_dzdl = sign(dzdl);
760  float onedzdl2_inv = 1. / (1. - dzdl * dzdl);
761  float startx = d * cosphi;
762  float starty = d * sinphi;
763  float D = sqrt((startx - x) * (startx - x) + (starty - y) * (starty - y));
764  float D_inv = 1. / D;
765  float v = 0.5 * k * D;
766  z = 0.;
767  if (v > 0.1) {
768  if (v >= 0.999999) {
769  v = 0.999999;
770  }
771  float s = 2. * asin(v) / k;
772  float s_inv = 1. / s;
773  float sqrtvv = sqrt(1 - v * v);
774  float dz = sqrt(s * s * dzdl * dzdl / (1. - dzdl * dzdl));
775  z = z0 + sign_dzdl * dz;
776  float dz_inv = 1. / dz;
777  float dz2 = dz * dz;
778 
779  // dz/dphi
780  float dstartxdA = -d * sinphi;
781  float dstartydA = d * cosphi;
782  float dkdA = 0.;
783  float ddzdldA = 0.;
784  float dz0dA = 0.;
785 
786  float dDdA = 0.5 * D_inv * (2. * (startx - x) * dstartxdA +
787  2. * (starty - y) * dstartydA);
788  float dvdA = 0.5 * (k * dDdA + D * dkdA);
789  float dsdA = (2. / (k * sqrtvv)) * dvdA - (s / k) * dkdA;
790  float ddzdA = 0.5 * dz_inv *
791  (2. * (dsdA)*dz2 * s_inv +
792  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
793  dxda(2, 0) = dz0dA + sign_dzdl * ddzdA;
794 
795  // dz/dd
796  dstartxdA = cosphi;
797  dstartydA = sinphi;
798  dkdA = 0.;
799  ddzdldA = 0.;
800  dz0dA = 0.;
801 
802  dDdA = 0.5 * D_inv *
803  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
804  dvdA = 0.5 * (k * dDdA + D * dkdA);
805  dsdA = (2. / (k * sqrtvv)) * dvdA - (s / k) * dkdA;
806  ddzdA = 0.5 * dz_inv *
807  (2. * (dsdA)*dz2 * s_inv +
808  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
809  dxda(2, 1) = dz0dA + sign_dzdl * ddzdA;
810 
811  // dz/dk
812  dstartxdA = 0.;
813  dstartydA = 0.;
814  dkdA = 1.;
815  ddzdldA = 0.;
816  dz0dA = 0.;
817 
818  dDdA = 0.5 * D_inv *
819  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
820  dvdA = 0.5 * (k * dDdA + D * dkdA);
821  dsdA = (2. / (k * sqrtvv)) * dvdA - (s / k) * dkdA;
822  ddzdA = 0.5 * dz_inv *
823  (2. * (dsdA)*dz2 * s_inv +
824  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
825  dxda(2, 2) = dz0dA + sign_dzdl * ddzdA;
826 
827  // dz/dz0
828  dstartxdA = 0.;
829  dstartydA = 0.;
830  dkdA = 0.;
831  ddzdldA = 0.;
832  dz0dA = 1.;
833 
834  dDdA = 0.5 * D_inv *
835  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
836  dvdA = 0.5 * (k * dDdA + D * dkdA);
837  dsdA = (2. / (k * sqrtvv)) * dvdA - (s / k) * dkdA;
838  ddzdA = 0.5 * dz_inv *
839  (2. * (dsdA)*dz2 * s_inv +
840  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
841  dxda(2, 3) = dz0dA + sign_dzdl * ddzdA;
842 
843  // dz/dzdl
844  dstartxdA = 0.;
845  dstartydA = 0.;
846  dkdA = 0.;
847  ddzdldA = 1.;
848  dz0dA = 0.;
849 
850  dDdA = 0.5 * D_inv *
851  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
852  dvdA = 0.5 * (k * dDdA + D * dkdA);
853  dsdA = (2. / (k * sqrtvv)) * dvdA - (s / k) * dkdA;
854  ddzdA = 0.5 * dz_inv *
855  (2. * (dsdA)*dz2 * s_inv +
856  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
857  dxda(2, 4) = dz0dA + sign_dzdl * ddzdA;
858  } else {
859  float s = 0.;
860  float temp1 = k * D * 0.5;
861  temp1 *= temp1;
862  float temp2 = D * 0.5;
863  s += 2. * temp2;
864  temp2 *= temp1;
865  s += temp2 / 3.;
866  temp2 *= temp1;
867  s += (3. / 20.) * temp2;
868  temp2 *= temp1;
869  s += (5. / 56.) * temp2;
870  float s_inv = 1. / s;
871  float dz = sqrt(s * s * dzdl * dzdl / (1. - dzdl * dzdl));
872  z = z0 + sign_dzdl * dz;
873  float dz_inv = 1. / dz;
874  float dz2 = dz * dz;
875 
876  float k2 = k * k;
877  float k3 = k2 * k;
878  float k4 = k3 * k;
879  float k5 = k4 * k;
880  float k6 = k5 * k;
881  float D2 = D * D;
882  float D3 = D2 * D;
883  float D4 = D3 * D;
884  float D5 = D4 * D;
885  float D6 = D5 * D;
886  float D7 = D6 * D;
887 
888  // dz/dphi
889  float dstartxdA = -d * sinphi;
890  float dstartydA = d * cosphi;
891  float dkdA = 0.;
892  float ddzdldA = 0.;
893  float dz0dA = 0.;
894 
895  float dDdA = 0.5 * D_inv * (2. * (startx - x) * dstartxdA +
896  2. * (starty - y) * dstartydA);
897  float dsdA = dDdA;
898  dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
899  dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
900  dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
901  float ddzdA = 0.5 * dz_inv *
902  (2. * (dsdA)*dz2 * s_inv +
903  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
904  dxda(2, 0) = dz0dA + sign_dzdl * ddzdA;
905 
906  // dz/dd
907  dstartxdA = cosphi;
908  dstartydA = sinphi;
909  dkdA = 0.;
910  ddzdldA = 0.;
911  dz0dA = 0.;
912 
913  dDdA = 0.5 * D_inv *
914  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
915  dsdA = dDdA;
916  dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
917  dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
918  dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
919  ddzdA = 0.5 * dz_inv *
920  (2. * (dsdA)*dz2 * s_inv +
921  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
922  dxda(2, 1) = dz0dA + sign_dzdl * ddzdA;
923 
924  // dz/dk
925  dstartxdA = 0.;
926  dstartydA = 0.;
927  dkdA = 1.;
928  ddzdldA = 0.;
929  dz0dA = 0.;
930 
931  dDdA = 0.5 * D_inv *
932  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
933  dsdA = dDdA;
934  dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
935  dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
936  dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
937  ddzdA = 0.5 * dz_inv *
938  (2. * (dsdA)*dz2 * s_inv +
939  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
940  dxda(2, 2) = dz0dA + sign_dzdl * ddzdA;
941 
942  // dz/dz0
943  dstartxdA = 0.;
944  dstartydA = 0.;
945  dkdA = 0.;
946  ddzdldA = 0.;
947  dz0dA = 1.;
948 
949  dDdA = 0.5 * D_inv *
950  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
951  dsdA = dDdA;
952  dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
953  dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
954  dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
955  ddzdA = 0.5 * dz_inv *
956  (2. * (dsdA)*dz2 * s_inv +
957  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
958  dxda(2, 3) = dz0dA + sign_dzdl * ddzdA;
959 
960  // dz/ddzdl
961  dstartxdA = 0.;
962  dstartydA = 0.;
963  dkdA = 0.;
964  ddzdldA = 1.;
965  dz0dA = 0.;
966 
967  dDdA = 0.5 * D_inv *
968  (2. * (startx - x) * dstartxdA + 2. * (starty - y) * dstartydA);
969  dsdA = dDdA;
970  dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
971  dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
972  dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
973  ddzdA = 0.5 * dz_inv *
974  (2. * (dsdA)*dz2 * s_inv +
975  s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
976  dxda(2, 4) = dz0dA + sign_dzdl * ddzdA;
977  }
978 
979  // we want dx/dnu instead of dx/dk
980  // nu = sqrt(k)
981  // dx/dnu = dx/dk * dk/dnu
982  // k = nu^2 , dk/dnu = 2.*nu
983  for (int i = 0; i < 3; ++i) {
984  dxda(i, 2) *= 2. * state.nu;
985  }
986 }
987 
988