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);
  }