lcsim/src/org/lcsim/util/step
diff -u -r1.2 -r1.3
--- CamKalmanStepper.java 21 May 2007 17:17:58 -0000 1.2
+++ CamKalmanStepper.java 5 Jul 2007 21:48:22 -0000 1.3
@@ -116,11 +116,25 @@
*@param x the position
*@param b the magnetic field
*/
- public boolean propagateTo(Double x, double b){
+ public boolean propagateTo(double r, 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);
+ double dr = r-Math.sqrt(kRp[0]*kRp[0]+kRp[1]*kRp[1]);
+ int nsteps = (int)(Math.abs(dr/0.005)+0.5);
+ for (int i=0;i<nsteps;i++) stepByDr(dr/nsteps);
+ return (ktransport=true);
+}
+ /** Propagate the particle from a position
+ * xyz defined either by x, y, or z
+ * to s new position in a magnetic field b
+ *@param xyz the position
+ *@param b the magnetic field
+ */
+
+public boolean propagateTo(int ixyz,double xyz, double b){
+ setBField1Dim(b);
+ double dxyz = xyz-kRp[ixyz];
+ int nsteps = (int)(Math.abs(dxyz/0.005)+0.5);
+ for (int i=0;i<nsteps;i++) stepByDx(dxyz/nsteps);
return (ktransport=true);
}
/** stepby ds on the particle pathLength
@@ -186,6 +200,19 @@
}
return stepByDs(dx*P/kRp[3]);
}
+
+ /** stepby dxyz on the xyz direction
+ *@param dxyz one step in xyz
+ */
+ private boolean stepByDx(int ixyz,double dxyz){ // one step by dx
+ double P = Math.sqrt(kRp[3]*kRp[3]+ kRp[4]*kRp[4]+kRp[5]*kRp[5]);
+ if(Math.abs(kRp[ixyz+3])<1.E-010){
+ System.out.println("Zero Pxyz, can't propagate in direction"+ixyz);
+ return (stopTkELow=true);
+ }
+ return stepByDs(dxyz*P/kRp[3]);
+ }
+
/** stepby dr along the radius direction
*@param dr one step in r
*/
@@ -211,14 +238,13 @@
{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];
+ 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);