/// /// @primary authors: I.Kisel /// @ authors: M.Zyzak; I.Kulakov /// #ifndef FitSR_H #define FitSR_H /// square root KF approach #include "fit_interface_functional.h" #include "fit_interface_base.h" template class FitSR: 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 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; private: void AddMaterial( CovV &C, dense Q22, dense Q32, dense Q33 ) const; }; template void FitSR::AddMaterial( CovV &C, dense Q22, dense Q32, dense Q33 ) const { // sQ = sqrt(Q)' const dense sQ22 = sqrt( Q22 ); const dense sQ32 = Q32 / sQ22; const dense sQ33 = sqrt( Q33 - sQ32*sQ32 ); { // update covariance matrix // solve [ S_new' ] = T * [ S' ] // [ 0 ] [sqrt(Q)'] // A = [ S' ] // [sqrt(Q)'] // ? ? ? ? ? // 0 ? ? ? ? // 0 0 ? ? ? // 0 0 0 ? ? // A = 0 0 0 0 ? // 0 0 0 0 0 // 0 0 0 0 0 // 0 0 ? ? 0 // 0 0 0 ? 0 // 0 0 0 0 0 const dense Zero = fill(0.f, sQ22.length()); dense A00 = C.S00; dense A01 = C.S10; dense A02 = C.S20; dense A03 = C.S30; dense A04 = C.S40; #ifdef SQUARE_ROOT1S // Potter with square matrix dense A10 = C.S01; dense A11 = C.S11; dense A12 = C.S21; dense A13 = C.S31; dense A14 = C.S41; dense A20 = C.S02; dense A21 = C.S12; dense A22 = C.S22; dense A23 = C.S32; dense A24 = C.S42; dense A30 = C.S03; dense A31 = C.S13; dense A32 = C.S23; dense A33 = C.S33; dense A34 = C.S43; dense A40 = C.S04; dense A41 = C.S14; dense A42 = C.S24; dense A43 = C.S34; dense A44 = C.S44; #else // triangle dense A10 = Zero; dense A11 = C.S11; dense A12 = C.S21; dense A13 = C.S31; dense A14 = C.S41; dense A20 = Zero; dense A21 = Zero; dense A22 = C.S22; dense A23 = C.S32; dense A24 = C.S42; dense A30 = Zero; dense A31 = Zero; dense A32 = Zero; dense A33 = C.S33; dense A34 = C.S43; dense A40 = Zero; dense A41 = Zero; dense A42 = Zero; dense A43 = Zero; dense A44 = C.S44; #endif // SQUARE_ROOT1S dense A70 = Zero; dense A71 = Zero; dense A72 = sQ22; dense A73 = sQ32; dense A74 = Zero; dense A80 = Zero; dense A81 = Zero; dense A82 = Zero; dense A83 = sQ33; dense A84 = Zero; #define S_DEFINE( k ) \ const dense s##k = sqrt( A0##k*A0##k + A1##k*A1##k + A2##k*A2##k + A3##k*A3##k + A4##k*A4##k + A7##k*A7##k + A8##k*A8##k ); \ const dense is##k = 1.f/s##k; #define W_DEFINE( k, j ) \ const dense W##k##j = is##k*( A0##k*A0##j + A1##k*A1##j + A2##k*A2##j + A3##k*A3##j + A4##k*A4##j + A7##k*A7##j + A8##k*A8##j ) #define A_UPDATE( k, j, l ) A##l##j -= is##k * W##k##j * A##l##k #define A_UPDATE_COLUMN( k, j ) \ A_UPDATE( k, j, 0 ); \ A_UPDATE( k, j, 1 ); \ A_UPDATE( k, j, 2 ); \ A_UPDATE( k, j, 3 ); \ A_UPDATE( k, j, 4 ); \ A_UPDATE( k, j, 7 ); \ A_UPDATE( k, j, 8 ); S_DEFINE( 0 ); const dense W00 = s0; W_DEFINE( 0, 1 ); W_DEFINE( 0, 2 ); W_DEFINE( 0, 3 ); W_DEFINE( 0, 4 ); A_UPDATE_COLUMN( 0, 1 ); A_UPDATE_COLUMN( 0, 2 ); A_UPDATE_COLUMN( 0, 3 ); A_UPDATE_COLUMN( 0, 4 ); S_DEFINE( 1 ); const dense W11 = s1; W_DEFINE( 1, 2 ); W_DEFINE( 1, 3 ); W_DEFINE( 1, 4 ); A_UPDATE_COLUMN( 1, 2 ); A_UPDATE_COLUMN( 1, 3 ); A_UPDATE_COLUMN( 1, 4 ); S_DEFINE( 2 ); const dense W22 = s2; W_DEFINE( 2, 3 ); W_DEFINE( 2, 4 ); A_UPDATE_COLUMN( 2, 3 ); A_UPDATE_COLUMN( 2, 4 ); S_DEFINE( 3 ); const dense W33 = s3; W_DEFINE( 3, 4 ); A_UPDATE_COLUMN( 3, 4 ); S_DEFINE( 4 ); const dense W44 = s4; C.S00 = W00; C.S10 = W01; C.S11 = W11; C.S20 = W02; C.S21 = W12; C.S22 = W22; C.S30 = W03; C.S31 = W13; C.S32 = W23; C.S33 = W33; C.S40 = W04; C.S41 = W14; C.S42 = W24; C.S43 = W34; C.S44 = W44; #ifdef SQUARE_ROOT1S // Potter with square matrix C.S01 = C.S02 = C.S12 = C.S03 = C.S13 = C.S23 = C.S04 = C.S14 = C.S24 = C.S34 = Zero; #endif // SQUARE_ROOT1S #undef A_UPDATE_COLUMN #undef A_UPDATE #undef W_DEFINE #undef S_DEFINE } } template void FitSR::FilterFirst( TracksCt &ts, StationsCt &ss, usize iStation, FitResults& results ) const { dense* T = results.VecT; CovV& C = results.VecC; U w = 1; U w1 = 1 - w; const U s00 = sqrt(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; const U s10 = c10/s00; const U s11 = c11 - s10*s10; dense _V0 = fill((U)0, vTracks.nTracks ); dense sigV = fill( s00, vTracks.nTracks ); dense infV = fill( sqrt(U(INF)), vTracks.nTracks ); dense inf2V = fill( sqrt(U(INF2)), vTracks.nTracks ); // initialize covariance matrix C.S00 = fill( s00, vTracks.nTracks ); //! C00 C.S10 = fill( s10, vTracks.nTracks ); C.S11 = fill( s11, vTracks.nTracks ); //! C10, C11 C.S20 = _V0; C.S21 = _V0; C.S22 = inf2V; //! C20, C21, C22 C.S30 = _V0; C.S31 = _V0; C.S32 = _V0; C.S33 = inf2V; //! C30, C31, std::complex , C33 C.S40 = _V0; C.S41 = _V0; C.S42 = _V0; C.S43 = _V0; C.S44 = infV; //! C40, C41, C42, C43, C44 #ifdef SQUARE_ROOT1S // Potter with square matrix C.S01 = C.S02 = C.S12 = C.S03 = C.S13 = C.S23 = C.S04 = C.S14 = C.S24 = C.S34 = _V0; #endif // SQUARE_ROOT1S T[0] = w * ts.hitsX2.row( iStation ) + w1 * T[0]; T[1] = w * ts.hitsY2.row( iStation ) + w1 * T[1]; } template void FitSR::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 { // update covariance matrix // solve [ S_new' ] = [ W ] = T * [ S'*J'] // A = S'*J' #ifdef SQUARE_ROOT1S // Potter with square matrix dense A00 = C.S00 + C.S20*j(0,2) + C.S30*j(0,3) + C.S40*j(0,4); dense A01 = C.S10 + C.S20*j(1,2) + C.S30*j(1,3) + C.S40*j(1,4); dense A02 = C.S20*j(2,2) + C.S30*j(2,3) + C.S40*j(2,4); dense A03 = C.S20*j(3,2) + C.S30*j(3,3) + C.S40*j(3,4); dense A04 = C.S40; dense A10 = C.S01 + C.S21*j(0,2) + C.S31*j(0,3) + C.S41*j(0,4); dense A11 = C.S11 + C.S21*j(1,2) + C.S31*j(1,3) + C.S41*j(1,4); dense A12 = C.S21*j(2,2) + C.S31*j(2,3) + C.S41*j(2,4); dense A13 = C.S21*j(3,2) + C.S31*j(3,3) + C.S41*j(3,4); dense A14 = C.S41; dense A20 = C.S02 + C.S22*j(0,2) + C.S32*j(0,3) + C.S42*j(0,4); dense A21 = C.S12 + C.S22*j(1,2) + C.S32*j(1,3) + C.S42*j(1,4); dense A22 = C.S22*j(2,2) + C.S32*j(2,3) + C.S42*j(2,4); dense A23 = C.S22*j(3,2) + C.S32*j(3,3) + C.S42*j(3,4); dense A24 = C.S42; dense A30 = C.S03 + C.S23*j(0,2) + C.S33*j(0,3) + C.S43*j(0,4); dense A31 = C.S13 + C.S23*j(1,2) + C.S33*j(1,3) + C.S43*j(1,4); dense A32 = C.S23*j(2,2) + C.S33*j(2,3) + C.S43*j(2,4); dense A33 = C.S23*j(3,2) + C.S33*j(3,3) + C.S43*j(3,4); dense A34 = C.S43; dense A40 = C.S04 + C.S24*j(0,2) + C.S34*j(0,3) + C.S44*j(0,4); dense A41 = C.S14 + C.S24*j(1,2) + C.S34*j(1,3) + C.S44*j(1,4); dense A42 = C.S24*j(2,2) + C.S34*j(2,3) + C.S44*j(2,4); dense A43 = C.S24*j(3,2) + C.S34*j(3,3) + C.S44*j(3,4); dense A44 = C.S44; #else // triangular dense A00 = C.S00 + C.S20*j(0,2) + C.S30*j(0,3) + C.S40*j(0,4); dense A01 = C.S10 + C.S20*j(1,2) + C.S30*j(1,3) + C.S40*j(1,4); dense A02 = C.S20*j(2,2) + C.S30*j(2,3) + C.S40*j(2,4); dense A03 = C.S20*j(3,2) + C.S30*j(3,3) + C.S40*j(3,4); dense A04 = C.S40; dense A10 = C.S21*j(0,2) + C.S31*j(0,3) + C.S41*j(0,4); dense A11 = C.S11 + C.S21*j(1,2) + C.S31*j(1,3) + C.S41*j(1,4); dense A12 = C.S21*j(2,2) + C.S31*j(2,3) + C.S41*j(2,4); dense A13 = C.S21*j(3,2) + C.S31*j(3,3) + C.S41*j(3,4); dense A14 = C.S41; dense A20 = C.S22*j(0,2) + C.S32*j(0,3) + C.S42*j(0,4); dense A21 = C.S22*j(1,2) + C.S32*j(1,3) + C.S42*j(1,4); dense A22 = C.S22*j(2,2) + C.S32*j(2,3) + C.S42*j(2,4); dense A23 = C.S22*j(3,2) + C.S32*j(3,3) + C.S42*j(3,4); dense A24 = C.S42; dense A30 = C.S33*j(0,3) + C.S43*j(0,4); dense A31 = C.S33*j(1,3) + C.S43*j(1,4); dense A32 = C.S33*j(2,3) + C.S43*j(2,4); dense A33 = C.S33*j(3,3) + C.S43*j(3,4); dense A34 = C.S43; dense A40 = C.S44*j(0,4); dense A41 = C.S44*j(1,4); dense A42 = C.S44*j(2,4); dense A43 = C.S44*j(3,4); dense A44 = C.S44; #endif // square matrix #define S_DEFINE( k ) \ const dense s##k = sqrt( A0##k*A0##k + A1##k*A1##k + A2##k*A2##k + A3##k*A3##k + A4##k*A4##k ); \ const dense is##k = 1.f/s##k; #define W_DEFINE( k, j ) const dense W##k##j = is##k*( A0##k*A0##j + A1##k*A1##j + A2##k*A2##j + A3##k*A3##j + A4##k*A4##j ) #define A_UPDATE( k, j, l ) A##l##j -= is##k * W##k##j * A##l##k #define A_UPDATE_COLUMN( k, j ) \ A_UPDATE( k, j, 0 ); \ A_UPDATE( k, j, 1 ); \ A_UPDATE( k, j, 2 ); \ A_UPDATE( k, j, 3 ); \ A_UPDATE( k, j, 4 ); S_DEFINE( 0 ); const dense W00 = s0; W_DEFINE( 0, 1 ); W_DEFINE( 0, 2 ); W_DEFINE( 0, 3 ); W_DEFINE( 0, 4 ); A_UPDATE_COLUMN( 0, 1 ); A_UPDATE_COLUMN( 0, 2 ); A_UPDATE_COLUMN( 0, 3 ); A_UPDATE_COLUMN( 0, 4 ); S_DEFINE( 1 ); const dense W11 = s1; W_DEFINE( 1, 2 ); W_DEFINE( 1, 3 ); W_DEFINE( 1, 4 ); A_UPDATE_COLUMN( 1, 2 ); A_UPDATE_COLUMN( 1, 3 ); A_UPDATE_COLUMN( 1, 4 ); S_DEFINE( 2 ); const dense W22 = s2; W_DEFINE( 2, 3 ); W_DEFINE( 2, 4 ); A_UPDATE_COLUMN( 2, 3 ); A_UPDATE_COLUMN( 2, 4 ); S_DEFINE( 3 ); const dense W33 = s3; W_DEFINE( 3, 4 ); A_UPDATE_COLUMN( 3, 4 ); S_DEFINE( 4 ); const dense W44 = s4; C.S00 = W00; C.S10 = W01; C.S11 = W11; C.S20 = W02; C.S21 = W12; C.S22 = W22; C.S30 = W03; C.S31 = W13; C.S32 = W23; C.S33 = W33; C.S40 = W04; C.S41 = W14; C.S42 = W24; C.S43 = W34; C.S44 = W44; #ifdef SQUARE_ROOT1S // Potter with square matrix C.S01 = C.S02 = C.S12 = C.S03 = C.S13 = C.S23 = C.S04 = C.S14 = C.S24 = C.S34 = fill(0,C.S34.length()); #endif // SQUARE_ROOT1S #undef A_UPDATE_COLUMN #undef A_UPDATE #undef W_DEFINE #undef S_DEFINE } } template void FitSR::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; const dense V = info.sigma2[iS]*p; const U &H0 = info.cos_phi[iS]; const U &H1 = info.sin_phi[iS]; // F = S'*H' const dense F0 = C.S00*H0 + C.S10*H1; #if !defined(SQUARE_ROOT1S) const dense F1 = C.S11*H1; #else const dense F1 = C.S01*H0 + C.S11*H1; const dense F2 = C.S02*H0 + C.S12*H1; const dense F3 = C.S03*H0 + C.S13*H1; const dense F4 = C.S04*H0 + C.S14*H1; #endif const dense zeta = (H0*T[0] + H1*T[1] - u); #ifdef SQUARE_ROOT1S // Potter // a = 1/(F'*F + V) const dense FF = F0*F0 + F1*F1; const dense a = select(mask, rcp(FF + V), Zero); // K = a*S*F const dense aF0 = a*F0; const dense aF1 = a*F1; const dense aF2 = a*F2; const dense aF3 = a*F3; const dense aF4 = a*F4; const dense K0 = C.S00*aF0 + C.S01*aF1 + C.S02*aF2 + C.S03*aF3 + C.S04*aF4; const dense K1 = C.S10*aF0 + C.S11*aF1 + C.S12*aF2 + C.S13*aF3 + C.S14*aF4; const dense K2 = C.S20*aF0 + C.S21*aF1 + C.S22*aF2 + C.S23*aF3 + C.S24*aF4; const dense K3 = C.S30*aF0 + C.S31*aF1 + C.S32*aF2 + C.S33*aF3 + C.S34*aF4; const dense K4 = C.S40*aF0 + C.S41*aF1 + C.S42*aF2 + C.S43*aF3 + C.S44*aF4; T[0] -= K0*zeta; T[1] -= K1*zeta; T[2] -= K2*zeta; T[3] -= K3*zeta; T[4] -= K4*zeta; dense Chi2 = zeta * a * zeta ; ts.NDF += 1; const dense gamma = 1/(1 + sqrt(a*V)); // S = S - S*a*gamma*F*F' #define GAFF_DEFINE(i,j) const dense gaFF##i##j = gamma*aF##i*F##j; GAFF_DEFINE(0,0); GAFF_DEFINE(1,0); GAFF_DEFINE(1,1); GAFF_DEFINE(2,0); GAFF_DEFINE(2,1); GAFF_DEFINE(2,2); GAFF_DEFINE(3,0); GAFF_DEFINE(3,1); GAFF_DEFINE(3,2); GAFF_DEFINE(3,3); GAFF_DEFINE(4,0); GAFF_DEFINE(4,1); GAFF_DEFINE(4,2); GAFF_DEFINE(4,3); GAFF_DEFINE(4,4); #undef GAFF_DEFINE CovV Cin = C; C.S00 -= Cin.S00*gaFF00 + Cin.S01*gaFF10 + Cin.S02*gaFF20 + Cin.S03*gaFF30 + Cin.S04*gaFF40; C.S01 -= Cin.S00*gaFF10 + Cin.S01*gaFF11 + Cin.S02*gaFF21 + Cin.S03*gaFF31 + Cin.S04*gaFF41; C.S02 -= Cin.S00*gaFF20 + Cin.S01*gaFF21 + Cin.S02*gaFF22 + Cin.S03*gaFF32 + Cin.S04*gaFF42; C.S03 -= Cin.S00*gaFF30 + Cin.S01*gaFF31 + Cin.S02*gaFF32 + Cin.S03*gaFF33 + Cin.S04*gaFF43; C.S04 -= Cin.S00*gaFF40 + Cin.S01*gaFF41 + Cin.S02*gaFF42 + Cin.S03*gaFF43 + Cin.S04*gaFF44; C.S10 -= Cin.S10*gaFF00 + Cin.S11*gaFF10 + Cin.S12*gaFF20 + Cin.S13*gaFF30 + Cin.S14*gaFF40; C.S11 -= Cin.S10*gaFF10 + Cin.S11*gaFF11 + Cin.S12*gaFF21 + Cin.S13*gaFF31 + Cin.S14*gaFF41; C.S12 -= Cin.S10*gaFF20 + Cin.S11*gaFF21 + Cin.S12*gaFF22 + Cin.S13*gaFF32 + Cin.S14*gaFF42; C.S13 -= Cin.S10*gaFF30 + Cin.S11*gaFF31 + Cin.S12*gaFF32 + Cin.S13*gaFF33 + Cin.S14*gaFF43; C.S14 -= Cin.S10*gaFF40 + Cin.S11*gaFF41 + Cin.S12*gaFF42 + Cin.S13*gaFF43 + Cin.S14*gaFF44; C.S20 -= Cin.S20*gaFF00 + Cin.S21*gaFF10 + Cin.S22*gaFF20 + Cin.S23*gaFF30 + Cin.S24*gaFF40; C.S21 -= Cin.S20*gaFF10 + Cin.S21*gaFF11 + Cin.S22*gaFF21 + Cin.S23*gaFF31 + Cin.S24*gaFF41; C.S22 -= Cin.S20*gaFF20 + Cin.S21*gaFF21 + Cin.S22*gaFF22 + Cin.S23*gaFF32 + Cin.S24*gaFF42; C.S23 -= Cin.S20*gaFF30 + Cin.S21*gaFF31 + Cin.S22*gaFF32 + Cin.S23*gaFF33 + Cin.S24*gaFF43; C.S24 -= Cin.S20*gaFF40 + Cin.S21*gaFF41 + Cin.S22*gaFF42 + Cin.S23*gaFF43 + Cin.S24*gaFF44; C.S30 -= Cin.S30*gaFF00 + Cin.S31*gaFF10 + Cin.S32*gaFF20 + Cin.S33*gaFF30 + Cin.S34*gaFF40; C.S31 -= Cin.S30*gaFF10 + Cin.S31*gaFF11 + Cin.S32*gaFF21 + Cin.S33*gaFF31 + Cin.S34*gaFF41; C.S32 -= Cin.S30*gaFF20 + Cin.S31*gaFF21 + Cin.S32*gaFF22 + Cin.S33*gaFF32 + Cin.S34*gaFF42; C.S33 -= Cin.S30*gaFF30 + Cin.S31*gaFF31 + Cin.S32*gaFF32 + Cin.S33*gaFF33 + Cin.S34*gaFF43; C.S34 -= Cin.S30*gaFF40 + Cin.S31*gaFF41 + Cin.S32*gaFF42 + Cin.S33*gaFF43 + Cin.S34*gaFF44; C.S40 -= Cin.S40*gaFF00 + Cin.S41*gaFF10 + Cin.S42*gaFF20 + Cin.S43*gaFF30 + Cin.S44*gaFF40; C.S41 -= Cin.S40*gaFF10 + Cin.S41*gaFF11 + Cin.S42*gaFF21 + Cin.S43*gaFF31 + Cin.S44*gaFF41; C.S42 -= Cin.S40*gaFF20 + Cin.S41*gaFF21 + Cin.S42*gaFF22 + Cin.S43*gaFF32 + Cin.S44*gaFF42; C.S43 -= Cin.S40*gaFF30 + Cin.S41*gaFF31 + Cin.S42*gaFF32 + Cin.S43*gaFF33 + Cin.S44*gaFF43; C.S44 -= Cin.S40*gaFF40 + Cin.S41*gaFF41 + Cin.S42*gaFF42 + Cin.S43*gaFF43 + Cin.S44*gaFF44; #elif defined(SQUARE_ROOT1) // Potter with triangular // a = 1/(F'*F + V) const dense FF = F0*F0 + F1*F1; const dense a = select( mask, 1/(FF + V), Zero); // K = a*S*F const dense aF0 = a*F0; const dense aF1 = a*F1; const dense K0 = C.S00*aF0; const dense K1 = C.S10*aF0 + C.S11*aF1; const dense K2 = C.S20*aF0 + C.S21*aF1; const dense K3 = C.S30*aF0 + C.S31*aF1; const dense K4 = C.S40*aF0 + C.S41*aF1; T[0] -= K0*zeta; T[1] -= K1*zeta; T[2] -= K2*zeta; T[3] -= K3*zeta; T[4] -= K4*zeta; // ts.Chi2 += zeta * a * zeta ; dense chi2 = zeta * a * zeta; ts.NDF += 1; #if 0 // only for square matrix S, not triangular const dense gamma = 1/(1 + sqrt(a*V)); // S = S - S*a*gamma*F*F' // CHECKME why this is correct for triangular matrix? const dense gaFF00 = gamma*aF0*F0; const dense gaFF10 = gamma*aF1*F0; const dense gaFF11 = gamma*aF1*F1; C.S00 -= C.S00*gaFF00; const dense S10 = C.S10; C.S10 -= C.S10*gaFF00 + C.S11*gaFF10; C.S11 -= S10*gaFF10 + C.S11*gaFF11; const dense S20 = C.S20; C.S20 -= C.S20*gaFF00 + C.S21*gaFF10; C.S21 -= S20*gaFF10 + C.S21*gaFF11; const dense S30 = C.S30; C.S30 -= C.S30*gaFF00 + C.S31*gaFF10; C.S31 -= S30*gaFF10 + C.S31*gaFF11; const dense S40 = C.S40; C.S40 -= C.S40*gaFF00 + C.S41*gaFF10; C.S41 -= S40*gaFF10 + C.S41*gaFF11; #else // G = I - a*F*F' CovVConventional G; G.C00 = One - aF0*F0; G.C10 = - aF1*F0; G.C20 = Zero; G.C30 = Zero; G.C40 = Zero; G.C11 = One - aF1*F1; G.C21 = Zero; G.C31 = Zero; G.C41 = Zero; G.C22 = One; G.C32 = Zero; G.C42 = Zero; G.C33 = One; G.C43 = Zero; G.C44 = One; // S -> S*sqrt(G) C *= CovV(G); #endif // 0 #else // SR2 // solve [ (V+F'F)^1/2' (V+F'F)^1/2*K' ] = [ W ] = T * [ V'^1/2 0 ] // [ 0 S_new' ] [ F S'] // A = [ V'^1/2 0 ] // [ F S'] // ? 0 0 0 0 0 // ? ? ? ? ? ? // A = ? 0 ? ? ? ? // 0 0 0 ? ? ? // 0 0 0 0 ? ? // 0 0 0 0 0 ? dense A00 = fill(info.sigma2[iS], Zero.length());//V; dense A01 = Zero; dense A02 = Zero; dense A03 = Zero; dense A04 = Zero; dense A05 = Zero; dense A10 = F0;//select(mask, F0, Zero); dense A20 = F1;//select(mask, F1, Zero); dense A30 = Zero; dense A40 = Zero; dense A50 = Zero; dense A11 = C.S00; dense A12 = C.S10; dense A13 = C.S20; dense A14 = C.S30; dense A15 = C.S40; dense A21 = Zero; dense A22 = C.S11; dense A23 = C.S21; dense A24 = C.S31; dense A25 = C.S41; dense A31 = Zero; dense A32 = Zero; dense A33 = C.S22; dense A34 = C.S32; dense A35 = C.S42; dense A41 = Zero; dense A42 = Zero; dense A43 = Zero; dense A44 = C.S33; dense A45 = C.S43; dense A51 = Zero; dense A52 = Zero; dense A53 = Zero; dense A54 = Zero; dense A55 = C.S44; dense W01; dense W02; dense W03; dense W04; dense W05; dense& W11 = C.S00; dense& W12 = C.S10; dense& W22 = C.S11; dense& W13 = C.S20; dense& W23 = C.S21; dense& W33 = C.S22; dense& W14 = C.S30; dense& W24 = C.S31; dense& W34 = C.S32; dense& W44 = C.S33; dense& W15 = C.S40; dense& W25 = C.S41; dense& W35 = C.S42; dense& W45 = C.S43; dense& W55 = C.S44; #define S_DEFINE( k ) \ const dense s##k = sqrt( A0##k*A0##k + A1##k*A1##k + A2##k*A2##k + A3##k*A3##k + A4##k*A4##k + A5##k*A5##k ); \ const dense is##k = 1.f/s##k; #define W_DEFINE( k, j ) W##k##j = is##k*( A0##k*A0##j + A1##k*A1##j + A2##k*A2##j + A3##k*A3##j + A4##k*A4##j + A5##k*A5##j ) #define A_UPDATE( k, j, l ) A##l##j -= is##k * W##k##j * A##l##k #define A_UPDATE_COLUMN( k, j ) \ A_UPDATE( k, j, 0 ); \ A_UPDATE( k, j, 1 ); \ A_UPDATE( k, j, 2 ); \ A_UPDATE( k, j, 3 ); \ A_UPDATE( k, j, 4 ); \ A_UPDATE( k, j, 5 ); S_DEFINE( 0 ); const dense W00 = s0; W_DEFINE( 0, 1 ); W_DEFINE( 0, 2 ); W_DEFINE( 0, 3 ); W_DEFINE( 0, 4 ); W_DEFINE( 0, 5 ); A_UPDATE_COLUMN( 0, 1 ); A_UPDATE_COLUMN( 0, 2 ); A_UPDATE_COLUMN( 0, 3 ); A_UPDATE_COLUMN( 0, 4 ); A_UPDATE_COLUMN( 0, 5 ); S_DEFINE( 1 ); W11 = s1; W_DEFINE( 1, 2 ); W_DEFINE( 1, 3 ); W_DEFINE( 1, 4 ); W_DEFINE( 1, 5 ); A_UPDATE_COLUMN( 1, 2 ); A_UPDATE_COLUMN( 1, 3 ); A_UPDATE_COLUMN( 1, 4 ); A_UPDATE_COLUMN( 1, 5 ); S_DEFINE( 2 ); W22 = s2; W_DEFINE( 2, 3 ); W_DEFINE( 2, 4 ); W_DEFINE( 2, 5 ); A_UPDATE_COLUMN( 2, 3 ); A_UPDATE_COLUMN( 2, 4 ); A_UPDATE_COLUMN( 2, 5 ); S_DEFINE( 3 ); W33 = s3; W_DEFINE( 3, 4 ); W_DEFINE( 3, 5 ); A_UPDATE_COLUMN( 3, 4 ); A_UPDATE_COLUMN( 3, 5 ); S_DEFINE( 4 ); W44 = s4; W_DEFINE( 4, 5 ); A_UPDATE_COLUMN( 4, 5 ); S_DEFINE( 5 ); W55 = s5; #undef A_UPDATE_COLUMN #undef A_UPDATE #undef W_DEFINE #undef S_DEFINE // Get state vector update const dense zetaA = zeta/W00; T[0] -= W01*zetaA; T[1] -= W02*zetaA; T[2] -= W03*zetaA; T[3] -= W04*zetaA; T[4] -= W05*zetaA; // track.Chi2 += zetaA * zetaA; const dense chi2 = zetaA * zetaA; ts.NDF += 1; #endif // IMPL1 } template void FitSR::ExtrapolateWithMaterial( FitResults& results, const W &zOut, dense& qp0, const FieldRegionCt &F, StationsCt &ss, usize iStation, bool isPipe, dense w ) const { dense* T = results.VecT; CovV& C = results.VecC; const dense initialised = (0 >= w); Jacobian_t j; ExtrapolateJ( T, zOut, qp0, F, j, w ); dense Q22, Q32, Q33; if (isPipe) GetMSMatrix( T[2], T[3], ss.RadThick[iStation] + PipeRadThick, log( ss.RadThick[iStation] + PipeRadThick ), qp0, Q22, Q32, Q33 ); else GetMSMatrix( T[2], T[3], ss.RadThick[iStation], ss.logRadThick[iStation], qp0, Q22, Q32, Q33 ); { // update covariance matrix // solve [ S_new' ] = T * [ S'*J' ] // [ 0 ] [sqrt(Q)' ] // A = [ S' ] // [sqrt(Q)'] // ? ? ? ? ? // 0 ? ? ? ? // 0 0 ? ? ? // 0 0 0 ? ? // A = 0 0 0 0 ? // 0 0 0 0 0 // 0 0 0 0 0 // 0 0 ? ? 0 // 0 0 0 ? 0 // 0 0 0 0 0 const dense Zero = fill(0.f, Q22.length()); #ifdef SQUARE_ROOT1S // Potter with square matrix dense A00 = C.S00 + C.S20*j(0,2) + C.S30*j(0,3) + C.S40*j(0,4); dense A01 = C.S10 + C.S20*j(1,2) + C.S30*j(1,3) + C.S40*j(1,4); dense A02 = C.S20*j(2,2) + C.S30*j(2,3) + C.S40*j(2,4); dense A03 = C.S20*j(3,2) + C.S30*j(3,3) + C.S40*j(3,4); dense A04 = C.S40; dense A10 = C.S01 + C.S21*j(0,2) + C.S31*j(0,3) + C.S41*j(0,4); dense A11 = C.S11 + C.S21*j(1,2) + C.S31*j(1,3) + C.S41*j(1,4); dense A12 = C.S21*j(2,2) + C.S31*j(2,3) + C.S41*j(2,4); dense A13 = C.S21*j(3,2) + C.S31*j(3,3) + C.S41*j(3,4); dense A14 = C.S41; dense A20 = C.S02 + C.S22*j(0,2) + C.S32*j(0,3) + C.S42*j(0,4); dense A21 = C.S12 + C.S22*j(1,2) + C.S32*j(1,3) + C.S42*j(1,4); dense A22 = C.S22*j(2,2) + C.S32*j(2,3) + C.S42*j(2,4); dense A23 = C.S22*j(3,2) + C.S32*j(3,3) + C.S42*j(3,4); dense A24 = C.S42; dense A30 = C.S03 + C.S23*j(0,2) + C.S33*j(0,3) + C.S43*j(0,4); dense A31 = C.S13 + C.S23*j(1,2) + C.S33*j(1,3) + C.S43*j(1,4); dense A32 = C.S23*j(2,2) + C.S33*j(2,3) + C.S43*j(2,4); dense A33 = C.S23*j(3,2) + C.S33*j(3,3) + C.S43*j(3,4); dense A34 = C.S43; dense A40 = C.S04 + C.S24*j(0,2) + C.S34*j(0,3) + C.S44*j(0,4); dense A41 = C.S14 + C.S24*j(1,2) + C.S34*j(1,3) + C.S44*j(1,4); dense A42 = C.S24*j(2,2) + C.S34*j(2,3) + C.S44*j(2,4); dense A43 = C.S24*j(3,2) + C.S34*j(3,3) + C.S44*j(3,4); dense A44 = C.S44; #else // triangular dense A00 = C.S00 + C.S20*j(0,2) + C.S30*j(0,3) + C.S40*j(0,4); dense A01 = C.S10 + C.S20*j(1,2) + C.S30*j(1,3) + C.S40*j(1,4); dense A02 = C.S20*j(2,2) + C.S30*j(2,3) + C.S40*j(2,4); dense A03 = C.S20*j(3,2) + C.S30*j(3,3) + C.S40*j(3,4); dense A04 = C.S40; dense A10 = C.S21*j(0,2) + C.S31*j(0,3) + C.S41*j(0,4); dense A11 = C.S11 + C.S21*j(1,2) + C.S31*j(1,3) + C.S41*j(1,4); dense A12 = C.S21*j(2,2) + C.S31*j(2,3) + C.S41*j(2,4); dense A13 = C.S21*j(3,2) + C.S31*j(3,3) + C.S41*j(3,4); dense A14 = C.S41; dense A20 = C.S22*j(0,2) + C.S32*j(0,3) + C.S42*j(0,4); dense A21 = C.S22*j(1,2) + C.S32*j(1,3) + C.S42*j(1,4); dense A22 = C.S22*j(2,2) + C.S32*j(2,3) + C.S42*j(2,4); dense A23 = C.S22*j(3,2) + C.S32*j(3,3) + C.S42*j(3,4); dense A24 = C.S42; dense A30 = C.S33*j(0,3) + C.S43*j(0,4); dense A31 = C.S33*j(1,3) + C.S43*j(1,4); dense A32 = C.S33*j(2,3) + C.S43*j(2,4); dense A33 = C.S33*j(3,3) + C.S43*j(3,4); dense A34 = C.S43; dense A40 = C.S44*j(0,4); dense A41 = C.S44*j(1,4); dense A42 = C.S44*j(2,4); dense A43 = C.S44*j(3,4); dense A44 = C.S44; #endif // SQUARE_ROOT1S dense A70 = Zero; dense A71 = Zero; dense A72 = sqrt( Q22 ); // sQ22; // sQ = sqrt(Q)' dense A73 = Q32 / A72; // sQ32; dense A74 = Zero; dense A80 = Zero; dense A81 = Zero; dense A82 = Zero; dense A83 = sqrt( Q33 - A73*A73 ); // sQ33; dense A84 = Zero; #define S_DEFINE( k ) \ const dense s##k = sqrt( A0##k*A0##k + A1##k*A1##k + A2##k*A2##k + A3##k*A3##k + A4##k*A4##k + A7##k*A7##k + A8##k*A8##k ); \ const dense is##k = 1.f/s##k; #define W_DEFINE( k, j ) \ C.S##j##k = is##k*( A0##k*A0##j + A1##k*A1##j + A2##k*A2##j + A3##k*A3##j + A4##k*A4##j + A7##k*A7##j + A8##k*A8##j ) // W##k##j = C.S##j##k #define A_UPDATE( k, j, l ) A##l##j -= is##k * C.S##j##k * A##l##k #define A_UPDATE_COLUMN( k, j ) \ A_UPDATE( k, j, 0 ); \ A_UPDATE( k, j, 1 ); \ A_UPDATE( k, j, 2 ); \ A_UPDATE( k, j, 3 ); \ A_UPDATE( k, j, 4 ); \ A_UPDATE( k, j, 7 ); \ A_UPDATE( k, j, 8 ); S_DEFINE( 0 ); C.S00 = s0; W_DEFINE( 0, 1 ); W_DEFINE( 0, 2 ); W_DEFINE( 0, 3 ); W_DEFINE( 0, 4 ); A_UPDATE_COLUMN( 0, 1 ); A_UPDATE_COLUMN( 0, 2 ); A_UPDATE_COLUMN( 0, 3 ); A_UPDATE_COLUMN( 0, 4 ); S_DEFINE( 1 ); C.S11 = s1; W_DEFINE( 1, 2 ); W_DEFINE( 1, 3 ); W_DEFINE( 1, 4 ); A_UPDATE_COLUMN( 1, 2 ); A_UPDATE_COLUMN( 1, 3 ); A_UPDATE_COLUMN( 1, 4 ); S_DEFINE( 2 ); C.S22 = s2; W_DEFINE( 2, 3 ); W_DEFINE( 2, 4 ); A_UPDATE_COLUMN( 2, 3 ); A_UPDATE_COLUMN( 2, 4 ); S_DEFINE( 3 ); C.S33 = s3; W_DEFINE( 3, 4 ); A_UPDATE_COLUMN( 3, 4 ); S_DEFINE( 4 ); C.S44 = s4; #ifdef SQUARE_ROOT1S // Potter with square matrix C.S01 = C.S02 = C.S12 = C.S03 = C.S13 = C.S23 = C.S04 = C.S14 = C.S24 = C.S34 = Zero; #endif // SQUARE_ROOT1S #undef A_UPDATE_COLUMN #undef A_UPDATE #undef W_DEFINE #undef S_DEFINE } } #endif