lcsim/src/org/lcsim/util/step
diff -N CamKalmanStepper.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ CamKalmanStepper.java 21 May 2007 17:09:52 -0000 1.1
@@ -0,0 +1,359 @@
+package org.lcsim.util.step;
+import org.lcsim.geometry.compact.converter.lcdd.util.*;
+import org.lcsim.units.clhep.PhysicalConstants.*;
+import java.util.*;
+import java.text.*;
+import org.lcsim.util.aida.*;
+import Jama.*;
+import Jama.util.*;
+import org.lcsim.recon.muon.CoilSubLayer;
+import org.lcsim.recon.cluster.util.CalHitMapMgr;
+import org.lcsim.geometry.subdetector.CylindricalBarrelCalorimeter;
+import org.lcsim.geometry.Detector;
+import org.lcsim.geometry.subdetector.*;
+import org.lcsim.geometry.field.*;
+import org.lcsim.geometry.layer.*;
+import org.lcsim.geometry.Layered;
+import org.lcsim.geometry.LayeredSubdetector;
+import org.lcsim.material.*;
+ /**
+ * CamKalmanStepper- A Kalman Stepper
+ * trasnsported from JAS2
+ *JINST - published October 2006
+ *arXiv: physics/0604197
+ *@Author-C. Milstene <[log in to unmask]>
+ * Code developped consulting Hans Wenzel
+ * More comments and references to come
+ */
+public class CamKalmanStepper{
+ private final double kAlmost1=0.999;
+ private final double kAlmost0=1E-33;
+ private final double kVeryBig =(1./kAlmost0);
+ private final double kB2C=0.299979245;
+ private final double kAlmost0Field =1.E-33;
+ private final double kMostProbablePt=0.35;
+ private final double cmTomm=10.;
+ private final double mevTogev=1./1000.;
+ private final double gevTomev=1000.;
+ private double [] zRhoZoverA;
+ private double kQ;
+ private double kB;
+ private double kMass;
+ private String MaterialName;
+ private boolean kRungeKutta;
+ private boolean kupDate = false;
+ private static boolean ktransport=true;
+ private static boolean stopTkELow=false;
+ private double [] kRp = new double [6];
+ private double [] kRPlus ;
+ private double [] measState;
+ private double [][]measCov;
+ private double [] nwRp = new double[8];
+ private Matrix PkMinus = new Matrix(6,6);
+ private Matrix PkPlus = new Matrix(6,6) ;
+ private Matrix Hk ;
+ private double[][] kCovariance = new double [6][6];// construct a 6 by 6 array of zeroes
+ private Matrix fi = new Matrix(6,6); // construct
+ private AIDA aida = AIDA.defaultInstance();
+ private CalHitMapMgr evtptr = null;
+ private boolean StopTkElow=false;
+ private Random rndcm = new Random();
+ private Detector det ;
+ /** default constructor*/
+ public CamKalmanStepper(){
+ evtptr = CalHitMapMgr.getInstance();
+ kQ = -1. ;
+ kMass = 0.105;
+ kRungeKutta = true;
+ for(int i=0; i<6; i++)kRp[i]=0.;
+ for(int i=0; i<6; i++){
+ for(int j=0; j<6; j++)kCovariance[i][j]=0.; // or any starting value
+ }
+ }
+ /** Copy Constructor-
+ *@param rp an Array with the particle position and Momnetum
+ * (x,y,z,px,py,pz) - a phase-space point
+ */
+ public CamKalmanStepper(double [] rp){
+ evtptr = CalHitMapMgr.getInstance();
+ kQ = rp[6] ;
+ kMass = rp[7] ;
+ kRungeKutta = true;
+ for(int i=0; i<6; i++)this.kRp[i]=rp[i];
+ for(int i=0; i<6; i++){
+ for(int j=0; j<6; j++)this.kCovariance[i][j]=kCovariance[i][j];
+ }
+ }
+ /** Copy Constructor-
+ *@param rp an array with the particle position and momentum(x,y,z,px,py,pz)
+ *@param cova a covariant matrix
+ */
+ public CamKalmanStepper(double [] parm, double [][]covar){ // Copy constructor
+ evtptr = CalHitMapMgr.getInstance();
+ kQ = parm[6] ;
+ kMass = parm[7] ;
+ kRungeKutta = true;
+ set(parm,covar); // create external track parameters from given arguments
+
+ }
+/** set rp and covariant Matrix
+ *@param rp an array with the particle position and momentum
+ *@param covar the covariant matrix
+ */
+ public void set(double [] rp, double [] [] covar){
+ for(int i=0; i<6; i++)this.kRp[i]=rp[i];
+ for(int i=0; i<6; i++){
+ for(int j=0;j<6;j++){
+ kCovariance[i][j] = covar[i][j];
+ }
+ }
+ }
+ /** Propagate the particle from a position x
+ * to s new position in a magnetic field b
+ *@param x the position
+ *@param b the magnetic field
+ */
+ public boolean propagateTo(Double x, double b){
+ setBField1Dim(b);
+ double dx = x-getX();
+ int nsteps = (int)(Math.abs(dx/0.005)+0.5);
+ for (int i=0;i<nsteps;i++) stepByDx(dx/nsteps);
+ return (ktransport=true);
+}
+ /** stepby ds on the particle pathLength
+ *@param ds one step length
+ */
+ public boolean stepByDs(double ds){ // one step by ds
+ double[]wk=new double[3];
+ double P = Math.sqrt(kRp[3]*kRp[3]+ kRp[4]*kRp[4]+kRp[5]*kRp[5]) ;
+ double E = Math.sqrt(P*P+kMass*kMass) ;
+ double k=kB2C*kQ*kB/P ;
+ double dpdx = getDeDx(P, kMass,"Iron")*E/P*ds ;
+
+ if(dpdx>P) return stopTkELow=true ;
+ double rdL= getRadLength("Iron"); //im mm
+
+ wk=vwk((getRadLength("iron"))*ds);
+ //System.out.println("wk[0]="+wk[0]+" wk[1]="+wk[1]+" wk[2]="+wk[2]);
+ //Next to be checked again
+ double[] [] valsq={{0.,0.,0.,0.,0.,0.},{0.,0.,0.,0.,0.,0.},{0.,0.,0.,0.,0.,0.},
+ {0.,0.,0.,wk[0],0.,0.},{0.,0.,0.,0.,wk[1],0.},{0.,0.,0.,0.,0.,wk[2]}};
+ Matrix Qk = new Matrix(valsq);
+ double[][]fifi=new double[6][6];
+ kRp[0]+=kRp[3]/P*ds ;
+ kRp[1]+=kRp[4]/P*ds ;
+ kRp[2]+=kRp[5]/P*ds ;
+ k*=ds;
+ kRp[3]+=(k/(1.+0.25*k*k)*kRp[4]-0.5*k*k*kRp[3])-dpdx*kRp[3]/P ;
+ kRp[4]+=(-k/(1.+0.25*k*k)*kRp[3]-0.5*k*k*kRp[4])-dpdx*kRp[4]/P ;
+ kRp[5]+=-dpdx*kRp[5]/P;
+ //System.out.println("!!!!!! px="+kRp[3]+" py="+kRp[4]+" pz="+kRp[5]+" P="+P);
+ fifi[0][3] =-kRp[3]*kRp[3]/(P*P*P)+1./P ;
+ fifi[0][4] =-kRp[3]*kRp[4]/(P*P*P) ;
+ fifi[0][5] =-kRp[3]*kRp[5]/(P*P*P) ;
+ fifi[1][3] =-kRp[4]*kRp[3]/(P*P*P) ;
+ fifi[1][4] =-kRp[4]*kRp[4]/(P*P*P)+1./P ;
+ fifi[1][5] =-kRp[4]*kRp[5]/(P*P*P) ;
+ fifi[2][3] =-kRp[5]*kRp[3]/(P*P*P) ;
+ fifi[2][4] =-kRp[5]*kRp[4]/(P*P*P) ;
+ fifi[2][5] =-kRp[5]*kRp[5]/(P*P*P)+1./P ;
+ fifi[3][3] =-k*kRp[4]*kRp[3]/(P*P) ;
+ fifi[3][4] =-k*kRp[4]*kRp[4]/(P*P)+k ;
+ fifi[3][5] =-k*kRp[4]*kRp[5]/(P*P) ;
+ fifi[4][3] =-k*kRp[3]*kRp[3]/(P*P)-k ;
+ fifi[4][4] =-k*kRp[3]*kRp[4]/(P*P) ;
+ fifi[4][5] =-k*kRp[3]*kRp[5]/(P*P) ;
+ fi.constructWithCopy(fifi);
+ fi=fi.times(ds);
+ Matrix fiMtx = fi.copy();
+ fi.plus(Matrix.identity(6,6));
+ //update covariant matrix
+ PkMinus.constructWithCopy(kCovariance); //
+ PkMinus =((fiMtx.times(PkMinus)).times(fiMtx.transpose())).plus(Qk);
+ return (ktransport=true);
+}
+/** stepby dx on the x direction
+ *@param dx one step in x
+ */
+ private boolean stepByDx(double dx){ // one step by dx
+ double P = Math.sqrt(kRp[3]*kRp[3]+ kRp[4]*kRp[4]+kRp[5]*kRp[5]);
+ if(Math.abs(kRp[3])<1.E-010){
+ System.out.println("Zero Px, can't propagate by dx");
+ return (stopTkELow=true);
+ }
+ return stepByDs(dx*P/kRp[3]);
+ }
+/** stepby dr along the radius direction
+ *@param dr one step in r
+ */
+ private boolean stepByDr(double dr){ // one step by dr
+ double P = Math.sqrt(kRp[3]*kRp[3]+ kRp[4]*kRp[4]+kRp[5]*kRp[5]);
+ double a = kRp[3]*kRp[3]+kRp[4]*kRp[4]/P/P;
+ double b = kRp[0]*kRp[3]+kRp[1]*kRp[4]/P ;
+ double c =-(dr*dr+2*dr*Math.sqrt(kRp[0]*kRp[0]+kRp[1]*kRp[1]));
+ double ds=(-b+Math.sqrt(b*b-a*c))/a ;
+ return stepByDs(ds);
+ }
+/** update the phase space point and covariant Matrix
+ *Using the measured position and errors
+ *@param mstate a 3dim array with the measure position
+ *@param mCov the measurement error Matrix
+ */
+ public boolean upDate(double []mState,double[][]mCov){
+ // update vector with current measurements
+ double [] cState= new double[6];
+ for(int i=0;i<6;i++) {cState[i]=kRp[i];}
+ measState = new double [3];
+ double [][]val1 = {{1.,0.,0.,0.,0.,0.,0.},{0.,1.,0.,0.,0.,0.,0.},
+ {0.,0.,1.,0.,0.,0.,0.}};
+ Hk=new Matrix(val1);
+ // measurement covariante Matrix
+ for(int i=0;i<3;i++) measCov[i][i]=mCov[i][i];
+ /** update error covariance Matrix- Kalman Gain Matrix
+ */
+ Matrix KkPart = (Hk.times(PkMinus).times(Hk.transpose()));
+ Matrix Meas= new Matrix(3,3);
+ Meas.constructWithCopy(measCov); KkPart.plusEquals(Meas) ;
+ Matrix Kk = PkMinus.times(Hk.transpose()).times(KkPart.inverse());
+
+ Matrix PkPlus =(Matrix.identity(6,6).minus(Kk.times(Hk))).times(PkMinus);
+ double []rrpa=new double[3]; double []rrpb = new double[3];
+ rrpa = Kk.times(mState); rrpb=Hk.times(cState);
+ for(int i=0;i<3;i++){
+ kRp[i] +=rrpa[i]+rrpb[i];
+// kRp[i] +=Kk.times(mState[i]-Hk.times(cState));
+ }
+ return (kupDate = true);
+ }
+/** F=Get the dedx for a given material calculated from
+ *the Bethe-Block-(Sternheimer) calculation
+ *@param pmom (double)absolute particle momentum in GeV/c
+ *@param mass (double)particle mass in GeV
+ *@param materialName a String with the material gone through
+ *@return dedx (double)the energy loss in GeV/mm
+ */
+ public double getDeDx(double pmom, double mass, String materialName){
+ if(Math.abs(pmom/mass )< 1.e-3){ return 0.;}
+ double dedx = 1e-10;
+ double [] p={gevTomev*kRp[3],gevTomev*kRp[4],gevTomev*kRp[5]};
+ //System.out.println("MeV px="+p[0]+" py="+p[1]+" pz="+p[2]);
+ MaterialManager manager = MaterialManager.instance();
+ try{
+ org.lcsim.material.Material mat = manager.findMaterial(materialName);
+ double oneCm=1.;
+ dedx = MaterialCalculator.computeBetheBloch(mat, p,(1000*mass), kQ, oneCm);// dedx MeV/cm
+ //System.out.println("getDeDx !!! dedx(MeV/cm)="+dedx);
+ dedx*=mevTogev/cmTomm;
+ //System.out.println("getDeDx !!! dedx(GeV/mm)="+dedx);
+ } catch (MaterialNotFoundException e){}
+ if(dedx<=0.) return 1e-10;
+ return dedx;
+ }
+ public double getRadLength(String materialName){
+ MaterialManager manager = MaterialManager.instance();
+ double radLength=1.E-14;
+ double density=1.E-13;
+ try{
+ org.lcsim.material.Material mat = manager.findMaterial(materialName);
+ double zeff = mat.getZeff();
+ double aeff = mat.getAeff();
+ density= mat.getDensity();
+ radLength=MaterialCalculator.computeRadiationLengthTsai(aeff,zeff);
+ } catch (MaterialNotFoundException e){}
+ if(radLength<=0.) return 1.e-14;
+ return (radLength/density); // in cm
+ }
+ public void setMatNam(String mtn){MaterialName=mtn;}
+ public String getMatNam(){return MaterialName;}
+ public double getX(){return kRp[0];}
+ public double getY(){return kRp[1];}
+ public double getZ(){return kRp[2];}
+ public double [] getParameter(){return kRp ;}
+ public double[] getNewRp(){
+ for(int i=0;i<6;i++) nwRp[i]=kRp[i];
+ nwRp[6]=kQ; nwRp[7]=kMass;
+ return nwRp;
+ }
+ public double [][] getCovariance(){return measCov;}
+ public void resetCovariance(){
+ for(int i=0; i<6; i++){
+ for(int j=0;j<6;j++){
+ kCovariance[i][j] = 0.;
+ }
+ }
+ }
+ public void setBField1Dim(double bZ){ kB=bZ;}
+
+ public void setRungeKutta(){kRungeKutta=true;}
+
+ public double getBField1Dim(){return kB;}
+ public void setSign(double xQ){kQ=xQ;}
+ public double[] getBfield(){
+ double []bField=new double[3];
+ try{
+ bField = det.getFieldMap().getField(kRp);
+ }catch(Exception e) {}
+ return bField;
+ }
+
+/**@return vectorial product p x n_unit(rndm:-1:1) -CM- for mult.scat.-March05*/
+ private double [] pCrossN()
+ {
+ double []alfa = new double[3];
+ double [] nnx = new double[3];
+ double alfNorm;
+ /** new direction of vector momentum for ms-C.M.-March 05*/
+ nnx[0]=-1.+2*rndcm.nextDouble();
+ nnx[1]=-1.+2*rndcm.nextDouble();
+ nnx[2]=-1.+2*rndcm.nextDouble();
+ alfa[0] = kRp[4]*nnx[2]-kRp[5]*nnx[1];
+ alfa[1] = kRp[5]*nnx[0]-kRp[3]*nnx[2];
+ alfa[2] = kRp[4]*nnx[0]-kRp[3]*nnx[1];
+ alfNorm = Math.sqrt(alfa[0]*alfa[0]+alfa[1]*alfa[1]+alfa[2]*alfa[2]);
+ alfa[0] = alfa[0]/alfNorm; // without units
+ alfa[1] = alfa[1]/alfNorm;
+ alfa[2] = alfa[2]/alfNorm;
+ //System.out.println(" setpxn="+alfa[0]+alfa[1]+alfa[2]);
+ return alfa;
+ }
+ /**@param numRadL(double) the number of radiation Length
+ * @return Multiple Scattering contributions to Momentum componants-C.M.-March05*/
+ private double [] vwk(double numRadL)
+ {
+ double [] alf = new double[3];
+ double []thetak = new double[3];
+ double [] wk = new double[3];
+ double theta0;
+ double pabs = Math.sqrt(kRp[3]*kRp[3]+kRp[4]*kRp[4]+kRp[5]*kRp[5]);
+ //System.out.println("numRadL ="+numRadL);
+ if(numRadL==0.) theta0=0.;
+ else{theta0 = 13.6*Math.sqrt(numRadL)*(1.+0.038*Math.log(numRadL));}
+ //System.out.println("theta0 first="+theta0);
+ if(pabs==0.)theta0=0.;
+ else{theta0=theta0/(1000.*pabs);}
+ //System.out.println("Gauss Random"+rndcm.nextGaussian());
+ alf = pCrossN();
+ //System.out.println("alfa="+alf[0]+" "+alf[1]+" "+alf[2]);
+ thetak[0]=rndcm.nextGaussian()*theta0*alf[0];
+ thetak[1]=rndcm.nextGaussian()*theta0*alf[1];
+ thetak[2]=rndcm.nextGaussian()*theta0*alf[2];
+ //System.out.println("thetak = "+thetak[0]+" "+thetak[1]+" "+thetak[2]);
+ wk[0]=2*pabs*thetak[0];
+ wk[1]=2*pabs*thetak[1];
+ wk[2]=2*pabs*thetak[2];
+ //System.out.println("wk ="+wk[0]+" "+wk[1]+" "+wk[2]);
+ return wk;
+ }
+ /** Set Boolean Indicator If Energy Left
+ */
+ public void setStopTkELow(boolean b)
+ { stopTkELow=b;}
+ /** Reset Boolean Indicator Energy Left for the step
+ */
+ public void resetStopTkELow()
+ {setStopTkELow(false);}
+ /** Return Boolean Indicator Energy Left for the step
+ */
+ public boolean getStopTkELow()
+ {return stopTkELow;}
+}