63 double px,
double py,
double pz,
64 double phi0,
double omega,
double tanLambda,
65 const TMatrixD& covTrack) {
68 double pT = std::sqrt(px*px + pz*pz);
70 double pT3 = pT * pT2;
74 double bfield_times_mom = pT * std::fabs(omega);
76 double sign_omega = (omega > 0) ? 1.0 : -1.0;
78 double sin_phi0 = std::sin(phi0);
79 double cos_phi0 = std::cos(phi0);
82 double dpt_domega = -sign_omega / (omega * omega) * bfield_times_mom;
89 J(0, 1) = pT * cos_phi0;
90 J(0, 2) = dpt_domega * sin_phi0;
97 J(1, 2) = dpt_domega * tanLambda;
103 J(2, 1) = -pT * sin_phi0;
104 J(2, 2) = dpt_domega * cos_phi0;
109 TMatrixD J_T(TMatrixD::kTransposed, J);
110 TMatrixD temp = J * covTrack;
111 TMatrixD covMomentum = temp * J_T;
131 float x_vtx,
float y_vtx,
float z_vtx,
132 float px,
float py,
float pz,
float Q,
float cB,
136 float pT = std::sqrt(px*px + pz*pz);
137 float omega = -Q * cB / pT;
140 float tan_lambda = py / pT;
141 float phi0 = std::atan2(px, pz);
144 float tan_L_helix = -tan_lambda;
145 float phi0_helix = -phi0;
146 float alpha_over_kappa = -1.0 / omega;
149 float cos_phi0_h = std::cos(phi0_helix);
150 float sin_phi0_h = std::sin(phi0_helix);
152 float x_center = x_vtx + alpha_over_kappa * cos_phi0_h;
153 float z_center = z_vtx + alpha_over_kappa * sin_phi0_h;
157 float phi0_new_helix = std::atan2(targetPos - z_center, -x_center);
158 float cos_phi_new = std::cos(phi0_new_helix);
159 float sin_phi_new = std::sin(phi0_new_helix);
162 float d0_helix = (x_center * cos_phi_new +
163 (z_center - targetPos) * sin_phi_new -
167 float phi_diff = phi0_new_helix - phi0_helix;
168 float dz_helix = -y_vtx - alpha_over_kappa * phi_diff * tan_L_helix;
171 std::vector<float> params;
172 params.push_back(d0_helix);
173 params.push_back(-phi0_new_helix);
174 params.push_back(omega);
175 params.push_back(-dz_helix);
176 params.push_back(tan_lambda);
186 float x_vtx,
float y_vtx,
float z_vtx,
187 float px,
float py,
float pz,
192 float pT2 = px*px + pz*pz;
193 float pT = std::sqrt(pT2);
194 float pT3 = pT * pT2;
196 float omega = -Q * cB / pT;
198 float tan_lambda = py / pT;
199 float phi0 = std::atan2(px, pz);
202 float tan_L_helix = -tan_lambda;
203 float phi0_helix = -phi0;
204 float alpha_over_kappa = -1.0 / omega;
206 float cos_phi0_h = std::cos(phi0_helix);
207 float sin_phi0_h = std::sin(phi0_helix);
210 float x_center = x_vtx + alpha_over_kappa * cos_phi0_h;
211 float z_center = z_vtx + alpha_over_kappa * sin_phi0_h;
214 double dx_c = -x_center;
215 double dz_c = targetPos - z_center;
216 float r_c2 = dx_c*dx_c + dz_c*dz_c;
218 float phi0_new_helix = std::atan2(dz_c, dx_c);
219 float cos_phi_new = std::cos(phi0_new_helix);
220 float sin_phi_new = std::sin(phi0_new_helix);
222 float phi_diff = phi0_new_helix - phi0_helix;
227 float dpT_dpx = px / pT;
228 float dpT_dpz = pz / pT;
232 float dphi0_dpx = pz / pT2;
233 float dphi0_dpz = -px / pT2;
236 float dphi0h_dpx = -dphi0_dpx;
237 float dphi0h_dpz = -dphi0_dpz;
240 float dtanL_dpx = -py * px / pT3;
241 float dtanL_dpy = 1.0 / pT;
242 float dtanL_dpz = -py * pz / pT3;
245 float domega_dpT = Q * cB / pT2;
248 float dalpha_over_kappa_dpx = (1 / (omega*omega)) * domega_dpT * dpT_dpx;
249 float dalpha_over_kappa_dpy = 0;
250 float dalpha_over_kappa_dpz = (1 / (omega*omega)) * domega_dpT * dpT_dpz;
256 float dxc_dpx = -alpha_over_kappa * sin_phi0_h * dphi0h_dpx + cos_phi0_h * dalpha_over_kappa_dpx;
257 float dxc_dpz = -alpha_over_kappa * sin_phi0_h * dphi0h_dpz + cos_phi0_h * dalpha_over_kappa_dpz;
261 float dzc_dpx = alpha_over_kappa * cos_phi0_h * dphi0h_dpx + sin_phi0_h * dalpha_over_kappa_dpx;
262 float dzc_dpz = alpha_over_kappa * cos_phi0_h * dphi0h_dpz + sin_phi0_h * dalpha_over_kappa_dpz;
265 float dphi0new_dxc = dz_c / r_c2;
266 float dphi0new_dzc = -dx_c / r_c2;
269 float dphi0new_dx = dphi0new_dxc * (dxc_dx);
270 float dphi0new_dz = dphi0new_dzc * (dzc_dz);
271 float dphi0new_dpx = dphi0new_dxc * (dxc_dpx) + dphi0new_dzc * (dzc_dpx);
272 float dphi0new_dpz = dphi0new_dxc * (dxc_dpz) + dphi0new_dzc * (dzc_dpz);
283 double phi_term = -(x_center * sin_phi_new) + (z_center - targetPos) * cos_phi_new;
285 double dd0_dxc = cos_phi_new + phi_term * dphi0new_dxc;
286 double dd0_dzc = sin_phi_new + phi_term * dphi0new_dzc;
288 J(0, 0) = dd0_dxc * dxc_dx;
290 J(0, 2) = dd0_dzc * dzc_dz;
291 J(0, 3) = dd0_dxc * dxc_dpx + dd0_dzc * dzc_dpx - dalpha_over_kappa_dpx;
293 J(0, 5) = dd0_dxc * dxc_dpz + dd0_dzc * dzc_dpz - dalpha_over_kappa_dpz;
299 J(1, 0) = -dphi0new_dx;
301 J(1, 2) = -dphi0new_dz;
302 J(1, 3) = -dphi0new_dpx;
304 J(1, 5) = -dphi0new_dpz;
313 J(2, 3) = domega_dpT * dpT_dpx;
315 J(2, 5) = domega_dpT * dpT_dpz;
324 J(3, 0) = alpha_over_kappa * tan_L_helix * dphi0new_dx;
326 J(3, 2) = alpha_over_kappa * tan_L_helix * dphi0new_dz;
332 double dphidiff_dpx = dphi0new_dpx - dphi0h_dpx;
333 double dphidiff_dpz = dphi0new_dpz - dphi0h_dpz;
335 J(3, 3) = alpha_over_kappa * (dphidiff_dpx * tan_L_helix - phi_diff * dtanL_dpx) + phi_diff * tan_L_helix * dalpha_over_kappa_dpx;
336 J(3, 4) = -alpha_over_kappa * phi_diff * dtanL_dpy;
337 J(3, 5) = alpha_over_kappa * (dphidiff_dpz * tan_L_helix - phi_diff * dtanL_dpz) + phi_diff * tan_L_helix * dalpha_over_kappa_dpz;
423 float x_vtx,
float y_vtx,
float z_vtx,
424 float px,
float py,
float pz,
425 float Q,
float cB,
float targetPos) {
427 const double epsilon = 1e-4;
428 TMatrixD J_num(5, 6);
432 x_vtx, y_vtx, z_vtx, px, py, pz, Q, cB, targetPos);
435 std::vector<double> inputs = {x_vtx, y_vtx, z_vtx, px, py, pz};
437 for (
int j = 0; j < 6; j++) {
438 std::vector<double> inputs_plus = inputs;
439 inputs_plus[j] += epsilon;
442 inputs_plus[0], inputs_plus[1], inputs_plus[2],
443 inputs_plus[3], inputs_plus[4], inputs_plus[5],
447 for (
int i = 0; i < 5; i++) {
448 J_num(i, j) = (params_plus[i] - params_nominal[i]) / epsilon;
452 double diff = params_plus[i] - params_nominal[i];
453 if (diff > M_PI) diff -= 2*M_PI;
454 if (diff < -M_PI) diff += 2*M_PI;
455 J_num(i, j) = diff / epsilon;