14 template <
typename input_track_t>
18 const auto vtxPos = vtx.
fullPosition().template head<3>();
24 if (linTrack.covarianceAtPCA.determinant() == 0.) {
30 const auto posJac = linTrack.positionJacobian.block<5, 3>(0, 0);
31 const auto momJac = linTrack.momentumJacobian.block<5, 3>(0, 0);
32 const auto trkParams = linTrack.parametersAtPCA.head<5>();
33 const auto trkParamWeight = linTrack.weightAtPCA.block<5, 5>(0, 0);
37 (momJac.transpose() * (trkParamWeight * momJac)).inverse();
39 const auto residual = linTrack.constantTerm.head<5>();
42 Vector3D newTrkMomentum = sMat * momJac.transpose() * trkParamWeight *
43 (trkParams - residual - posJac * vtxPos);
49 auto correctedPhiTheta =
58 const auto vtxWeight = vtxCov.inverse();
62 -vtxCov * posJac.transpose() * trkParamWeight * momJac * sMat;
67 KalmanVertexUpdater::updatePosition<input_track_t>(
74 auto posDiff = vtx.position() - matrixCache.
newVertexPos;
78 trkParams - (residual + posJac * vtx.fullPosition().template head<3>() +
79 momJac * newTrkMomentum);
82 double chi2 = posDiff.dot(reducedVtxWeight * posDiff) +
83 smParams.dot(trkParamWeight * smParams);
89 newFullTrkCov.block<3, 3>(0, 0) = newTrkCov;
92 vtxFullWeight.block<3, 3>(0, 0) = vtxWeight;
95 vtxFullCov.block<3, 3>(0, 0) = vtxCov;
98 sMat, newFullTrkCov, vtxFullWeight, vtxFullCov, newTrkParams);
101 std::shared_ptr<PerigeeSurface> perigeeSurface =
102 Surface::makeShared<PerigeeSurface>(
106 gctx, std::move(fullPerTrackCov), newTrkParams, perigeeSurface);
124 sMat + (newTrkCov.block<3, 3>(0, 0)).transpose() *
125 (vtxWeight.block<3, 3>(0, 0) * newTrkCov.block<3, 3>(0, 0));
131 fullTrkCov.block<3, 3>(0, 0) = vtxCov.block<3, 3>(0, 0);
132 fullTrkCov.block<3, 3>(0, 3) = newTrkCov.block<3, 3>(0, 0);
133 fullTrkCov.block<3, 3>(3, 0) = (newTrkCov.block<3, 3>(0, 0)).transpose();
134 fullTrkCov.block<3, 3>(3, 3) = momCov;
140 trkJac(0, 0) = -std::sin(newTrkParams[2]);
141 trkJac(0, 1) = std::cos(newTrkParams[2]);
143 double tanTheta = std::tan(newTrkParams[3]);
146 trkJac(1, 0) = -trkJac(0, 1) / tanTheta;
147 trkJac(1, 1) = trkJac(0, 0) / tanTheta;
152 BoundMatrix fullPerTrackCov(BoundMatrix::Identity());
153 fullPerTrackCov.block<5, 5>(0, 0) =
154 (trkJac * (fullTrkCov * trkJac.transpose()));
156 return fullPerTrackCov;