/// /// @primary authors: S.Gorbunov; I.Kisel /// @ authors: H.Pabst et al. (Intel); M.Zyzak; I.Kulakov /// #ifndef FitC_H #define FitC_H /// conventinal KF approach #include "fit_interface_functional.h" #include "fit_interface_base.h" template class FitC: public virtual FitFunctional, public FitBase { public: void Extrapolate( FitResults& results, const W &zOut, dense& qp0, const FieldRegionCt &F, dense w) const; protected: void Filter( TracksCt &ts, StationsCt &ss, const HitInfoCt &info, usize iS, const dense &u, const dense w, FitResults& results ) const; void AddMaterial( CovV &C, dense Q22, dense Q32, dense Q33 ) const; void ExtrapolateWithMaterial( FitResults& results, const W &zOut, dense& qp0, const FieldRegionCt &F, StationsCt &ss, usize iStation, bool isPipe, dense w ) const; void FilterFirst( TracksCt &ts, StationsCt &ss, usize iStation, FitResults& results ) const; }; template void FitC::FilterFirst( TracksCt &ts, StationsCt &ss, usize iStation, FitResults& results ) const { dense* T = results.VecT; CovV& C = results.VecC; // CovV &C = track.C; const U w = 1; // hit.w const U w1 = 1 - w; const U c00 = w*ss.XYInfo.C00[iStation] + w1*INF; const U c10 = w*ss.XYInfo.C10[iStation] + w1*INF; const U c11 = w*ss.XYInfo.C11[iStation] + w1*INF; dense _V0 = fill((U)0, vTracks.nTracks ); dense c00V = fill( c00, vTracks.nTracks ); dense c10V = fill( c10, vTracks.nTracks ); dense c11V = fill( c11, vTracks.nTracks ); dense infV = fill( f32(INF), vTracks.nTracks ); dense inf2V = fill( f32(INF2), vTracks.nTracks ); // initialize covariance matrix C.C00 = c00V; //! C00 C.C10 = c10V; C.C11 = c11V; //! C10, C11 C.C20 = _V0; C.C21 = _V0; C.C22 = inf2V; //! C20, C21, C22 C.C30 = _V0; C.C31 = _V0; C.C32 = _V0; C.C33 = inf2V; //! C30, C31, std::complex , C33 C.C40= _V0; C.C41 = _V0; C.C42 = _V0; C.C43 = _V0; C.C44 = infV; //! C40, C41, C42, C43, C44 T[0] = w * ts.hitsX2.row( iStation ) + w1 * T[0]; T[1] = w * ts.hitsY2.row( iStation ) + w1 * T[1]; //! these two variables seem will never be used. //ftype NDF = -3.0; //ftype Chi2 = ZERO; } template void FitC::Extrapolate( FitResults& results, const W &zOut, // extrapolate to this z position dense& qp0, // use Q/p linearisation at this value const FieldRegionCt &F, dense w ) const { // // Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4! // Jacobian_t j; ExtrapolateJ( results.VecT, zOut, qp0, F, j, w ); CovV& C = results.VecC; const dense initialised = (0 >= w); // covariance matrix transport const dense c42 = C.C42, c43 = C.C43; const dense cj00 = C.C00 + C.C20*j(0,2) + C.C30*j(0,3) + C.C40*j(0,4); const dense cj10 = C.C10 + C.C21*j(0,2) + C.C31*j(0,3) + C.C41*j(0,4); const dense cj20 = C.C20 + C.C22*j(0,2) + C.C32*j(0,3) + c42*j(0,4); const dense cj30 = C.C30 + C.C32*j(0,2) + C.C33*j(0,3) + c43*j(0,4); const dense cj01 = C.C10 + C.C20*j(1,2) + C.C30*j(1,3) + C.C40*j(1,4); const dense cj11 = C.C11 + C.C21*j(1,2) + C.C31*j(1,3) + C.C41*j(1,4); const dense cj21 = C.C21 + C.C22*j(1,2) + C.C32*j(1,3) + c42*j(1,4); const dense cj31 = C.C31 + C.C32*j(1,2) + C.C33*j(1,3) + c43*j(1,4); const dense cj02 = C.C20*j(2,2) + C.C30*j(2,3) + C.C40*j(2,4); const dense cj12 = C.C21*j(2,2) + C.C31*j(2,3) + C.C41*j(2,4); const dense cj22 = C.C22*j(2,2) + C.C32*j(2,3) + c42*j(2,4); const dense cj32 = C.C32*j(2,2) + C.C33*j(2,3) + c43*j(2,4); const dense cj03 = C.C20*j(3,2) + C.C30*j(3,3) + C.C40*j(3,4); const dense cj13 = C.C21*j(3,2) + C.C31*j(3,3) + C.C41*j(3,4); const dense cj23 = C.C22*j(3,2) + C.C32*j(3,3) + c42*j(3,4); const dense cj33 = C.C32*j(3,2) + C.C33*j(3,3) + c43*j(3,4); C.C40 += select(initialised, (c42*j(0,2) + c43*j(0,3) + C.C44*j(0,4)), 0); // cj40 C.C41 += select(initialised, (c42*j(1,2) + c43*j(1,3) + C.C44*j(1,4)), 0); // cj41 C.C42 = select(initialised, (c42*j(2,2) + c43*j(2,3) + C.C44*j(2,4)), C.C42); // cj42 C.C43 = select(initialised, (c42*j(3,2) + c43*j(3,3) + C.C44*j(3,4)), C.C43); // cj43 C.C00 = select(initialised, (cj00 + j(0,2)*cj20 + j(0,3)*cj30 + j(0,4)*C.C40), C.C00); C.C10 = select(initialised, (cj01 + j(0,2)*cj21 + j(0,3)*cj31 + j(0,4)*C.C41), C.C10); C.C11 = select(initialised, (cj11 + j(1,2)*cj21 + j(1,3)*cj31 + j(1,4)*C.C41), C.C11); C.C20 = select(initialised, (j(2,2)*cj20 + j(2,3)*cj30 + j(2,4)*C.C40), C.C20); C.C30 = select(initialised, (j(3,2)*cj20 + j(3,3)*cj30 + j(3,4)*C.C40), C.C30); C.C21 = select(initialised, (j(2,2)*cj21 + j(2,3)*cj31 + j(2,4)*C.C41), C.C21); C.C31 = select(initialised, (j(3,2)*cj21 + j(3,3)*cj31 + j(3,4)*C.C41), C.C31); C.C22 = select(initialised, (j(2,2)*cj22 + j(2,3)*cj32 + j(2,4)*C.C42), C.C22); C.C32 = select(initialised, (j(3,2)*cj22 + j(3,3)*cj32 + j(3,4)*C.C42), C.C32); C.C33 = select(initialised, (j(3,2)*cj23 + j(3,3)*cj33 + j(3,4)*C.C43), C.C33); } template void FitC::Filter( TracksCt &ts, StationsCt &ss, const HitInfoCt &info, usize iS, const dense &u, const dense w, FitResults& results ) const { const dense One = fill( 1, u.length() ); const dense Zero = fill( 0, u.length() ); const dense p = One/w; const U w_th = 0.001f; // max w to filter measurement dense mask = w > w_th; dense* T = results.VecT; CovV& C = results.VecC; // convert input dense zeta = info.cos_phi[iS] * T[0] + info.sin_phi[iS] * T[1] - u; // F = CH' dense F0 = info.cos_phi[iS] * C.C00 + info.sin_phi[iS] * C.C10; dense F1 = info.cos_phi[iS] * C.C10 + info.sin_phi[iS] * C.C11; dense HCH = ( F0 * info.cos_phi[iS] + F1 * info.sin_phi[iS] ); dense F2 = info.cos_phi[iS] * C.C20 + info.sin_phi[iS] * C.C21; dense F3 = info.cos_phi[iS] * C.C30 + info.sin_phi[iS] * C.C31; dense F4 = info.cos_phi[iS] * C.C40 + info.sin_phi[iS] * C.C41; dense initialised = ( HCH < info.sigma216[iS]*p ); dense wi = select( mask, w * rcp( info.sigma2[iS]*p + HCH ), Zero ); dense zetawi = w * zeta * rcp( select( initialised, info.sigma2[iS]*p, Zero ) + HCH ); dense chi2 = select( initialised, zeta * zetawi, Zero ); ts.NDF += 1; dense K1 = F1 * wi; dense K2 = F2 * wi; dense K3 = F3 * wi; dense K4 = F4 * wi; T[0]-= F0*zetawi; T[1]-= F1*zetawi; T[2]-= F2*zetawi; T[3]-= F3*zetawi; T[4]-= F4*zetawi; C.C00 -= F0*F0*wi; C.C10 -= K1*F0; C.C11 -= K1*F1; C.C20 -= K2*F0; C.C21 -= K2*F1; C.C22 -= K2*F2; C.C30 -= K3*F0; C.C31 -= K3*F1; C.C32 -= K3*F2; C.C33 -= K3*F3; C.C40 -= K4*F0; C.C41 -= K4*F1; C.C42 -= K4*F2; C.C43 -= K4*F3; C.C44 -= K4*F4; } template inline void FitC::AddMaterial( CovV &C, dense Q22, dense Q32, dense Q33 ) const { C.C22 += Q22; C.C32 += Q32; C.C33 += Q33; } template void FitC::ExtrapolateWithMaterial( FitResults& results, const W &zOut, dense& qp0, const FieldRegionCt &F, StationsCt &ss, usize iStation, bool isPipe, dense w ) const { Extrapolate( results, zOut, qp0, F, w ); FitFunctional::AddMaterial( ss, iStation, qp0, results, isPipe ); } #endif