13 template <
typename propagator_t,
typename propagator_options_t>
22 propagator_options_t pOptions(gctx, mctx);
26 pOptions.targetTolerance =
m_cfg.targetTolerance;
34 .intersect(gctx, params.position(gctx), params.direction(),
false)
42 Direction::fromScalarZeroAsPositive(intersection.pathLength());
45 auto result =
m_cfg.propagator->propagate(params, perigeeSurface, pOptions);
46 if (not result.ok()) {
47 return result.error();
51 auto endParams = *result->endParameters;
75 Vector3 globalCoords = endParams.position(gctx);
81 paramVec << globalCoords, globalTime,
phi,
theta, qOvP;
82 pca << globalCoords, globalTime;
87 ActsMatrix<eBoundSize, eLinSize> completeJacobian =
96 "Wiggled theta outside range, choose a smaller wiggle (i.e., delta)! "
97 "You might need to decrease targetTolerance as well.")
106 paramVecCopy(
i) +=
m_cfg.delta;
115 paramVecCopy.template head<eLinPosSize>(), wiggledDir,
121 .intersect(gctx, paramVecCopy.template head<3>(), wiggledDir,
false)
124 Direction::fromScalarZeroAsPositive(intersection.pathLength());
127 auto newResult =
m_cfg.propagator->propagate(wiggledCurvilinearParams,
128 perigeeSurface, pOptions);
129 if (not newResult.ok()) {
130 return newResult.error();
132 newPerigeeParams = (*newResult->endParameters).
parameters();
135 completeJacobian.array().col(
i) =
136 (newPerigeeParams - perigeeParams) /
m_cfg.delta;
141 perigeeParams(
eLinPhi), 2 * M_PI) /
146 ActsMatrix<eBoundSize, eLinPosSize> positionJacobian =
148 ActsMatrix<eBoundSize, eLinMomSize> momentumJacobian =
153 perigeeParams - positionJacobian * pca - momentumJacobian * momentumAtPCA;
156 linPoint.head<3>() = perigeeSurface.center(gctx);
157 linPoint[3] = linPointTime;
159 return LinearizedTrack(perigeeParams, parCovarianceAtPCA, weightAtPCA,
160 linPoint, positionJacobian, momentumJacobian, pca,
161 momentumAtPCA, constTerm);