lcsim/src/org/lcsim/contrib/FermiLab/Cam/CamKalman
diff -N Test2.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ Test2.java 28 Aug 2007 19:40:59 -0000 1.1
@@ -0,0 +1,274 @@
+import java.util.*;
+import org.lcsim.util.aida.*;
+import Jama.*;
+import Jama.util.*;
+import org.lcsim.geometry.Detector;
+import org.lcsim.util.step.*;
+import org.lcsim.contrib.JanStrube.tracking.HelixSwimmer;
+import org.lcsim.spacegeom.CartesianPoint;
+import org.lcsim.spacegeom.CartesianVector;
+import org.lcsim.spacegeom.SpaceVector;
+import org.lcsim.spacegeom.SpacePoint;
+
+/*new modifications as of 06/29/07
+ * by Caroline Milstene and Kudzanayi Munetsi-Mugomba
+ * Test 1 will Test the CamKalmanStepper program
+ * xe, ye, ze,
+ * ntrylim represent the number of tracks
+ *
+*/
+
+public class Test2{
+
+ private static double [] sigma={5e-1,5e-1,5e-1,1e-4,1e-4,1e-4};
+ private static int npoints;
+ private static double ncount=0;
+ private static double ntry;
+ private static double [] tracksm = new double [8];
+ private static int npointslim=60;// hadron calorimeter
+ // private static int npointslim=5; // Silicon Tracker
+ private static int klim=4;
+ private static int ntrylim =20;
+ private static double[][] cov = new double[6][6];
+ private static double[][] mcov = new double[3][3];
+ protected static double [] rp0=new double[8];
+ public static AIDA aida = AIDA.defaultInstance();
+ private static double []mes = new double[3];
+ private static double B=5;
+ private static double sign=1.;
+ private static double kQ=+1;
+
+ public static void main(String args[]){
+ double track [] = new double [6];
+ track[0]=1.5; track[1]=0.5; track[2]=0.02; track[3]=0.5; track[4]=0.; track[5]=0.1;
+ double mMass=0.106;
+ rp0[6]=1.; rp0[7]=mMass;
+ for (int i=0; i<6; i++)
+ {
+ rp0[i]= track[i];
+ }
+ Test2(rp0,ntrylim, npointslim);
+ }
+
+ public static void Test2(double [] vect8,int ntrylim,int npointslim){
+
+ double k1B2C= 0.299792458*1E-03;
+ double xx, yy, zz;
+ double[] vectSix=new double[6];
+ boolean randnum=true;
+ double []se= new double[1000];
+ double []xe = new double[1000]; double []ye = new double[1000]; double []ze = new double[1000];
+ double[]x=new double[1000]; double[]y=new double[1000]; double[]z=new double[1000];
+
+ double[]x_tr=new double[1000]; double[]y_tr=new double[1000]; double[]z_tr=new double[1000];
+ double[]px_tr=new double[1000];double[]py_tr=new double[1000];double[]pz_tr=new double[1000];
+ for(int i=0;i<6;i++) cov[i][i]=50;
+ double pxhlx=vect8[3], pyhlx=vect8[4],pzhlx=vect8[5];
+ double xhlx=vect8[0],yhlx=vect8[1],zhlx=vect8[2];
+ double pxKalm=vect8[3], pyKalm=vect8[4],pzKalm=vect8[5];
+ double xKalm=vect8[0],yKalm=vect8[1],zKalm=vect8[2];
+ SpaceVector p = new CartesianVector(pxhlx,pyhlx,pzhlx);
+ SpacePoint r0 = new CartesianPoint(xhlx, yhlx, zhlx);
+ // measurement point created by an Helix swimmer
+ HelixSwimmer hswim=new HelixSwimmer(B);
+ int q=(int)kQ;
+ hswim.setTrack( p, r0, q);
+ se[0]=Math.sqrt(pxhlx*pxhlx+pyhlx*pyhlx+pzhlx*pzhlx);
+ r0= hswim.getPointAtLength(se[0]);
+ // 1st point on track
+ xe[0]=r0.x();ye[0]=r0.y();ze[0]=r0.z();
+ // measurement point created by the Kalman stepper
+ x_tr[0]=xKalm; y_tr[0]=yKalm; z_tr[0]=zKalm;
+ px_tr[0]=pxKalm; py_tr[0]=pyKalm; pz_tr[0]=pzKalm;
+ double []vectEight=new double[8];
+ vectEight[0]=xKalm; vectEight[1]=yKalm;vectEight[2]=zKalm;
+ vectEight[3]=pxKalm; vectEight[4]=pyKalm;vectEight[5]=pzKalm;
+ vectEight[6]=kQ; vectEight[7]=0.105;
+ CamKalmanStepper kalmStpr = new CamKalmanStepper(vectEight, cov);
+ kalmStpr.setSign(sign);
+ aida.cloud2D("Helix x versus y").fill(r0.x(), r0.y());
+ aida.cloud2D("Helix x versus z").fill(r0.x(), r0.z());
+ aida.cloud2D("Kalman Transport x versus y").fill(x_tr[0], y_tr[0]);
+ aida.cloud2D("Kalman Transport x versus z").fill(x_tr[0], z_tr[0]);
+
+ aida.cloud2D("Kalman Transport all the steps x versus y").fill(x_tr[0], y_tr[1]);
+ aida.cloud2D("Kalman Transport all the steps x versus z").fill(x_tr[0], z_tr[2]);
+ System.out.println(" Kalman/Helix 1st point :x="+vect8[0]+" y="+vect8[1]+" z="+vect8[2]);
+
+ aida.cloud2D("Px vs. Py - Kalman -Fe").fill(vect8[3], vect8[4]);
+ aida.cloud2D("tracksm[3] vs. tracksm[4]").fill(tracksm[3], tracksm[4]);
+
+ double ds=20.;
+//
+// Helix- Create measured points on track
+//
+ for(int j=1; j<npointslim; j++){
+ // swimmer
+ se[j]=se[(j-1)]+ds;
+ //spacePoints
+ r0= hswim.getPointAtLength(se[j]);
+ // 2nd to npoints on track
+ xe[j]=r0.x();ye[j]=r0.y();ze[j]=r0.z();
+ aida.cloud2D("Helix x versus y").fill(r0.x(), r0.y());
+ aida.cloud2D("Helix x versus z").fill(r0.x(), r0.z());
+ }
+ //
+ // Kalman transport
+ //
+ ds=20.;
+ for(int j=1; j<npointslim; j++){
+ //stepper -make klim steps to cover ds=100mm
+ for(int k=1;k<klim;k++){ // 10 steps
+ double ds2=ds/klim;
+ kalmStpr.stepByDs(ds2);
+ vectEight=kalmStpr.getNewRp(); // get x,y,z,px,py,pz, q,mass
+ x_tr[k+(j-1)*klim]=vectEight[0];y_tr[k+(j-1)*klim]=vectEight[1];z_tr[k+(j-1)*klim]=vectEight[2];
+ px_tr[k+(j-1)*klim]=vectEight[3]; py_tr[k+(j-1)*klim]=vectEight[4]; pz_tr[k+(j-1)*klim]=vectEight[5];
+ if(kalmStpr.getStopTkELow()){
+ break;
+ }
+ aida.cloud2D("Kalman Transport all the steps x versus y").fill(vectEight[0], vectEight[1]);
+ aida.cloud2D("Kalman Transport all the steps x versus z").fill(vectEight[0], vectEight[2]);
+
+ aida.cloud2D("Kalman Transport all the steps px versus py").fill(vectEight[3], vectEight[4]);
+ aida.cloud2D("Kalman Transport all the steps px versus pz").fill(vectEight[3], vectEight[5]);
+ }
+ if(kalmStpr.getStopTkELow())break;
+ aida.cloud2D("Kalman Transport x versus y").fill(vectEight[0], vectEight[1]);
+ aida.cloud2D("Kalman Transport x versus z").fill(vectEight[0], vectEight[2]);
+ }
+ // Kalman transport + update
+ // re-do Kalman level 1 before updating starting at point 0
+ // with starting momenta- then update
+ for(int i=0;i<6;i++) cov[i][i]=50;
+ vectEight=new double[8];
+ // First -IP-
+ vectEight[0]=xKalm; vectEight[1]=yKalm; vectEight[2]=zKalm;
+ vectEight[3]=pxKalm; vectEight[4]=pyKalm;vectEight[5]=pzKalm;
+ vectEight[6]=1.;vectEight[7]=0.105;
+ kalmStpr.setSign(1);
+ kalmStpr = new CamKalmanStepper(vectEight, cov);
+ // CamKalmanStepper kalmStpr2 = new CamKalmanStepper(vectEight, cov);
+ kalmStpr.resetStopTkELow();
+ //update with measured points
+ System.out.println("PRINT updating");
+ for(int k=0;k<3;k++) mcov[k][k]=1.e-03;
+ mes[0]= xe[0]; mes[1]= ye[0]; mes[2]= ze[0];
+ kalmStpr.upDate(mes, mcov);
+ vectEight=kalmStpr.getNewRp(); // get x,y,z,px,py,pz,q,mass
+ aida.cloud2D("kalmStpr after update all the steps x vs. y").fill(vectEight[0], vectEight[1]);
+ aida.cloud2D("kalmStpr after update all the steps x vs. z").fill(vectEight[0], vectEight[2]);
+ aida.cloud2D("Kalman x versus y after update").fill(vectEight[0], vectEight[1]);
+ aida.cloud2D("Kalman x versus after update z").fill(vectEight[0], vectEight[2]);
+//
+ for(int j=1; j<npointslim; j++){
+ //stepper -make 4 steps to cover ds=20mm
+ for(int k=1;k<klim;k++){ // 4 steps
+ double ds2=ds/klim;
+ kalmStpr.stepByDs(ds2);
+ vectEight=kalmStpr.getNewRp();
+ if(kalmStpr.getStopTkELow()) break;
+ aida.cloud2D("kalmStpr after update all the steps x vs. y").fill(vectEight[0], vectEight[1]);
+ aida.cloud2D("kalmStpr after update all the steps x vs. z").fill(vectEight[0], vectEight[2]);
+//
+ }
+ if(kalmStpr.getStopTkELow())break;
+ //update with measured points
+ System.out.println("PRINT updating");
+ mes[0]= xe[j]; mes[1]= ye[j]; mes[2]= ze[j];
+ kalmStpr.upDate(mes, mcov);
+ vectEight=kalmStpr.getNewRp(); // get x,y,z,px,py,pz,q,mass
+ aida.cloud2D("Kalman x versus y after update").fill(vectEight[0], vectEight[1]);
+ aida.cloud2D("Kalman x versus after update z").fill(vectEight[0], vectEight[2]);
+ }
+//========================================================================================================
+ // create ntrylim tracks out of it by smearing
+
+ if(ntrylim==-1)return;
+ for(int ntry=0; ntry<ntrylim; ntry++){
+ // at start of each track (number ntry)
+ kalmStpr.resetStopTkELow();
+ kalmStpr.resetCovariance();
+ for(int i=0;i<6;i++) cov[i][i]=50;
+ npoints=-1; // reset the number of points for each track
+ Random rndm =new Random();
+ randnum=true;
+ // smear the IP-(xyz,px,py,pz) within the precision
+
+ if(randnum == true){
+ tracksm[0]=xKalm+Math.abs(2*rndm.nextDouble()-1.)*0.0001;
+ tracksm[1]=yKalm+2*(rndm.nextDouble()-1.)*0.0001;
+ tracksm[2]=zKalm+(2*rndm.nextDouble()-1.)*0.0001;
+ if(tracksm[0]<0.)System.out.println
+ ( "==========smaller than 0. xKalm="+xKalm+"yKalm="+yKalm+" tracksm[0]="+tracksm[0]);
+ tracksm[3]=pxKalm+(2*rndm.nextDouble()-1.)*0.0001; tracksm[4]=pyKalm+(2*rndm.nextDouble()-1.)*0.0001;
+ tracksm[5]=pzKalm+(2*rndm.nextDouble()-1.)*0.0001;
+ }else{
+ tracksm[0] = xKalm+rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[1]));
+ tracksm[1] = yKalm+rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[1]));
+ tracksm[2] = zKalm+rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[2]));
+ tracksm[3] = pxKalm+rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[3]));
+ tracksm[4] = pyKalm+rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[4]));
+ tracksm[5] = pzKalm+rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[5]));
+ }
+ tracksm[6]=1.; tracksm[7]=0.106;
+ //
+ // measured covariance next track
+ //
+ for(int j=0;j<3;j++) mcov[j][j]=1e-3;
+
+ // smeared Helix
+ for(int k=0;k<npointslim;k++){
+ xx=xe[k]; yy=ye[k]; zz=ze[k];
+ x[k]=xx; y[k]=yy; z[k]=zz;
+ x[k]+= 2*rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[0]));
+ y[k]+= 2*rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[1]));
+ z[k]+= 2*rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[2]));
+ aida.cloud2D("Smeared Helix x versus y").fill(x[k], y[k]);
+ aida.cloud2D("Smeared Helix x versus z").fill(x[k], z[k]);
+ }// end of npointslim
+ //
+ // Transport Matrix
+ //
+ if((ntry==8)|| (ntry==9)){
+ System.out.print(" tracksm[0]="+tracksm[0]+" tracksm[1]="+tracksm[1]+" tracksm[2]="+tracksm[2]);
+ System.out.print(" tracksm[3]="+tracksm[3]+" tracksm[4]="+tracksm[4]+" tracksm[5]="+tracksm[5]);
+ System.out.println(" ntry="+ntry);
+ }
+ // New Instance for that track
+ kalmStpr = new CamKalmanStepper(tracksm, cov);
+ kalmStpr.setSign(sign);
+ ds=20.;
+ int kpoints=-1;
+ System.out.println("npointslim="+ npointslim);
+ for( int i=0; i<npointslim; i++){ //propagate to xe[i], ye[i], ze[i]
+ // if (ncount=0)
+ npoints = i;
+ for(int k=0;k<klim;k++){
+ double ds2=ds/klim;
+ kalmStpr.stepByDs(ds2);
+ tracksm = kalmStpr.getNewRp();
+ if(tracksm[0]<0.)System.out.println(" smaller than 0 tracksm[0]="+tracksm[0]+
+ " i="+i+" k="+k+" ntry="+ntry);
+ aida.cloud2D(" Smeared Kalman all steps x versus y").fill(tracksm[0], tracksm[1]);
+ aida.cloud2D(" Smeared Kalman all steps x versus z").fill(tracksm[0], tracksm[2]);
+ if(kalmStpr.getStopTkELow()==true) {
+ break;
+ }
+ }
+ if(kalmStpr.getStopTkELow()==true) break;
+ aida.cloud2D(" Smeared Kalman x versus y").fill(tracksm[0], tracksm[1]);
+ aida.cloud2D(" Smeared Kalman x versus z").fill(tracksm[0], tracksm[2]);
+
+ //update with measured points
+ kalmStpr.resetCovariance();
+ mes[0]= x[i]; mes[1]= y[i]; mes[2]= z[i];
+ kalmStpr.upDate(mes, mcov);
+ tracksm=kalmStpr.getNewRp();
+ aida.cloud2D("kalmStpr smeared tracks after update x vs. y").fill(tracksm[0], tracksm[1]);
+ aida.cloud2D("kalmStpr smeared tracks after update x vs. z").fill(tracksm[0],tracksm[2]);
+ }// End of npointslim
+ } // End of each track- Each ntry
+ }// End of test2 method
+ }// End of the java/class software
+
\ No newline at end of file