13 using namespace Eigen;
15 static inline float sign(
float x) {
16 return ((
float)(x > 0.)) - ((float)(x < 0.));
20 vector<float>& detector_material,
float B)
21 :
HelixKalman(B), nlayers(detector_radii.size()), signk_store(0.) {
32 Matrix<float, 3, 5>& dxda,
float&
x,
42 float cosphi = cos(phi);
43 float sinphi = sin(phi);
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;
58 float h = sqrt(R2 - a * a);
60 float ux = -kcy * kd_inv;
61 float uy = kcx * kd_inv;
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()) +
69 float diff2 = (x2 - hit.
get_x()) * (x2 - hit.
get_x()) +
78 x = P2x + signk * ux *
h;
79 y = P2y + signk * uy *
h;
84 float dkcxdA = -kd * sinphi;
86 float dkcydA = kd * cosphi;
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);
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);
111 dkd_invdA = -kd_inv * kd_inv *
k;
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);
124 duydA = kd_inv * dkcxdA + kcx * dkd_invdA;
125 dP2ydA = tmp1 * dkcydA + kcy * dtmp1dA;
126 dxda(1, 1) = dP2ydA + signk * (duydA * h + uy * dhdA);
135 dkd_invdA = -kd_inv * kd_inv *
d;
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);
148 duydA = kd_inv * dkcxdA + kcx * dkd_invdA;
149 dP2ydA = tmp1 * dkcydA + kcy * dtmp1dA;
150 dxda(1, 2) = dP2ydA + signk * (duydA * h + uy * dhdA);
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;
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;
175 float dstartxdA = -d * sinphi;
176 float dstartydA = d * cosphi;
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;
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;
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;
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;
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;
255 float temp1 = k * D * 0.5;
257 float temp2 = D * 0.5;
262 s += (3. / 20.) *
temp2;
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;
284 float dstartxdA = -d * sinphi;
285 float dstartydA = d * cosphi;
290 float dDdA = 0.5 * D_inv * (2. * (startx -
x) * dstartxdA +
291 2. * (starty - y) * dstartydA);
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;
309 (2. * (startx -
x) * dstartxdA + 2. * (starty - y) * dstartydA);
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;
327 (2. * (startx -
x) * dstartxdA + 2. * (starty - y) * dstartydA);
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;
345 (2. * (startx -
x) * dstartxdA + 2. * (starty - y) * dstartydA);
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;
363 (2. * (startx -
x) * dstartxdA + 2. * (starty - y) * dstartydA);
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;
378 for (
int i = 0; i < 3; ++i) {
379 dxda(i, 2) *= 2. * state.
nu;
384 Matrix<float, 2, 1>&
ha,
385 Matrix<float, 2, 1>& diff) {
387 if (diff(0, 0) >
M_PI) {
388 diff(0, 0) -= (2. *
M_PI);
390 if (diff(0, 0) < (-
M_PI)) {
391 diff(0, 0) += (2. *
M_PI);
397 Matrix<float, 2, 5>& H,
398 Matrix<float, 2, 1>&
ha) {
404 Matrix<float, 3, 5> dxda = Matrix<float, 3, 5>::Zero(3, 5);
411 Matrix<float, 2, 3> dmdx = Matrix<float, 2, 3>::Zero(2, 3);
413 float r2_inv = 1. / (x * x + y *
y);
414 dmdx(0, 0) = -y * r2_inv;
415 dmdx(0, 1) = x * r2_inv;
420 ha(0, 0) = atan2(y, x);
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);
432 V(0, 0) = 3.33333333333333426e-01 *
441 V(1, 1) = 3.33333333333333426e-01 *
445 G = V.fullPivLu().inverse();
447 m = Matrix<float, 2, 1>::Zero(2, 1);
464 float cosphi = cos(phi);
465 float sinphi = sin(phi);
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;
480 float h = sqrt(R2 - a * a);
482 float ux = -kcy * kd_inv;
483 float uy = kcx * kd_inv;
486 state.
x_int = P2x + signk * ux *
h;
487 state.
y_int = P2y + signk * uy *
h;
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;
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;
504 float temp1 = k * D * 0.5;
506 float temp2 = D * 0.5;
511 s += (3. / 20.) *
temp2;
513 s += (5. / 56.) *
temp2;
514 float dz = sqrt(s * s * dzdl * dzdl / (1. - dzdl * dzdl));
515 state.
z_int = z0 + sign_dzdl *
dz;
528 float p_inv = 3.33333333333333314e+02 * k *
Bfield_inv *