#include "L1TrackParFit.h" #define cnst const fvec void L1TrackParFit::Filter( L1UMeasurementInfo &info, fvec u, fvec w) { fvec wi, zeta, zetawi, HCH; fvec F0, F1, F2, F3, F4, F5; fvec K1, K2, K3, K4, K5; zeta = info.cos_phi*x + info.sin_phi*y - u; // F = CH' F0 = info.cos_phi*C00 + info.sin_phi*C10; F1 = info.cos_phi*C10 + info.sin_phi*C11; HCH = ( F0*info.cos_phi + F1*info.sin_phi ); F2 = info.cos_phi*C20 + info.sin_phi*C21; F3 = info.cos_phi*C30 + info.sin_phi*C31; F4 = info.cos_phi*C40 + info.sin_phi*C41; F5 = info.cos_phi*C50 + info.sin_phi*C51; #if 0 // use mask const fvec mask = (HCH < info.sigma2 * 16.); wi = w/( (mask & info.sigma2) +HCH ); zetawi = zeta *wi; chi2 += mask & (zeta * zetawi); #else wi = w/( info.sigma2 + HCH ); zetawi = zeta *wi; chi2 += zeta * zetawi; #endif // 0 NDF += w; K1 = F1*wi; K2 = F2*wi; K3 = F3*wi; K4 = F4*wi; K5 = F5*wi; x -= F0*zetawi; y -= F1*zetawi; tx -= F2*zetawi; ty -= F3*zetawi; qp -= F4*zetawi; t -= F5*zetawi; C00-= F0*F0*wi; C10-= K1*F0; C11-= K1*F1; C20-= K2*F0; C21-= K2*F1; C22-= K2*F2; C30-= K3*F0; C31-= K3*F1; C32-= K3*F2; C33-= K3*F3; C40-= K4*F0; C41-= K4*F1; C42-= K4*F2; C43-= K4*F3; C44-= K4*F4; C50-= K5*F0; C51-= K5*F1; C52-= K5*F2; C53-= K5*F3; C54-= K5*F4; C55-= K5*F5; } void L1TrackParFit::Filter( fvec t0, fvec dt0, fvec w) { fvec wi, zeta, zetawi, HCH; fvec F0, F1, F2, F3, F4, F5; fvec K1, K2, K3, K4, K5; zeta = t - t0; // F = CH' F0 = C50; F1 = C51; HCH = C55; F2 = C52; F3 = C53; F4 = C54; F5 = C55; #if 0 // use mask const fvec mask = (HCH < info.sigma2 * 16.); wi = w/( (mask & info.sigma2) +HCH ); zetawi = zeta *wi; chi2 += mask & (zeta * zetawi); #else wi = w/( dt0*dt0 + HCH ); zetawi = zeta *wi; chi2 += zeta * zetawi; #endif // 0 NDF += w; K1 = F1*wi; K2 = F2*wi; K3 = F3*wi; K4 = F4*wi; K5 = F5*wi; x -= F0*zetawi; y -= F1*zetawi; tx -= F2*zetawi; ty -= F3*zetawi; qp -= F4*zetawi; t -= F5*zetawi; C00-= F0*F0*wi; C10-= K1*F0; C11-= K1*F1; C20-= K2*F0; C21-= K2*F1; C22-= K2*F2; C30-= K3*F0; C31-= K3*F1; C32-= K3*F2; C33-= K3*F3; C40-= K4*F0; C41-= K4*F1; C42-= K4*F2; C43-= K4*F3; C44-= K4*F4; C50-= K5*F0; C51-= K5*F1; C52-= K5*F2; C53-= K5*F3; C54-= K5*F4; C55-= K5*F5; } void L1TrackParFit::Extrapolate // extrapolates track parameters and returns jacobian for extrapolation of CovMatrix ( L1TrackParFit &T, fvec z_out , // extrapolate to this z position fvec qp0 , // use Q/p linearisation at this value const L1FieldRegion &F, fvec *w ) { // // Forth-order Runge-Kutta method for solution of the equation // of motion of a particle with parameter qp = Q /P // in the magnetic field B() // // | x | tx // | y | ty // d/dz = | tx| = ft * ( ty * ( B(3)+tx*b(1) ) - (1+tx**2)*B(2) ) // | ty| ft * (-tx * ( B(3)+ty+b(2) - (1+ty**2)*B(1) ) , // // where ft = c_light*qp*sqrt ( 1 + tx**2 + ty**2 ) . // // In the following for RK step : // // x=x[0], y=x[1], tx=x[3], ty=x[4]. // //======================================================================== // // NIM A395 (1997) 169-184; NIM A426 (1999) 268-282. // // the routine is based on LHC(b) utility code // //======================================================================== cnst ZERO = 0.0, ONE = 1.; cnst c_light = 0.000299792458; const fvec a[4] = {0.0f, 0.5f, 0.5f, 1.0f}; const fvec c[4] = {1.0f/6.0f, 1.0f/3.0f, 1.0f/3.0f, 1.0f/6.0f}; const fvec b[4] = {0.0f, 0.5f, 0.5f, 1.0f}; int step4; fvec k[20],x0[5],x[5],k1[20]; fvec Ax[4],Ay[4],Ax_tx[4],Ay_tx[4],Ax_ty[4],Ay_ty[4], At[4], At_tx[4], At_ty[4]; //---------------------------------------------------------------- fvec qp_in = T.qp; const fvec z_in = T.z; const fvec h = (z_out - T.z); fvec hC = h * c_light; x0[0] = T.x; x0[1] = T.y; x0[2] = T.tx; x0[3] = T.ty; x0[4] = T.t; // // Runge-Kutta step // int step; int i; fvec B[4][3]; for (step = 0; step < 4; ++step) { F.Get( z_in + a[step] * h, B[step] ); } for (step = 0; step < 4; ++step) { for(i=0; i < 5; ++i) { if(step == 0) { x[i] = x0[i]; } else { x[i] = x0[i] + b[step] * k[step*5-5+i]; } } fvec tx = x[2]; fvec ty = x[3]; fvec tx2 = tx * tx; fvec ty2 = ty * ty; fvec txty = tx * ty; fvec tx2ty21= 1.f + tx2 + ty2; // if( tx2ty21 > 1.e4 ) return 1; fvec I_tx2ty21 = 1.f / tx2ty21 * qp0; fvec tx2ty2 = sqrt(tx2ty21 ) ; // fvec I_tx2ty2 = qp0 * hC / tx2ty2 ; unsused ??? tx2ty2 *= hC; fvec tx2ty2qp = tx2ty2 * qp0; Ax[step] = ( txty*B[step][0] + ty*B[step][2] - ( 1.f + tx2 )*B[step][1] ) * tx2ty2; Ay[step] = (-txty*B[step][1] - tx*B[step][2] + ( 1.f + ty2 )*B[step][0] ) * tx2ty2; Ax_tx[step] = Ax[step]*tx*I_tx2ty21 + ( ty*B[step][0]-2.f*tx*B[step][1])*tx2ty2qp; Ax_ty[step] = Ax[step]*ty*I_tx2ty21 + ( tx*B[step][0]+ B[step][2])*tx2ty2qp; Ay_tx[step] = Ay[step]*tx*I_tx2ty21 + (-ty*B[step][1]- B[step][2])*tx2ty2qp; Ay_ty[step] = Ay[step]*ty*I_tx2ty21 + (-tx*B[step][1]+2.f*ty*B[step][0])*tx2ty2qp; At[step] = tx2ty2/29.9792458f; At_tx[step] = tx/tx2ty2/29.9792458f; At_ty[step] = ty/tx2ty2/29.9792458f; step4 = step * 5; k[step4 ] = tx * h; k[step4+1] = ty * h; k[step4+2] = Ax[step] * qp0; k[step4+3] = Ay[step] * qp0; k[step4+4] = At[step]; } // end of Runge-Kutta steps fvec initialised = ZERO; if(w) //TODO use operator {?:} { const fvec zero = ZERO; initialised = fvec( zero < *w ); } else { const fvec one = ONE; const fvec zero = ZERO; initialised = fvec( zero < one ); } { T.x = ( (x0[0]+c[0]*k[0]+c[1]*k[5+0]+c[2]*k[10+0]+c[3]*k[15+0]) & initialised) + ( (!initialised) & T.x); T.y = ( (x0[1]+c[0]*k[1]+c[1]*k[5+1]+c[2]*k[10+1]+c[3]*k[15+1]) & initialised) + ( (!initialised) & T.y); T.tx = ( (x0[2]+c[0]*k[2]+c[1]*k[5+2]+c[2]*k[10+2]+c[3]*k[15+2]) & initialised) + ( (!initialised) & T.tx); T.ty = ( (x0[3]+c[0]*k[3]+c[1]*k[5+3]+c[2]*k[10+3]+c[3]*k[15+3]) & initialised) + ( (!initialised) & T.ty); T.t = ( (x0[4]+c[0]*k[4]+c[1]*k[5+4]+c[2]*k[10+4]+c[3]*k[15+4]) & initialised) + ( (!initialised) & T.t); T.z = ( z_out & initialised) + ( (!initialised) & T.z); } // // Derivatives dx/dqp // x0[0] = 0.f; x0[1] = 0.f; x0[2] = 0.f; x0[3] = 0.f; x0[4] = 0.f; // // Runge-Kutta step for derivatives dx/dqp for (step = 0; step < 4; ++step) { for(i=0; i < 5; ++i) { if(step == 0) { x[i] = x0[i]; } else { x[i] = x0[i] + b[step] * k1[step*5-5+i]; } } step4 = step * 5; k1[step4 ] = x[2] * h; k1[step4+1] = x[3] * h; k1[step4+2] = Ax[step] + Ax_tx[step] * x[2] + Ax_ty[step] * x[3]; k1[step4+3] = Ay[step] + Ay_tx[step] * x[2] + Ay_ty[step] * x[3]; k1[step4+4] = At[step] + At_tx[step] * x[2] + At_ty[step] * x[3]; } // end of Runge-Kutta steps for derivatives dx/dqp fvec J[36]; for (i = 0; i < 4; ++i ) { J[24+i]=x0[i]+c[0]*k1[i]+c[1]*k1[5+i]+c[2]*k1[10+i]+c[3]*k1[15+i]; } J[28] = 1.; J[29]=x0[4]+c[0]*k1[4]+c[1]*k1[5+4]+c[2]*k1[10+4]+c[3]*k1[15+4]; // // end of derivatives dx/dqp // // Derivatives dx/tx // x0[0] = 0.f; x0[1] = 0.f; x0[2] = 1.f; x0[3] = 0.f; x0[4] = 0.f; // // Runge-Kutta step for derivatives dx/dtx // for (step = 0; step < 4; ++step) { for(i=0; i < 5; ++i) { if(step == 0) { x[i] = x0[i]; } else if ( i!=2 ) { x[i] = x0[i] + b[step] * k1[step*5-5+i]; } } step4 = step * 5; k1[step4 ] = x[2] * h; k1[step4+1] = x[3] * h; // k1[step4+2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3]; k1[step4+3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3]; k1[step4+4] = At_tx[step] * x[2] + At_ty[step] * x[3]; } // end of Runge-Kutta steps for derivatives dx/dtx for (i = 0; i < 4; ++i ) { if(i != 2) { J[12+i]=x0[i]+c[0]*k1[i]+c[1]*k1[5+i]+c[2]*k1[10+i]+c[3]*k1[15+i]; } } // end of derivatives dx/dtx J[14] = 1.f; J[16] = 0.f; J[17]=x0[4]+c[0]*k1[4]+c[1]*k1[5+4]+c[2]*k1[10+4]+c[3]*k1[15+4]; // Derivatives dx/ty // x0[0] = 0.f; x0[1] = 0.f; x0[2] = 0.f; x0[3] = 1.f; x0[4] = 0.f; // // Runge-Kutta step for derivatives dx/dty // for (step = 0; step < 4; ++step) { for(i=0; i < 5; ++i) { if(step == 0) { x[i] = x0[i]; // ty fixed } else if(i!=3) { x[i] = x0[i] + b[step] * k1[step*5-5+i]; } } step4 = step * 5; k1[step4 ] = x[2] * h; k1[step4+1] = x[3] * h; k1[step4+2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3]; // k1[step4+3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3]; k1[step4+4] = At_tx[step] * x[2] + At_ty[step] * x[3]; } // end of Runge-Kutta steps for derivatives dx/dty for (i = 0; i < 3; ++i ) { J[18+i]=x0[i]+c[0]*k1[i]+c[1]*k1[5+i]+c[2]*k1[10+i]+c[3]*k1[15+i]; } // end of derivatives dx/dty J[21] = 1.; J[22] = 0.; J[23]=x0[4]+c[0]*k1[4]+c[1]*k1[5+4]+c[2]*k1[10+4]+c[3]*k1[15+4]; // // derivatives dx/dx and dx/dy for(i = 0; i < 12; ++i) J[i] = 0.; J[0] = 1.; J[7] = 1.; for(i=30; i<35; i++) J[i] = 0.f; J[35] = 1.f; fvec dqp = qp_in - qp0; { // update parameters T.x += ((J[6*4+0]*dqp) & initialised); T.y += ((J[6*4+1]*dqp) & initialised); T.tx +=((J[6*4+2]*dqp) & initialised); T.ty +=((J[6*4+3]*dqp) & initialised); T.t +=((J[6*4+5]*dqp) & initialised); } // covariance matrix transport // // if ( C_in&&C_out ) CbmKFMath::multQtSQ( 5, J, C_in, C_out); // TODO // j(0,2) = J[5*2 + 0]; // j(1,2) = J[5*2 + 1]; // j(2,2) = J[5*2 + 2]; // j(3,2) = J[5*2 + 3]; // // j(0,3) = J[5*3 + 0]; // j(1,3) = J[5*3 + 1]; // j(2,3) = J[5*3 + 2]; // j(3,3) = J[5*3 + 3]; // // j(0,4) = J[5*4 + 0]; // j(1,4) = J[5*4 + 1]; // j(2,4) = J[5*4 + 2]; // j(3,4) = J[5*4 + 3]; const fvec c42 = T.C42, c43 = T.C43; const fvec cj00 = T.C00 + T.C20*J[6*2 + 0] + T.C30*J[6*3 + 0] + T.C40*J[6*4 + 0]; const fvec cj10 = T.C10 + T.C21*J[6*2 + 0] + T.C31*J[6*3 + 0] + T.C41*J[6*4 + 0]; const fvec cj20 = T.C20 + T.C22*J[6*2 + 0] + T.C32*J[6*3 + 0] + c42*J[6*4 + 0]; const fvec cj30 = T.C30 + T.C32*J[6*2 + 0] + T.C33*J[6*3 + 0] + c43*J[6*4 + 0]; const fvec cj40 = T.C40 + T.C42*J[6*2 + 0] + T.C43*J[6*3 + 0] + T.C44*J[6*4 + 0]; const fvec cj50 = T.C50 + T.C52*J[6*2 + 0] + T.C53*J[6*3 + 0] + T.C54*J[6*4 + 0]; const fvec cj01 = T.C10 + T.C20*J[6*2 + 1] + T.C30*J[6*3 + 1] + T.C40*J[6*4 + 1]; const fvec cj11 = T.C11 + T.C21*J[6*2 + 1] + T.C31*J[6*3 + 1] + T.C41*J[6*4 + 1]; const fvec cj21 = T.C21 + T.C22*J[6*2 + 1] + T.C32*J[6*3 + 1] + c42*J[6*4 + 1]; const fvec cj31 = T.C31 + T.C32*J[6*2 + 1] + T.C33*J[6*3 + 1] + c43*J[6*4 + 1]; const fvec cj41 = T.C41 + T.C42*J[6*2 + 1] + T.C43*J[6*3 + 1] + T.C44*J[6*4 + 1]; const fvec cj51 = T.C51 + T.C52*J[6*2 + 1] + T.C53*J[6*3 + 1] + T.C54*J[6*4 + 1]; const fvec cj02 = T.C20*J[6*2 + 2] + T.C30*J[6*3 + 2] + T.C40*J[6*4 + 2]; const fvec cj12 = T.C21*J[6*2 + 2] + T.C31*J[6*3 + 2] + T.C41*J[6*4 + 2]; const fvec cj22 = T.C22*J[6*2 + 2] + T.C32*J[6*3 + 2] + c42*J[6*4 + 2]; const fvec cj32 = T.C32*J[6*2 + 2] + T.C33*J[6*3 + 2] + c43*J[6*4 + 2]; const fvec cj42 = T.C42*J[6*2 + 2] + T.C43*J[6*3 + 2] + T.C44*J[6*4 + 2]; const fvec cj52 = T.C52*J[6*2 + 2] + T.C53*J[6*3 + 2] + T.C54*J[6*4 + 2]; const fvec cj03 = T.C20*J[6*2 + 3] + T.C30*J[6*3 + 3] + T.C40*J[6*4 + 3]; const fvec cj13 = T.C21*J[6*2 + 3] + T.C31*J[6*3 + 3] + T.C41*J[6*4 + 3]; const fvec cj23 = T.C22*J[6*2 + 3] + T.C32*J[6*3 + 3] + c42*J[6*4 + 3]; const fvec cj33 = T.C32*J[6*2 + 3] + T.C33*J[6*3 + 3] + c43*J[6*4 + 3]; const fvec cj43 = T.C42*J[6*2 + 3] + T.C43*J[6*3 + 3] + T.C44*J[6*4 + 3]; const fvec cj53 = T.C52*J[6*2 + 3] + T.C53*J[6*3 + 3] + T.C54*J[6*4 + 3]; const fvec cj24 = T.C42; const fvec cj34 = T.C43; const fvec cj44 = T.C44; const fvec cj54 = T.C54; const fvec cj05 = T.C50 + T.C20*J[17] + T.C30*J[23] + T.C40*J[29]; const fvec cj15 = T.C51 + T.C21*J[17] + T.C31*J[23] + T.C41*J[29]; const fvec cj25 = T.C52 + T.C22*J[17] + T.C32*J[23] + T.C42*J[29]; const fvec cj35 = T.C53 + T.C32*J[17] + T.C33*J[23] + T.C43*J[29]; const fvec cj45 = T.C54 + T.C42*J[17] + T.C43*J[23] + T.C44*J[29]; const fvec cj55 = T.C55 + T.C52*J[17] + T.C53*J[23] + T.C54*J[29]; C00 = ((cj00 + cj20*J[12] + cj30*J[18] + cj40*J[24]) & initialised) + ((!initialised) & C00); C10 = ((cj10 + cj20*J[13] + cj30*J[19] + cj40*J[25]) & initialised) + ((!initialised) & C10); C11 = ((cj11 + cj21*J[13] + cj31*J[19] + cj41*J[25]) & initialised) + ((!initialised) & C11); C20 = ((cj20 + cj30*J[20] + cj40*J[26]) & initialised) + ((!initialised) & C20); C21 = ((cj21 + cj31*J[20] + cj41*J[26]) & initialised) + ((!initialised) & C21); C22 = ((cj22 + cj32*J[20] + cj42*J[26]) & initialised) + ((!initialised) & C22); C30 = ((cj30 + cj20*J[15] + cj40*J[27]) & initialised) + ((!initialised) & C30); C31 = ((cj31 + cj21*J[15] + cj41*J[27]) & initialised) + ((!initialised) & C31); C32 = ((cj32 + cj22*J[15] + cj42*J[27]) & initialised) + ((!initialised) & C32); C33 = ((cj33 + cj23*J[15] + cj43*J[27]) & initialised) + ((!initialised) & C33); C40 = ((cj40) & initialised) + ((!initialised) & C40); C41 = ((cj41) & initialised) + ((!initialised) & C41); C42 = ((cj42) & initialised) + ((!initialised) & C42); C43 = ((cj43) & initialised) + ((!initialised) & C43); C44 = ((cj44) & initialised) + ((!initialised) & C44); C50 = ((cj50 + cj20*J[17] + cj30*J[23] + cj40*J[29]) & initialised) + ((!initialised) & C50); C51 = ((cj51 + cj21*J[17] + cj31*J[23] + cj41*J[29]) & initialised) + ((!initialised) & C51); C52 = ((cj52 + cj22*J[17] + cj32*J[23] + cj42*J[29]) & initialised) + ((!initialised) & C52); C53 = ((cj53 + cj23*J[17] + cj33*J[23] + cj43*J[29]) & initialised) + ((!initialised) & C53); C54 = ((cj54 + cj24*J[17] + cj34*J[23] + cj44*J[29]) & initialised) + ((!initialised) & C54); C55 = ((cj55 + cj25*J[17] + cj35*J[23] + cj45*J[29]) & initialised) + ((!initialised) & C55); } /* inline void L1AddMaterial( L1TrackParFit &T, L1MaterialInfo &info, fvec qp0, fvec w = 1, fvec mass2 = 0.1395679f*0.1395679f ) { cnst ONE = 1.f; fvec tx = T.tx; fvec ty = T.ty; fvec time = T.t; fvec txtx = tx*tx; fvec tyty = ty*ty; fvec txtx1 = txtx + ONE; fvec h = txtx + tyty; fvec t = sqrt(txtx1 + tyty); fvec h2 = h*h; fvec qp0t = qp0*t; cnst c1=0.0136f, c2=c1*0.038f, c3=c2*0.5f, c4=-c3/2.0f, c5=c3/3.0f, c6=-c3/4.0f; fvec s0 = (c1+c2*info.logRadThick + c3*h + h2*(c4 + c5*h +c6*h2) )*qp0t; //fvec a = ( (ONE+mass2*qp0*qp0t)*info.RadThick*s0*s0 ); fvec a = ( (t+mass2*qp0*qp0t)*info.RadThick*s0*s0 ); T.C22 += w*txtx1*a; T.C32 += w*tx*ty*a; T.C33 += w*(ONE+tyty)*a; }*/ #undef cnst