lcsim/src/org/lcsim/contrib/FermiLab/Cam/CamKalman
diff -N Test1.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ Test1.java 13 Aug 2007 21:20:09 -0000 1.1
@@ -0,0 +1,264 @@
+package org.lcsim.contrib.FermiLab.Cam;
+import java.util.*;
+import org.lcsim.util.aida.*;
+import Jama.*;
+import Jama.util.*;
+import org.lcsim.geometry.Detector;
+import org.lcsim.contrib.FermiLab.Cam.CamKalman.*;
+import org.lcsim.contrib.FermiLab.Cam.testCamKalm.*;
+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 Test1{
+
+ 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=10;// hadron calorimeter
+ // private static int npointslim=5; // Silicon Tracker
+ private static int klim=4;
+ private static int ntrylim =10;
+ 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]=10.; 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];
+ }
+ Test1(rp0,ntrylim, npointslim);
+ }
+
+ public static void Test1(double [] rppp,int ntrylim,int npointslim){
+
+ double k1B2C= 0.299792458*1E-03;
+ double xx, yy, zz;
+ double[] rpp1=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=rppp[3], pyhlx=rppp[4],pzhlx=rppp[5];
+ double xhlx=rppp[0],yhlx=rppp[1],zhlx=rppp[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]=rppp[0]; y_tr[0]=rppp[1]; z_tr[0]=rppp[2];
+ px_tr[0]=rppp[3]; py_tr[0]=rppp[4]; pz_tr[0]=rppp[5];
+ CamKalmanStepper kalmStpr = new CamKalmanStepper(rppp, 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="+rppp[0]+" y="+rppp[1]+" z="+rppp[2]);
+
+ aida.cloud2D("Px vs. Py - Kalman -Fe").fill(rppp[3], rppp[4]);
+ aida.cloud2D("tracksm[3] vs. tracksm[4]").fill(tracksm[3], tracksm[4]);
+
+ double ds=20.;
+//
+// 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);
+ rpp1=kalmStpr.getParameter(); // get x,y,z,px,py,pz
+ x_tr[k]=rpp1[0];y_tr[k]=rpp1[1];z_tr[k]=rpp1[2];
+ px_tr[k]=rpp1[3]; py_tr[k]=rpp1[4]; pz_tr[k]=rpp1[5];
+ if(kalmStpr.getStopTkELow()){
+ break;
+ }
+ aida.cloud2D("Kalman Transport all the steps x versus y").fill(rpp1[0], rpp1[1]);
+ aida.cloud2D("Kalman Transport all the steps x versus z").fill(rpp1[0], rpp1[2]);
+
+ aida.cloud2D("Kalman Transport all the steps px versus py").fill(rpp1[3], rpp1[4]);
+ aida.cloud2D("Kalman Transport all the steps px versus pz").fill(rpp1[3], rpp1[5]);
+ }
+ if(kalmStpr.getStopTkELow())break;
+ aida.cloud2D("Kalman Transport x versus y").fill(rpp1[0], rpp1[1]);
+ aida.cloud2D("Kalman Transport x versus z").fill(rpp1[0], rpp1[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;
+ double [ ]rpp2= new double[6];
+ double [ ]rpp3=new double[8];
+ rpp3[0]=x_tr[0]; rpp3[1]=y_tr[0]; rpp3[2]=z_tr[0];
+ rpp3[3]=px_tr[0]; rpp3[4]=py_tr[0];rpp3[5]=pz_tr[0];
+ rpp3[6]=1.;rpp3[7]=0.105;
+ kalmStpr.setSign(1);
+ kalmStpr = new CamKalmanStepper(rpp3, cov);
+ // CamKalmanStepper kalmStpr2 = new CamKalmanStepper(rpp3, 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);
+ rpp2=kalmStpr.getParameter(); // get x,y,z,px,py,pz
+ aida.cloud2D("kalmStpr after update all the steps x vs. y").fill(rpp2[0], rpp2[1]);
+ aida.cloud2D("kalmStpr after update all the steps x vs. z").fill(rpp2[0], rpp2[2]);
+ aida.cloud2D("Kalman x versus y after update").fill(rpp2[0], rpp2[1]);
+ aida.cloud2D("Kalman x versus after update z").fill(rpp2[0], rpp2[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);
+ rpp2=kalmStpr.getParameter();
+ if(kalmStpr.getStopTkELow())break;
+ aida.cloud2D("kalmStpr after update all the steps x vs. y").fill(rpp2[0], rpp2[1]);
+ aida.cloud2D("kalmStpr after update all the steps x vs. z").fill(rpp2[0], rpp2[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);
+ rpp2=kalmStpr.getParameter(); // get x,y,z,px,py,pz
+ aida.cloud2D("Kalman x versus y after update").fill(rpp2[0], rpp2[1]);
+ aida.cloud2D("Kalman x versus after update z").fill(rpp2[0], rpp2[2]);
+ }
+//========================================================================================================
+ // create ntrylim tracks out of it by smearing
+
+ if(ntrylim==-1)return;
+ for(int ntry=0; ntry<ntrylim; ntry++){
+ kalmStpr.resetStopTkELow(); // at start of track
+ npoints=0; // 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]=x_tr[0]+(2*rndm.nextDouble()-1.)*0.0001; tracksm[1]=y_tr[0]+(2*rndm.nextDouble()-1.)*0.0001;
+ tracksm[2]=z_tr[0]+(2*rndm.nextDouble()-1.)*0.0001;
+ tracksm[3]=px_tr[0]+(2*rndm.nextDouble()-1.)*0.0001; tracksm[4]=py_tr[0]+(2*rndm.nextDouble()-1.)*0.0001;
+ tracksm[5]=pz_tr[0]+(2*rndm.nextDouble()-1.)*0.0001;
+ }else{
+ tracksm[0]=x_tr[0]+rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[1])); tracksm[1]=y_tr[0]+rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[1]));
+ tracksm[2]=z_tr[0]+rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[2]));
+ tracksm[3]=px_tr[0]+rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[3])); tracksm[4]=py_tr[0]+rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[4]));
+ tracksm[5]=pz_tr[0]+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;
+ for(int j=0; j<6;j++) cov[j][j]=50.;
+ // 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]+=rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[0]));
+ y[k]+=rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[1]));
+ z[k]+=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]);
+ }
+ // Transport Matrix
+ kalmStpr = new CamKalmanStepper(tracksm, cov);
+ kalmStpr.setSign(sign);
+ kalmStpr.resetCovariance();
+ for(int i=0;i<6;i++) cov[i][i]=50;
+ ds=20.;
+ 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++){
+ if(kalmStpr.getStopTkELow()==true) break;
+ double ds2=ds/klim;
+ kalmStpr.stepByDs(ds2);
+ double [] ttt=new double[6];
+ ttt=kalmStpr.getParameter();
+
+ for(int m =0;m<6;m++){
+ tracksm[m]=ttt[m];
+ //+rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[m]));
+ tracksm[6]=1.; tracksm[7]=0.106;
+
+ 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]);
+ }
+ aida.cloud2D(" Smeared Kalman x versus y").fill(tracksm[0], tracksm[1]);
+ aida.cloud2D(" Smeared Kalman x versus z").fill(tracksm[0], tracksm[2]);
+
+ if(kalmStpr.getStopTkELow()==true) break;
+ //update with measured points
+ kalmStpr.resetStopTkELow(); // at start of track
+ kalmStpr.resetCovariance();
+ mes[0]= x[i]; mes[1]= y[i]; mes[2]= z[i];
+ kalmStpr.upDate(mes, mcov);
+ ttt=kalmStpr.getParameter();
+ for(int m =0;m<6;m++){
+ tracksm[m]=ttt[m];
+ tracksm[6]=1.; tracksm[7]=0.106;
+
+ 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]);
+ }
+ }
+ if(kalmStpr.getStopTkELow()==true)break;}
+ kalmStpr.resetStopTkELow(); // at start of track
+ kalmStpr.resetCovariance();
+ }
+ }
+ }
+
+
\ No newline at end of file