// $Id$ // // File: DTrackFitterKalmanSIMD_ALT1.cc // Created: Tue Mar 29 09:45:14 EDT 2011 // Creator: staylor (on Linux ifarml1 2.6.18-128.el5 x86_64) // #include "DTrackFitterKalmanSIMD_ALT1.h" // Kalman engine for forward tracks. For FDC hits only the position along // the wire is used in the fit kalman_error_t DTrackFitterKalmanSIMD_ALT1::KalmanForward(double fdc_anneal_factor, double cdc_anneal_factor, DMatrix5x1 &S, DMatrix5x5 &C, double &chisq, unsigned int &numdof){ DMatrix1x5 H; // Track projection matrix for cdc hits DMatrix5x1 H_T; // Transpose of track projection matrix for cdc hits DMatrix5x5 J; // State vector Jacobian matrix DMatrix5x5 Q; // Process noise covariance matrix DMatrix5x1 K; // Kalman gain matrix for cdc hits DMatrix5x1 S0,S0_; //State vector DMatrix5x5 Ctest; // Covariance matrix // double Vc=0.2028; // covariance for cdc wires =1.56*1.56/12.; double Vc=0.0507*1.15; // Vectors for cdc wires DVector2 origin,dir,wirepos; double z0w=0.; // origin in z for wire // Set used_in_fit flags to false for fdc and cdc hits unsigned int num_cdc=cdc_updates.size(); unsigned int num_fdc=fdc_updates.size(); for (unsigned int i=0;i999) forward_traj[i].h_id=0; } // Save the starting values for C and S in the deque forward_traj[break_point_step_index].Skk=S; forward_traj[break_point_step_index].Ckk=C; // Initialize chi squared chisq=0; // Initialize number of degrees of freedom numdof=0; double my_cdc_anneal=cdc_anneal_factor*cdc_anneal_factor; double my_fdc_anneal=fdc_anneal_factor*fdc_anneal_factor; double var_fdc_cut=NUM_FDC_SIGMA_CUT*NUM_FDC_SIGMA_CUT; double fdc_chi2cut=my_fdc_anneal*var_fdc_cut; double var_cdc_cut=NUM_CDC_SIGMA_CUT*NUM_CDC_SIGMA_CUT; double cdc_chi2cut=my_cdc_anneal*var_cdc_cut; // Variables for estimating t0 from tracking //mInvVarT0=mT0wires=0.; unsigned int num_fdc_hits=break_point_fdc_index+1; unsigned int max_num_fdc_used_in_fit=num_fdc_hits; unsigned int num_cdc_hits=my_cdchits.size(); unsigned int cdc_index=0; if (num_cdc_hits>0) cdc_index=num_cdc_hits-1; bool more_cdc_measurements=(num_cdc_hits>0); double old_doca2=1e6; if (num_fdc_hits+num_cdc_hitsorigin; dir=my_cdchits[cdc_index]->dir; z0w=my_cdchits[cdc_index]->z0wire; wirepos=origin+(forward_traj[break_point_step_index].z-z0w)*dir; } S0_=(forward_traj[break_point_step_index].S); for (unsigned int k=break_point_step_index+1;k0) _DBG_ << "Broken covariance matrix!" <=Q_OVER_P_MAX){ if (DEBUG_LEVEL>2) { _DBG_ << "Bailing: P = " << 1./fabs(S(state_q_over_p)) << endl; } break_point_fdc_index=(3*num_fdc)/4; return MOMENTUM_OUT_OF_RANGE; } //C=J*(C*J_T)+Q; C=Q.AddSym(C.SandwichMultiply(J)); // Save the current state and covariance matrix in the deque forward_traj[k].Skk=S; forward_traj[k].Ckk=C; // Save the current state of the reference trajectory S0_=S0; // Z position along the trajectory double z=forward_traj[k].z; // Add the hit if (num_fdc_hits>0){ if (forward_traj[k].h_id>0 && forward_traj[k].h_id<1000){ unsigned int id=forward_traj[k].h_id-1; double cosa=my_fdchits[id]->cosa; double sina=my_fdchits[id]->sina; double u=my_fdchits[id]->uwire; double v=my_fdchits[id]->vstrip; // Position and direction from state vector double x=S(state_x); double y=S(state_y); double tx=S(state_tx); double ty=S(state_ty); // Projected position along the wire without doca-dependent corrections double vpred_uncorrected=x*sina+y*cosa; // Projected postion in the plane of the wires transverse to the wires double upred=x*cosa-y*sina; // Direction tangent in the u-z plane double tu=tx*cosa-ty*sina; double alpha=atan(tu); double cosalpha=cos(alpha); double cosalpha2=cosalpha*cosalpha; double sinalpha=sin(alpha); // (signed) distance of closest approach to wire double doca=(upred-u)*cosalpha; // Correction for lorentz effect double nz=my_fdchits[id]->nz; double nr=my_fdchits[id]->nr; double nz_sinalpha_plus_nr_cosalpha=nz*sinalpha+nr*cosalpha; // Variance in coordinate along wire double V=my_fdchits[id]->vvar; // Difference between measurement and projection double tv=tx*sina+ty*cosa; double Mdiff=v-(vpred_uncorrected+doca*(nz_sinalpha_plus_nr_cosalpha -tv*sinalpha )); if (DEBUG_HISTS && fit_type==kTimeBased){ fdc_dy_vs_d->Fill(doca,v-vpred_uncorrected); } // To transform from (x,y) to (u,v), need to do a rotation: // u = x*cosa-y*sina // v = y*cosa+x*sina double temp2=nz_sinalpha_plus_nr_cosalpha -tv*sinalpha ; H_T(state_x)=sina+cosa*cosalpha*temp2; H_T(state_y)=cosa-sina*cosalpha*temp2; double cos2_minus_sin2=cosalpha2-sinalpha*sinalpha; double fac=nz*cos2_minus_sin2-2.*nr*cosalpha*sinalpha; double doca_cosalpha=doca*cosalpha; double temp=doca_cosalpha*fac; H_T(state_tx)=cosa*temp -doca_cosalpha*(tu*sina+tv*cosa*cos2_minus_sin2) ; H_T(state_ty)=-sina*temp -doca_cosalpha*(tu*cosa-tv*sina*cos2_minus_sin2) ; // Matrix transpose H_T -> H H(state_x)=H_T(state_x); H(state_y)=H_T(state_y); H(state_tx)=H_T(state_tx); H(state_ty)=H_T(state_ty); // Check to see if we have multiple hits in the same plane if (forward_traj[k].num_hits>1){ // If we do have multiple hits, then all of the hits within some // validation region are included with weights determined by how // close the hits are to the track projection of the state to the // "hit space". vector Klist; vector Mlist; vector Hlist; vector Vlist; vectorprobs; vectorused_ids; // Deal with the first hit: //double Vtemp=V+H*C*H_T; double Vtemp=V+C.SandwichMultiply(H_T); double InvV=1./Vtemp;; //probability double chi2_hit=Mdiff*Mdiff*InvV; double prob_hit=exp(-0.5*chi2_hit)/sqrt(M_TWO_PI*Vtemp); // Cut out outliers if (chi2_hitstatus==good_hit){ probs.push_back(prob_hit); Vlist.push_back(V); Hlist.push_back(H); Mlist.push_back(Mdiff); Klist.push_back(InvV*(C*H_T)); // Kalman gain used_ids.push_back(id); fdc_updates[id].used_in_fit=true; } // loop over the remaining hits for (unsigned int m=1;mstatus==good_hit){ u=my_fdchits[my_id]->uwire; v=my_fdchits[my_id]->vstrip; // Doca to this wire doca=(upred-u)*cosalpha; // variance for coordinate along the wire V=my_fdchits[my_id]->vvar; // Difference between measurement and projection Mdiff=v-(vpred_uncorrected+doca*(nz_sinalpha_plus_nr_cosalpha -tv*sinalpha )); // Update the terms in H/H_T that depend on the particular hit doca_cosalpha=doca*cosalpha; temp=doca_cosalpha*fac; H_T(state_tx)=cosa*temp -doca_cosalpha*(tu*sina+tv*cosa*cos2_minus_sin2) ; H_T(state_ty)=-sina*temp -doca_cosalpha*(tu*cosa-tv*sina*cos2_minus_sin2) ; // Matrix transpose H_T -> H H(state_tx)=H_T(state_tx); H(state_ty)=H_T(state_ty); // Calculate the kalman gain for this hit //Vtemp=V+H*C*H_T; Vtemp=V+C.SandwichMultiply(H_T); InvV=1./Vtemp; // probability chi2_hit=Mdiff*Mdiff*InvV; prob_hit=exp(-0.5*chi2_hit)/sqrt(M_TWO_PI*Vtemp); // Cut out outliers if(chi2_hithit->wire->layer!=PLANE_TO_SKIP){ DMatrix5x5 sum=I5x5; DMatrix5x5 sum2; for (unsigned int m=0;mhit->wire->layer==PLANE_TO_SKIP) scale=1.; fdc_updates[my_id].S=S; fdc_updates[my_id].C=C; fdc_updates[my_id].tflight =forward_traj[k].t*TIME_UNIT_CONVERSION; fdc_updates[my_id].tdrift =my_fdchits[id]->t-fdc_updates[my_id].tflight-mT0; fdc_updates[my_id].z=forward_traj[k].z; fdc_updates[my_id].B=forward_traj[k].B; fdc_updates[my_id].s=forward_traj[k].s; fdc_updates[my_id].residual=scale*Mlist[m]; fdc_updates[my_id].variance=scale*Vlist[m]; fdc_updates[my_id].doca=doca; // update chi2 chisq+=(probs[m]/prob_tot)*(1.-Hlist[m]*Klist[m])*Mlist[m]*Mlist[m]/Vlist[m]; } // update number of degrees of freedom numdof++; } else{ // Variance for this hit //double Vtemp=V+H*C*H_T; double Vproj=C.SandwichMultiply(H_T); double Vtemp=V+Vproj; double InvV=1./Vtemp; // Check if this hit is an outlier double chi2_hit=Mdiff*Mdiff*InvV; if (chi2_hithit->wire->layer!=PLANE_TO_SKIP){ // Update the state vector S+=Mdiff*K; // Update state vector covariance matrix //C=C-K*(H*C); C=C.SubSym(K*(H*C)); } // Store the "improved" values for the state vector and covariance double scale=1.-H*K; if (my_fdchits[id]->hit->wire->layer==PLANE_TO_SKIP) scale=1.; fdc_updates[id].S=S; fdc_updates[id].C=C; fdc_updates[id].tflight =forward_traj[k].t*TIME_UNIT_CONVERSION; fdc_updates[id].tdrift =my_fdchits[id]->t-fdc_updates[id].tflight-mT0; fdc_updates[id].z=forward_traj[k].z; fdc_updates[id].B=forward_traj[k].B; fdc_updates[id].residual=scale*Mdiff; fdc_updates[id].variance=scale*V; fdc_updates[id].s=forward_traj[k].s; fdc_updates[id].doca=doca; fdc_updates[id].used_in_fit=true; if (my_fdchits[id]->hit->wire->layer!=PLANE_TO_SKIP){ // Update chi2 for this segment chisq+=scale*Mdiff*Mdiff/V; // update number of degrees of freedom numdof++; } if (DEBUG_LEVEL>10){ printf("hit %d p %5.2f t %f dm %5.2f sig %f chi2 %5.2f z %5.2f\n", id,1./S(state_q_over_p),fdc_updates[id].tdrift,Mdiff,sqrt(V),(1.-H*K)*Mdiff*Mdiff/V, forward_traj[k].z); } break_point_fdc_index=id; break_point_step_index=k; } } if (num_fdc_hits>=forward_traj[k].num_hits) num_fdc_hits-=forward_traj[k].num_hits; } } else if (more_cdc_measurements /* && zold_doca2 /* && zstatus==good_hit){ double newz=z; // Get energy loss double dedx=0.; if (CORRECT_FOR_ELOSS){ dedx=GetdEdx(S(state_q_over_p), forward_traj[k].K_rho_Z_over_A, forward_traj[k].rho_Z_over_A, forward_traj[k].LnI); } // track direction variables double tx=S(state_tx); double ty=S(state_ty); double tanl=1./sqrt(tx*tx+ty*ty); double sinl=sin(atan(tanl)); // Wire direction variables double ux=dir.X(); double uy=dir.Y(); // Variables relating wire direction and track direction double my_ux=tx-ux; double my_uy=ty-uy; double denom=my_ux*my_ux+my_uy*my_uy; double dz=0.; // if the path length increment is small relative to the radius // of curvature, use a linear approximation to find dz bool do_brent=false; double step1=mStepSizeZ; double step2=mStepSizeZ; if (k>=2){ step1=-forward_traj[k].z+forward_traj[k_minus_1].z; step2=-forward_traj[k_minus_1].z+forward_traj[k-2].z; } //printf("step1 %f step 2 %f \n",step1,step2); double two_step=step1+step2; if (fabs(qBr2p*S(state_q_over_p) *bfield->GetBz(S(state_x),S(state_y),z) *two_step/sinl)<0.01 && denom>EPS) { double dzw=z-z0w; dz=-((S(state_x)-origin.X()-ux*dzw)*my_ux +(S(state_y)-origin.Y()-uy*dzw)*my_uy)/denom; if (fabs(dz)>two_step || dz<0){ do_brent=true; } else{ newz=z+dz; // Check for exiting the straw if (newz>endplate_z){ newz=endplate_z; dz=endplate_z-z; } // Step the state and covariance through the field if (dz>mStepSizeZ){ double my_z=z; int my_steps=int(dz/mStepSizeZ); double dz2=dz-my_steps*mStepSizeZ; for (int m=0;m=Q_OVER_P_MAX){ if (DEBUG_LEVEL>2) { _DBG_ << "Bailing: P = " << 1./fabs(S(state_q_over_p)) << endl; } break_point_fdc_index=(3*num_fdc)/4; return MOMENTUM_OUT_OF_RANGE; } // Step current state by step size Step(my_z,newz,dedx,S); my_z=newz; } newz=my_z+dz2; // Bail if the momentum has dropped below some minimum if (fabs(S(state_q_over_p))>=Q_OVER_P_MAX){ if (DEBUG_LEVEL>2) { _DBG_ << "Bailing: P = " << 1./fabs(S(state_q_over_p)) << endl; } break_point_fdc_index=(3*num_fdc)/4; return MOMENTUM_OUT_OF_RANGE; } Step(my_z,newz,dedx,S); } else{ // Bail if the momentum has dropped below some minimum if (fabs(S(state_q_over_p))>=Q_OVER_P_MAX){ if (DEBUG_LEVEL>2) { _DBG_ << "Bailing: P = " << 1./fabs(S(state_q_over_p)) << endl; } break_point_fdc_index=(3*num_fdc)/4; return MOMENTUM_OUT_OF_RANGE; } Step(z,newz,dedx,S); } } } else do_brent=true; if (do_brent){ // We have bracketed the minimum doca: use Brent's agorithm if (BrentsAlgorithm(z,-mStepSizeZ,dedx,z0w,origin,dir,S,dz)!=NOERROR){ break_point_fdc_index=(3*num_fdc)/4; return MOMENTUM_OUT_OF_RANGE; } newz=z+dz; if (fabs(dz)>2.*mStepSizeZ-EPS3){ // whoops, looks like we didn't actually bracket the minimum // after all. Swim to make sure we pass the minimum doca. double ztemp=newz; // new wire position wirepos=origin; wirepos+=(ztemp-z0w)*dir; // doca old_doca2=doca2; dx=S(state_x)-wirepos.X(); dy=S(state_y)-wirepos.Y(); doca2=dx*dx+dy*dy; while(doca2mStepSizeZ){ my_dz=(dz>0?1.0:-1.)*mStepSizeZ; num_steps=int(fabs(dz/my_dz)); dz3=dz-num_steps*my_dz; double my_z=z; for (int m=0;mcosstereo; double d=sqrt(dx*dx+dy*dy)*cosstereo; // Track projection double cosstereo2_over_d=cosstereo*cosstereo/d; H_T(state_x)=dx*cosstereo2_over_d; H(state_x)=H_T(state_x); H_T(state_y)=dy*cosstereo2_over_d; H(state_y)=H_T(state_y); //H.Print(); // The next measurement double dm=0.39,tdrift=0.,tcorr=0.; if (fit_type==kTimeBased || USE_PASS1_TIME_MODE){ tdrift=my_cdchits[cdc_index]->tdrift-mT0 -forward_traj[k_minus_1].t*TIME_UNIT_CONVERSION; double B=forward_traj[k_minus_1].B; ComputeCDCDrift(tdrift,B,dm,Vc,tcorr); } // Residual double res=dm-d; // inverse variance including prediction //double InvV1=1./(Vc+H*(C*H_T)); double Vproj=C.SandwichMultiply(H_T); double InvV1=1./(Vc+Vproj); if (InvV1<0.){ if (DEBUG_LEVEL>0) _DBG_ << "Negative variance???" << endl; return NEGATIVE_VARIANCE; } // Check if this hit is an outlier double chi2_hit=res*res*InvV1; if (chi2_hit0 && Ctest(1,1)>0 && Ctest(2,2)>0 && Ctest(3,3)>0 && Ctest(4,4)>0){ if (my_cdchits[cdc_index]->hit->wire->ring!=RING_TO_SKIP){ C=Ctest; } // Flag the place along the reference trajectory with hit id forward_traj[k].h_id=1000+cdc_index; // Update the state vector double res=dm-d; if (my_cdchits[cdc_index]->hit->wire->ring!=RING_TO_SKIP){ S+=res*K; } // Store the "improved" values of the state and covariance matrix double scale=1.-H*K; if (my_cdchits[cdc_index]->hit->wire->ring==RING_TO_SKIP){ scale=1.; } //cdc_updates[cdc_index].S=S; //cdc_updates[cdc_index].C=C; cdc_updates[cdc_index].tflight =forward_traj[k_minus_1].t*TIME_UNIT_CONVERSION; cdc_updates[cdc_index].z=newz; cdc_updates[cdc_index].tdrift=tcorr; cdc_updates[cdc_index].B=forward_traj[k_minus_1].B; cdc_updates[cdc_index].s=forward_traj[k_minus_1].s; cdc_updates[cdc_index].residual=res*scale; cdc_updates[cdc_index].variance=Vc*scale; cdc_updates[cdc_index].doca=dm; cdc_updates[cdc_index].used_in_fit=true; // Update chi2 and number of degrees of freedom for this hit if (my_cdchits[cdc_index]->hit->wire->ring!=RING_TO_SKIP){ chisq+=scale*res*res/Vc; numdof++; } if (DEBUG_LEVEL>10) cout << "Ring " << my_cdchits[cdc_index]->hit->wire->ring << " Straw " << my_cdchits[cdc_index]->hit->wire->straw << " Pred " << d << " Meas " << dm << " Sigma meas " << sqrt(Vc) << " t " << tcorr << " Chi2 " << (1.-H*K)*res*res/Vc << endl; break_point_cdc_index=cdc_index; break_point_step_index=k_minus_1; } } if (num_steps==0){ // Step C back to the z-position on the reference trajectory StepJacobian(newz,z,S0,dedx,J); //C=J*C*J.Transpose(); C=C.SandwichMultiply(J); // Step S to current position on the reference trajectory Step(newz,z,dedx,S); } else{ double my_z=newz; for (int m=0;m0){ cdc_index--; origin=my_cdchits[cdc_index]->origin; z0w=my_cdchits[cdc_index]->z0wire; dir=my_cdchits[cdc_index]->dir; } else more_cdc_measurements=false; // Update the wire position wirepos=origin+(z-z0w)*dir; // new doca dx=S(state_x)-wirepos.X(); dy=S(state_y)-wirepos.Y(); doca2=dx*dx+dy*dy; } old_doca2=doca2; } } // Save final z position z_=forward_traj[forward_traj.size()-1].z; // The following code segment addes a fake point at a well-defined z position // that would correspond to a thin foil target. It should not be turned on // for an extended target. if (ADD_VERTEX_POINT){ double dz_to_target=TARGET_Z-z_; double my_dz=mStepSizeZ*(dz_to_target>0?1.:-1.); int num_steps=int(fabs(dz_to_target/my_dz)); for (int k=0;k0) _DBG_ << "Negative variance???" << endl; return NEGATIVE_VARIANCE; } // Compute KalmanSIMD gain matrix K=InvV1*(C*H_T); // Update the state vector with the target point // "Measurement" is average of expected beam spot size double res=0.1466666667-d; S+=res*K; // Update state vector covariance matrix //C=C-K*(H*C); C=C.SubSym(K*(H*C)); // Update chi2 for this segment chisq+=(1.-H*K)*res*res/Vc; numdof++; } // Check that there were enough hits to make this a valid fit if (numdof<6){ chisq=MAX_CHI2; numdof=0; return INVALID_FIT; } // chisq*=anneal_factor; numdof-=5; // Final positions in x and y for this leg x_=S(state_x); y_=S(state_y); if (DEBUG_LEVEL>1){ cout << "Position after forward filter: " << x_ << ", " << y_ << ", " << z_ <0 && break_point_fdc_index>0){ if (break_point_fdc_index+num_cdc=MIN_HITS_FOR_REFIT){ break_point_fdc_index=new_index; } else{ break_point_fdc_index=MIN_HITS_FOR_REFIT-num_cdc; } } return BREAK_POINT_FOUND; } if (num_cdc==0 && break_point_fdc_index>2){ //_DBG_ << endl; if (break_point_fdc_index5 && break_point_cdc_index>2){ //_DBG_ << endl; unsigned int new_index=num_fdc/2; if (new_index+num_cdc>=MIN_HITS_FOR_REFIT){ break_point_fdc_index=new_index; } else{ break_point_fdc_index=MIN_HITS_FOR_REFIT-num_cdc; } return BREAK_POINT_FOUND; } unsigned int num_good=0; unsigned int num_hits=num_cdc+max_num_fdc_used_in_fit; for (unsigned int j=0;j=MIN_HITS_FOR_REFIT)?new_index:(MIN_HITS_FOR_REFIT-1); } else{ unsigned int new_index=num_fdc/2; if (new_index+num_cdc>=MIN_HITS_FOR_REFIT){ break_point_fdc_index=new_index; } else{ break_point_fdc_index=MIN_HITS_FOR_REFIT-num_cdc; } } return PRUNED_TOO_MANY_HITS; } return FIT_SUCCEEDED; } // Smoothing algorithm for the forward trajectory. Updates the state vector // at each step (going in the reverse direction to the filter) based on the // information from all the steps and outputs the state vector at the // outermost step. jerror_t DTrackFitterKalmanSIMD_ALT1::SmoothForward(void){ if (forward_traj.size()<2) return RESOURCE_UNAVAILABLE; unsigned int max=forward_traj.size()-1; DMatrix5x1 S=(forward_traj[max].Skk); DMatrix5x5 C=(forward_traj[max].Ckk); DMatrix5x5 JT=(forward_traj[max].JT); DMatrix5x1 Ss=S; DMatrix5x5 Cs=C; DMatrix5x5 A,dC; for (unsigned int m=max-1;m>0;m--){ if (forward_traj[m].h_id>0){ if (forward_traj[m].h_id<1000){ unsigned int id=forward_traj[m].h_id-1; A=fdc_updates[id].C*JT*C.InvertSym(); Ss=fdc_updates[id].S+A*(Ss-S); if (!finite(Ss(state_q_over_p))){ if (DEBUG_LEVEL>5) _DBG_ << "Invalid values for smoothed parameters..." << endl; return VALUE_OUT_OF_RANGE; } dC=A*(Cs-C)*A.Transpose(); Cs=fdc_updates[id].C+dC; double cosa=my_fdchits[id]->cosa; double sina=my_fdchits[id]->sina; double u=my_fdchits[id]->uwire; double v=my_fdchits[id]->vstrip; // Position and direction from state vector double x=Ss(state_x); double y=Ss(state_y); double tx=Ss(state_tx); double ty=Ss(state_ty); // Projected position along the wire without doca-dependent corrections double vpred_uncorrected=x*sina+y*cosa; // Projected position in the plane of the wires transverse to the wires double upred=x*cosa-y*sina; // Direction tangent in the u-z plane double tu=tx*cosa-ty*sina; double alpha=atan(tu); double cosalpha=cos(alpha); //double cosalpha2=cosalpha*cosalpha; double sinalpha=sin(alpha); // (signed) distance of closest approach to wire double doca=(upred-u)*cosalpha; // Correction for lorentz effect double nz=my_fdchits[id]->nz; double nr=my_fdchits[id]->nr; double nz_sinalpha_plus_nr_cosalpha=nz*sinalpha+nr*cosalpha; // Difference between measurement and projection double tv=tx*sina+ty*cosa; double resi=v-(vpred_uncorrected+doca*(nz_sinalpha_plus_nr_cosalpha -tv*sinalpha)); // Variance from filter step double V=fdc_updates[id].variance; // Compute projection matrix and find the variance for the residual DMatrix5x1 H_T; double temp2=nz_sinalpha_plus_nr_cosalpha-tv*sinalpha; H_T(state_x)=sina+cosa*cosalpha*temp2; H_T(state_y)=cosa-sina*cosalpha*temp2; double cos2_minus_sin2=cosalpha*cosalpha-sinalpha*sinalpha; double fac=nz*cos2_minus_sin2-2.*nr*cosalpha*sinalpha; double doca_cosalpha=doca*cosalpha; double temp=doca_cosalpha*fac; H_T(state_tx)=cosa*temp -doca_cosalpha*(tu*sina+tv*cosa*cos2_minus_sin2) ; H_T(state_ty)=-sina*temp -doca_cosalpha*(tu*cosa-tv*sina*cos2_minus_sin2) ; if (my_fdchits[id]->hit->wire->layer==PLANE_TO_SKIP){ V+=Cs.SandwichMultiply(H_T); } else{ V-=dC.SandwichMultiply(H_T); } pulls.push_back(pull_t(resi,sqrt(V), fdc_updates[id].s, fdc_updates[id].tdrift, fdc_updates[id].doca, NULL,my_fdchits[id]->hit)); } else{ unsigned int id=forward_traj[m].h_id-1000; A=cdc_updates[id].C*JT*C.InvertSym(); Ss=cdc_updates[id].S+A*(Ss-S); if (!finite(Ss(state_q_over_p))){ if (DEBUG_LEVEL>5) _DBG_ << "Invalid values for smoothed parameters..." << endl; return VALUE_OUT_OF_RANGE; } Cs=cdc_updates[id].C+A*(Cs-C)*A.Transpose(); // Fill in pulls information for cdc hits FillPullsVectorEntry(Ss,Cs,forward_traj[m],my_cdchits[id], cdc_updates[id]); } } else{ A=forward_traj[m].Ckk*JT*C.InvertSym(); Ss=forward_traj[m].Skk+A*(Ss-S); Cs=forward_traj[m].Ckk+A*(Cs-C)*A.Transpose(); } S=forward_traj[m].Skk; C=forward_traj[m].Ckk; JT=forward_traj[m].JT; } return NOERROR; }