lcsim/src/org/lcsim/util/step
diff -u -r1.3 -r1.4
--- CamKalmanStepper.java 5 Jul 2007 21:48:22 -0000 1.3
+++ CamKalmanStepper.java 17 Jul 2007 14:41:22 -0000 1.4
@@ -50,7 +50,7 @@
private double [] kRp = new double [6];
private double [] kRPlus ;
private double [] measState;
- private double [][]measCov;
+ private double [][]measCov=new double[3][3];
private double [] nwRp = new double[8];
private Matrix PkMinus = new Matrix(6,6);
private Matrix PkPlus = new Matrix(6,6) ;
@@ -62,6 +62,7 @@
private boolean StopTkElow=false;
private Random rndcm = new Random();
private Detector det ;
+ private static int ncount1=0;
/** default constructor*/
public CamKalmanStepper(){
evtptr = CalHitMapMgr.getInstance();
@@ -131,9 +132,11 @@
*/
public boolean propagateTo(int ixyz,double xyz, double b){
+ ncount1++;
setBField1Dim(b);
double dxyz = xyz-kRp[ixyz];
- int nsteps = (int)(Math.abs(dxyz/0.005)+0.5);
+ int nsteps = (int)(Math.abs(dxyz/0.01)+0.5);
+ if(ncount1<4)System.out.println("In propagateTo dxyz="+dxyz+" nsteps="+nsteps);
for (int i=0;i<nsteps;i++) stepByDx(dxyz/nsteps);
return (ktransport=true);
}
@@ -164,7 +167,7 @@
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);
+ //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) ;
@@ -180,13 +183,20 @@
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);
+ Matrix fi = new Matrix(fifi,6,6) ;
+ if (ncount1<2)System.out.println("Print Matrix fi- ncount1="+ncount1);
+ if(ncount1<2)fi.print(6,6);
+ // fi.constructWithCopy(fifi);
fi=fi.times(ds);
Matrix fiMtx = fi.copy();
fi.plus(Matrix.identity(6,6));
//update covariant matrix
- PkMinus.constructWithCopy(kCovariance); //
+ Matrix PkMinus= new Matrix(kCovariance,6,6); //
+ if(ncount1<2)System.out.println("Print PkMinus- ncount1="+ncount1);
+ if(ncount1<2)PkMinus.print(6,6);
PkMinus =((fiMtx.times(PkMinus)).times(fiMtx.transpose())).plus(Qk);
+ if(ncount1<2)System.out.println("Print PkMinus after MS inclusion");
+ if(ncount1<2)PkMinus.print(6,6);
return (ktransport=true);
}
/** stepby dx on the x direction
@@ -234,23 +244,28 @@
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.}};
+ double [][]val1 = {{1.,0.,0.,0.,0.,0.},{0.,1.,0.,0.,0.,0.},
+ {0.,0.,1.,0.,0.,0.}};
Hk=new Matrix(val1);
+ if(ncount1<4)System.out.println( "print HK Matrix");
+ if(ncount1<4)Hk.print(3,6);
// measurement covariante Matrix
for(int i=0;i<3;i++) measCov[i][i]=mCov[i][i];
/** update error covariance Matrix- Kalman Gain Matrix
- */
+ *Problem here !!!!!!
+ */
Matrix KkPart = (Hk.times(PkMinus).times(Hk.transpose()));
- Matrix Meas= new Matrix(3,3);
- Meas.constructWithCopy(measCov); KkPart.plusEquals(Meas) ;
+ Matrix Meas=new Matrix(measCov,3,3);
+ if(ncount1<4)System.out.println( "print Meas Matrix");
+ if(ncount1<4)Meas.print(3,3);
+ 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));
+ // kRp[i] +=Kk.times(mState[i]-Hk.times(cState[i]));
}
return (kupDate = true);
}