ExampleProject/src
diff -N Test1.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ Test1.java 1 Aug 2007 22:05:46 -0000 1.1
@@ -0,0 +1,269 @@
+import java.util.*;
+import org.lcsim.util.aida.*;
+import Jama.*;
+import Jama.util.*;
+import org.lcsim.geometry.Detector;
+import org.lcsim.util.step.CamKalmanStepper;
+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
+
+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;
+ private static int ntrylim =0;
+ private boolean testSingle=true;
+ // private static Matrix cov = new Matrix(6,6);
+ 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.; debug
+ 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];
+ }
+ Test1(rp0,ntrylim, npointslim);
+ }
+ public static void Test1(double [] rppp,int ntrylim,int npointslim){
+
+ double k1B2C= 0.299792458*1E-03;
+ double curv = k1B2C*B/Math.sqrt((rppp[3]*rppp[3])+(rppp[4]*rppp[4]));
+ 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 x versus y").fill(x_tr[0], y_tr[0]);
+ aida.cloud2D("Kalman x versus z").fill(x_tr[0], z_tr[0]);
+
+ aida.cloud2D("Kalman level 0 all the steps x versus y").fill(x_tr[0], y_tr[1]);
+ aida.cloud2D("Kalman level 0 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=100;
+//
+// 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 level 0
+ ds=100.;
+ for(int j=1; j<npointslim; j++){
+ //stepper -make 10 steps to cover ds=100mm
+ for(int k=1;k<11;k++){ // 10 steps
+ double ds2=ds/10;
+ 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]=rppp[3]; py_tr[k]=rppp[4]; pz_tr[k]=rppp[5];
+ if(kalmStpr.getStopTkELow()){
+ System.out.print("Tk stopped too little energy");
+ break;
+ }
+ aida.cloud2D("Kalman level 0 all the steps x versus y").fill(rpp1[0], rpp1[1]);
+ aida.cloud2D("Kalman level 0 all the steps x versus z").fill(rpp1[0], rpp1[2]);
+
+ aida.cloud2D("Kalman level 0 all the steps px versus py").fill(rpp1[3], rpp1[4]);
+ aida.cloud2D("Kalman level 0 all the steps px versus pz").fill(rpp1[3], rpp1[5]);
+ }
+ if(kalmStpr.getStopTkELow())break;
+ aida.cloud2D("Kalman level 0 x versus y").fill(rpp1[0], rpp1[1]);
+ aida.cloud2D("Kalman level 0 x versus z").fill(rpp1[0], rpp1[2]);
+ }
+ // Full Kalman
+ // re-do Kalman level 0 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);
+ 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 x vs. y").fill(rpp2[0], rpp2[1]);
+ aida.cloud2D("kalmStpr after update x vs. z").fill(rpp2[0], rpp2[2]);
+ for(int j=1; j<npointslim; j++){
+ System.out.println("!!!!!!!!!!!!!!.......................................Matrices......................................!!!!!!!!!!!!!!");
+ System.out.println("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!point#:"+j);
+ //stepper -make 10 steps to cover ds=100mm
+ for(int k=1;k<11;k++){ // 10 steps
+ double ds2=ds/10;
+ kalmStpr.stepByDs(ds2);
+ rpp2=kalmStpr.getParameter();
+ System.out.println(" !!!! rpp2[0,1,2]="+rpp2[0]+" "+rpp2[1]+" "+rpp2[2]);
+
+ System.out.println(" !!!! rpp2[3,4,5]="+rpp2[3]+" "+rpp2[4]+" "+rpp2[5]);
+ //update with measured points
+ System.out.println("PRINT updating");
+ mes[0]= xe[j]; mes[1]= ye[j]; mes[2]= ze[j];
+ System.out.println(" !!!! xe[j]="+xe[j]+" ye[j]="+ye[j]+" ze[j]="+ze[j]);
+ kalmStpr.upDate(mes, mcov);
+ rpp2=kalmStpr.getParameter(); // get x,y,z,px,py,pz
+ if(kalmStpr.getStopTkELow())break;
+ aida.cloud2D("kalmStpr after update x vs. y").fill(rpp2[0], rpp2[1]);
+ aida.cloud2D("kalmStpr after update x vs. z").fill(rpp2[0], rpp2[2]);
+ }
+ if(kalmStpr.getStopTkELow())break;
+ aida.cloud2D("Kalman x versus y").fill(rpp2[0], rpp2[1]);
+ aida.cloud2D("Kalman x versus z").fill(rpp2[0], rpp2[2]);
+ }
+
+ // create ntrylim tracks out of it by smearing
+ //
+ if(ntrylim==0)return;
+ for(int ntry=0; ntry<ntrylim; ntry++){
+ for(int k=0;k<3;k++) mcov[k][k]=1.e-03;
+ kalmStpr.resetStopTkELow(); // at start of track
+ for(int i=0;i<6;i++) cov[i][i]=50;
+ Random rndm =new Random();
+ // smear the IP-(xyz,px,py,pz)
+ /* if(randnum == true){
+ tracksm[0]=xe[0]+(2*rndm.nextDouble()-1.); tracksm[1]=ye[0]+(2*rndm.nextDouble()-1.);
+ tracksm[2]=ze[0]+(2*rndm.nextDouble()-1.);
+ tracksm[3]=px_tr[0]+(2*rndm.nextDouble()-1.)*0.1; tracksm[4]=py_tr[0]+(2*rndm.nextDouble()-1.)*0.1;
+ tracksm[5]=pz_tr[0]+(2*rndm.nextDouble()-1.)*0.1;
+ */// }
+ //else{
+ tracksm[0]=xe[0]+rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[1])); tracksm[1]=ye[0]+rndm.nextGaussian()*Math.abs(Math.sqrt(sigma[1]));
+ tracksm[2]=ze[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;
+ 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;
+ //xe[i]=xx; ye[i]=yy; ze[i]=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]);
+ }
+ // create the interaction point-IP- (x,y,z,px,py,pz)
+ // smear the IP at random
+ // for(int j=0; j<3; j++) tracksm[j] = rppp[j]+rndm.nextDouble();
+ // for(int j=3; j<6; j++) tracksm[j] = rppp[j]+(rndm.nextDouble()*0.1);
+ // if(ncount<5)System.out.println(" !!!! tracksm[0]="+tracksm[0]);
+ // --- New instance of kalmStpr ---
+ kalmStpr = new CamKalmanStepper(tracksm, cov);
+ kalmStpr.setSign(sign);
+ ds=100.;
+ for( int i=1; i<npointslim; i++){ //propagate to xe[i], ye[i], ze[i]
+
+ // if (ncount=0)
+ // System.out.println("ntry i :" +" xe[i]=" +xe[i]+ " ye[i]=" + ye[i]+ " ze[i]=" +ze[i]+ " i="+i);
+ for(int k=1;k<11;k++){
+ if(kalmStpr.getStopTkELow()==true){
+ break;
+ }
+ double ds2=ds/10;
+ 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]));
+ System.out.println("PRINT updating");
+ mes[0]= x[i]; mes[1]= y[i]; mes[2]= z[i];
+ kalmStpr.upDate(mes, mcov);
+ ttt=kalmStpr.getParameter(); // get x,y,z,px,py,pz
+ for(int km =0;km<6;km++) tracksm[km]=ttt[km];
+ tracksm[6]=1.; tracksm[7]=0.106;
+ if(kalmStpr.getStopTkELow()==false)break;
+
+ aida.cloud2D(" After update Smeared Kalman all steps x versus y").fill(tracksm[0], tracksm[1]);
+ aida.cloud2D(" After Update Smeared Kalman all steps x versus z").fill(tracksm[0], tracksm[2]);
+ }
+
+ aida.cloud2D("After Update Smeared Kalman x versus y").fill(tracksm[0], tracksm[1]);
+ aida.cloud2D("After Update Smeared Kalman x versus z").fill(tracksm[0], tracksm[2]);
+ }
+
+ // x_tr[i]=kalmStpr.getX() ; y_tr[i]=kalmStpr.getY() ; z_tr[i]=kalmStpr.getZ();
+ // Px_check[i]=kalmStpr.getPx(); Py_check[i]=kalmStpr.getPy(); Pz_check[i]=kalmStpr.getPz();
+ // aida.cloud2D("x versus y").fill(x[i],y[i]);
+ // aida.cloud2D("xe versus ye").fill(xe[i],ye[i]);
+ // aida.cloud2D("x versus z").fill(xe[i],ze[i]);
+ // aida.cloud2D("Px_check versus Py_check").fill(tracksm[3],tracksm[4]);
+ // aida.cloud2D("kalmStpr after propagate x vs. y").fill(kalmStpr.getX(), kalmStpr.getY());
+ /*
+ //update with measured points
+ //for(int j=0;j<3;j++) mcov[j][j]=1e-3;
+ System.out.println("PRINT updating");
+ mes[0]= x[i]; mes[1]= y[i]; mes[2]= z[i];
+ kalmStpr.upDate(mes, mcov);
+ // mcov.print(3,3);
+ aida.cloud2D("kalmStpr after update x vs. y").fill(kalmStpr.getX(), kalmStpr.getY());
+ aida.cloud2D("kalmStpr after update x vs. z").fill(kalmStpr.getX(), kalmStpr.getZ());
+ if(kalmStpr.getStopTkELow()==false)break;
+ kalmStpr.resetCovariance();
+ */
+ }
+ }
+ }
+}
+
\ No newline at end of file