11 template <
typename propagator_t,
typename propagator_options_t>
19 const std::shared_ptr<PerigeeSurface> perigeeSurface =
20 Surface::makeShared<PerigeeSurface>(linPointPos);
23 propagator_options_t pOptions(gctx, mctx);
28 auto result = m_cfg.propagator->propagate(params, *perigeeSurface, pOptions);
30 endParams = (*result).endParameters.
get();
33 return result.error();
41 if (endParams->
covariance()->determinant() == 0) {
50 double sinPhiV = std::sin(phiV);
51 double cosPhiV = std::cos(phiV);
55 const double sinTh = std::sin(th);
56 const double tanTh = std::tan(th);
60 double sgnH = (qOvP < 0.) ? -1 : 1;
62 Vector3D momentumAtPCA(phiV, th, qOvP);
70 if (Bz == 0. ||
std::abs(qOvP) < m_cfg.minQoP) {
74 rho2 = sinTh / (qOvP *
Bz);
75 rho = sinTh * (1. / qOvP) / Bz;
79 double X = positionAtPCA(0) - linPointPos.x() + rho * sinPhiV;
80 double Y = positionAtPCA(1) - linPointPos.y() - rho * cosPhiV;
81 const double S2 = (X * X + Y *
Y);
82 const double S = std::sqrt(S2);
89 int sgnX = (X < 0.) ? -1 : 1;
90 int sgnY = (Y < 0.) ? -1 : 1;
94 phiAtPCA = sgnH * sgnX * std::acos(-sgnH * Y / S);
96 phiAtPCA = std::asin(sgnH * X / S);
97 if ((sgnH * sgnY) > 0) {
98 phiAtPCA = sgnH * sgnX *
M_PI - phiAtPCA;
103 predParamsAtPCA[0] = rho - sgnH *
S;
105 positionAtPCA[
eZ] - linPointPos.z() + rho * (phiV - phiAtPCA) / tanTh;
106 predParamsAtPCA[2] = phiAtPCA;
107 predParamsAtPCA[3] = th;
108 predParamsAtPCA[4] = qOvP;
109 predParamsAtPCA[5] = 0.;
113 positionJacobian.setZero();
115 positionJacobian(0, 0) = -sgnH * X /
S;
116 positionJacobian(0, 1) = -sgnH * Y /
S;
118 const double S2tanTh = S2 * tanTh;
121 positionJacobian(1, 0) = rho * Y / S2tanTh;
122 positionJacobian(1, 1) = -rho * X / S2tanTh;
123 positionJacobian(1, 2) = 1.;
126 positionJacobian(2, 0) = -Y / S2;
127 positionJacobian(2, 1) = X / S2;
131 positionJacobian(5, 3) = 1;
135 momentumJacobian.setZero();
137 double R = X * cosPhiV + Y * sinPhiV;
138 double Q = X * sinPhiV - Y * cosPhiV;
139 double dPhi = phiAtPCA - phiV;
142 momentumJacobian(0, 0) = -sgnH * rho * R /
S;
144 double qOvSred = 1 - sgnH * Q /
S;
146 momentumJacobian(0, 1) = qOvSred * rho / tanTh;
147 momentumJacobian(0, 2) = -qOvSred * rho / qOvP;
149 const double rhoOverS2 = rho / S2;
152 momentumJacobian(1, 0) = (1 - rhoOverS2 *
Q) * rho / tanTh;
153 momentumJacobian(1, 1) = (dPhi + rho * R / (S2tanTh * tanTh)) * rho;
154 momentumJacobian(1, 2) = (dPhi - rhoOverS2 *
R) * rho / (qOvP * tanTh);
157 momentumJacobian(2, 0) = rhoOverS2 *
Q;
158 momentumJacobian(2, 1) = -rho * R / S2tanTh;
159 momentumJacobian(2, 2) = rhoOverS2 * R / qOvP;
162 momentumJacobian(3, 1) = 1.;
163 momentumJacobian(4, 2) = 1.;
166 BoundVector constTerm = predParamsAtPCA - positionJacobian * positionAtPCA -
167 momentumJacobian * momentumAtPCA;
171 (parCovarianceAtPCA.block<5, 5>(0, 0)).inverse();
174 weightAtPCA.block<5, 5>(0, 0) = parWeight;
176 return LinearizedTrack(paramsAtPCA, parCovarianceAtPCA, weightAtPCA, linPoint,
177 positionJacobian, momentumJacobian, positionAtPCA,
178 momentumAtPCA, constTerm);