lcsim/src/org/lcsim/contrib/JanStrube/standalone

FitterTestDriver_ReconTrack.java 1.1 -> 1.2

diff -u -r1.1 -r1.2
--- FitterTestDriver_ReconTrack.java	27 Aug 2006 22:41:11 -0000	1.1
+++ FitterTestDriver_ReconTrack.java	29 Aug 2006 19:50:18 -0000	1.2
@@ -1,3 +1,5 @@

+import java.util.ArrayList;
+import java.util.List;

 import java.util.Random;
 
 import org.lcsim.contrib.JanStrube.tracking.EMap;

@@ -10,21 +12,24 @@

 import org.lcsim.event.Track;
 import org.lcsim.mc.fast.tracking.ReconTrack;
 import org.lcsim.spacegeom.CartesianPoint;

+import org.lcsim.spacegeom.CartesianVector;

 import org.lcsim.spacegeom.SpacePoint;

+import org.lcsim.spacegeom.SpaceVector;

 import org.lcsim.util.Driver;
 import org.lcsim.util.aida.AIDA;
 
 import Jama.Matrix;
 
 import static java.lang.Math.abs;

+import static java.lang.Math.sqrt;

 
 class FitterTestDriver_ReconTrack extends Driver {
     private AIDA aida = AIDA.defaultInstance();
     private SpacePoint referencePoint;
     private SpacePoint origin;
     private Random rand;

-    private int count=0;
-

+    private int count = 0;
+

     public FitterTestDriver_ReconTrack() {
         referencePoint = new CartesianPoint(0, 0, 0);
         origin = new CartesianPoint(0, 0, 0);

@@ -32,36 +37,48 @@

     }
 
     public void process(EventHeader event) {

-    	count++;
-    	if (count % 500 == 0)
-    		System.out.printf("Processing event %d\n", count);

+        count++ ;
+        if (count % 500 == 0)
+            System.out.printf("Processing event %d\n", count);

         // beamconstraint when smearing the tracks ?
         NewFastMCTrackFactory tf = new NewFastMCTrackFactory(event, false);

-        Vertex smearedVertex = new Vertex();
-        smearedVertex.setOrigin(origin);

         MCParticle mother = null;
         // K0: 310, J/Psi: 443

-	for (MCParticle p : event.getMCParticles()) {
-	   if (abs(p.getPDGID()) == 443) {
-        	mother = p;
-	    }
-	}
-       	for (Track t : event.getTracks()) {
-        	ReconTrack rt = (ReconTrack) t;
-        	MCParticle part = (MCParticle) rt.getMCParticle();
-        	
-        	SpacePoint org = new CartesianPoint(t.getReferencePoint());
-        	SpacePoint mom = new CartesianPoint(t.getMomentum());
-        	Matrix errors = new Matrix(t.getErrorMatrix());
-        	EMap parameters = NewTrack.SpaceMomentum2Parameters(org, mom, referencePoint, t.getCharge(), 5);
-        	smearedVertex.addTrack(new NewTrack(referencePoint, parameters, errors, t.getCharge()), 0);

+        for (MCParticle p : event.getMCParticles()) {
+            if (abs(p.getPDGID()) == 443) {
+                mother = p;
+            }
+        }
+        List<org.lcsim.contrib.JanStrube.tracking.Track> tracks = new ArrayList<org.lcsim.contrib.JanStrube.tracking.Track>();
+        for (Track t : event.getTracks()) {
+            ReconTrack rt = (ReconTrack) t;
+            MCParticle part = (MCParticle) rt.getMCParticle();
+
+            SpacePoint org = new CartesianPoint(t.getReferencePoint());
+            SpaceVector mom = new CartesianVector(t.getMomentum());
+            Matrix errors = new Matrix(t.getErrorMatrix());
+            EMap parameters = NewTrack.SpaceMomentum2Parameters(org, mom, referencePoint, t.getCharge(), 5);
+            tracks.add(new NewTrack(referencePoint, parameters, errors, t.getCharge()));

-        Fitter fitter2 = new Fitter(smearedVertex, 5.0);
-        Vertex newVtx2 = fitter2.fit();

+        SpacePoint startingPoint = new SpacePoint();
+        Fitter fitter2 = new Fitter(event);
+        fitter2.setDelta_chi2(0);
+        Vertex newVtx2 = fitter2.fit(tracks, startingPoint);
+        double sigmaX = sqrt(newVtx2.getSpatialCovarianceMatrix().get(0, 0));
+        double sigmaY = sqrt(newVtx2.getSpatialCovarianceMatrix().get(1, 1));
+        double sigmaZ = sqrt(newVtx2.getSpatialCovarianceMatrix().get(2, 2));
+        double deltaX = newVtx2.origin().x() - mother.getOrigin().x();
+        double deltaY = newVtx2.origin().y() - mother.getOrigin().y();
+        double deltaZ = newVtx2.origin().z() - mother.getOrigin().z();

         aida.cloud1D("decayLength").fill(new SpacePoint(mother.getEndPoint()).magnitude());

-        aida.cloud1D("smeared_deltaX").fill(newVtx2.origin().x() - mother.getOrigin().x());
-        aida.cloud1D("smeared_deltaY").fill(newVtx2.origin().y() - mother.getOrigin().y());
-        aida.cloud1D("smeared_deltaZ").fill(newVtx2.origin().z() - mother.getOrigin().z());

+        aida.cloud1D("smeared_deltaX").fill(deltaX);
+        aida.cloud1D("smeared_deltaY").fill(deltaY);
+        aida.cloud1D("smeared_deltaZ").fill(deltaZ);
+        aida.cloud1D("sigma_x").fill(sigmaX);
+        aida.cloud1D("sigma_y").fill(sigmaY);
+        aida.cloud1D("sigma_z").fill(sigmaZ);
+        aida.cloud1D("pull_x").fill(deltaX/sigmaX);
+        aida.cloud1D("pull_y").fill(deltaY/sigmaY);
+        aida.cloud1D("pull_z").fill(deltaZ/sigmaZ);

     }
 }

lcsim/src/org/lcsim/contrib/JanStrube/vtxFitter

Fitter.java 1.10 -> 1.11

diff -u -r1.10 -r1.11
--- Fitter.java	20 Aug 2006 05:56:56 -0000	1.10
+++ Fitter.java	29 Aug 2006 19:50:20 -0000	1.11
@@ -1,94 +1,109 @@

 package org.lcsim.contrib.JanStrube.vtxFitter;

/**

- * @version $Id: Fitter.java,v 1.10 2006/08/20 05:56:56 jstrube Exp $

+ * @version $Id: Fitter.java,v 1.11 2006/08/29 19:50:20 jstrube Exp $

*/

+import static java.lang.Math.abs;

 import static java.lang.Math.atan2;
 import static java.lang.Math.sqrt;
 import static org.lcsim.constants.Constants.fieldConversion;

-import static org.lcsim.recon.vertexing.zvtop4.VectorArithmetic.subtract;
-import static org.lcsim.recon.vertexing.zvtop4.VectorArithmetic.add;
-import static org.lcsim.recon.vertexing.zvtop4.VectorArithmetic.dot;

 import static org.lcsim.contrib.JanStrube.tracking.NewTrack.SpaceMomentum2Parameters;

-import java.util.ArrayList;

+import java.util.Collection;

 
 import org.lcsim.contrib.JanStrube.tracking.HelixSwimmer;
 import org.lcsim.contrib.JanStrube.tracking.Track;

+import org.lcsim.event.EventHeader;

 import org.lcsim.spacegeom.CartesianPoint;
 import org.lcsim.spacegeom.SpacePoint;
 
 import Jama.Matrix;

+/**
+ * This is an implementation of a vertex fitter following the paper by Grab and Luchsinger. The parametrization of the
+ * Tracks follows the L3 convention. The fitter takes a collection of tracks as input, then performs the sequence of
+ * filtering and smoothing until convergence of the vertex in chi<sup>2</sup>. The expensive computation of the full
+ * covariance matrix is only done once, after convergence.
+ * 
+ * @author jstrube
+ * 
+ */

 public class Fitter {
     // The Vertex object

-	Vertex particle;
-    
-    Vertex unfittedParticle;
-	// convergence criterion
-	double deltaChi2 = 1e-4;
-

+    Vertex particle;
+

     double B_z;

+    // convergence criterion
+    double delta_chi2 = 1e-2;
+    int max_iter = 20;

     // position
     Matrix x_prev;
     // covariance for x

-    Matrix C_prev;	
-

+    Matrix C_prev;
+

     HelixSwimmer _swimmer;

     // chi2
     double chi2_prev;

-		
-	/**
-	 * Definitions:
-	 * x_k = estimate of the vertex position after using the information of k tracks
-	 * xt = true vertex position
-	 * C_k = cov(x_k) = cov(xk - xt)
-	 * qk = estimate of the momentum of particle k at xt
-	 * qkt = true momentum of particle k at xt
-	 * D_k = cov(qk) = cov(qk - qkt)
-	 * Ek = cov(xk, qk) = cov((xk-xt)(qk-qkt))
-	 * mk = measurement vector
-	 * vk = measurement noise
-	 * Vk = cov(vk)
-	 * Gk = Vk^-1 weightmatrix of particle k
-	 * @param p
-	 */
-	public Fitter(Vertex p, double bz) {
-	    this(p
-            , bz
-            , new Matrix(new double[][] {{10000, 0, 0}, {0, 10000, 0}, {0, 0, 10000}})
-        );
-	}
-
-	public Fitter(Vertex p, double bz, Matrix c0) {
-	    unfittedParticle = p;
-            B_z = bz;
-	    C_prev = c0;
-	    x_prev = new Matrix(p.origin().getCartesianArray(), 3);
-	    _swimmer = new HelixSwimmer(bz);
-	}
-	
-	
-	public Vertex fit() {
-	    particle = new Vertex();
-	    particle.setOrigin(unfittedParticle.origin());
-	    for (int i=0; i<10; ++i) {
-	    	particle._tracks.clear();
-	    	chi2_prev = 0;
-	    	for (Track t : unfittedParticle.getTracks()) {
-	    		double chi2_track = filter(t);
-//	    		System.out.printf("filtering: chi2=%f\n", chi2_track);
-	    	}
-	    	for (Track t : particle.getTracks()) {
-	    		double chi2_track = smoothe(t);
-//	    		System.out.printf("smoothing: %f\n", chi2_track);
-	    	}
-	    }
-	    return particle;
-	}

-    // The problem is that double[] is a column vector...

+    /**
+     * Definitions: x_k = estimate of the vertex position after using the information of k tracks xt = true vertex
+     * position C_k = cov(x_k) = cov(xk - xt) qk = estimate of the momentum of particle k at xt qkt = true momentum of
+     * particle k at xt D_k = cov(qk) = cov(qk - qkt) Ek = cov(xk, qk) = cov((xk-xt)(qk-qkt)) mk = measurement vector vk =
+     * measurement noise Vk = cov(vk) Gk = Vk^-1 weightmatrix of particle k
+     * 
+     * @param p
+     */
+
+    public Fitter(EventHeader event) {
+        this(event.getDetector().getFieldMap().getField(new double[] { 0, 0, 0 })[2]);
+    }
+
+    public Fitter(double bz) {
+        B_z = bz;
+        _swimmer = new HelixSwimmer(bz);
+    }
+
+    public Vertex fit(Collection< ? extends Track> tracks, SpacePoint initialPosition) {
+        C_prev = new Matrix(new double[][] { { 10000, 0, 0 }, { 0, 10000, 0 }, { 0, 0, 10000 } });
+
+        x_prev = new Matrix(initialPosition.getCartesianArray(), 3);
+        particle = new Vertex();
+        particle.setOrigin(initialPosition);
+
+        // set the conditions to enter the loop
+        chi2_prev = 1000;
+        double chi2_this = 0;
+        int n_iter = 0;
+        while (abs(chi2_prev - chi2_this) > delta_chi2 && n_iter < max_iter) {
+//        for (int i=0; i<10; ++i) {
+            n_iter++;
+            particle._tracks.clear();
+//            System.out.printf("before: %f\t after: %f\n", chi2_this, chi2_prev);
+            chi2_this = chi2_prev;
+            chi2_prev = 0;
+            for (Track t : tracks) {
+                double chi2_track = filter(t);
+                // System.out.printf("filtering: chi2=%f\n", chi2_track);
+            }
+            // FIXME: This only changes the parameters of the tracks.
+//            for (Track t : tracks) {
+//                double chi2_track = smoothe(t, false);
+                // System.out.printf("smoothing: %f\n", chi2_track);
+//            }
+        }
+        particle._spatialCovarianceMatrix = C_prev;
+        particle._chi2 = chi2_this;
+        // final iteration, this time compute the full covariance matrix
+//        for (Track t : tracks) {
+//            double chi2_track = smoothe(t, true);
+            // System.out.printf("smoothing: %f\n", chi2_track);
+//        }
+        return particle;
+    }
+

     // Turn everything into a Matrix
     private double filter(Track t) {
         // First, get the Matrices at the point of linearization x0, p0

@@ -99,212 +114,184 @@

         double alpha = _swimmer.getTrackLengthToPoint(particle.origin());
         SpacePoint p = _swimmer.getMomentumAtLength(alpha);
         // x is the POCA of the current track to the best estimate

         SpacePoint x = _swimmer.getPointAtLength(alpha);

         Matrix A_k = getSpatialDerivativeMatrix(particle.origin(), p, t.getCharge(), B_z);
         Matrix B_k = getMomentumDerivativeMatrix(particle.origin(), p, t.getCharge(), B_z);
         Matrix q0 = new Matrix(p.getCartesianArray(), 3);
         // turn the measurement into a Matrix object

-        Matrix h = new Matrix(SpaceMomentum2Parameters(
-                particle.origin(), p, particle.origin(), t.getCharge(), B_z).getValues()
-                , 5);

+        Matrix h = new Matrix(SpaceMomentum2Parameters(particle.origin(), p, particle.origin(), t.getCharge(), B_z)
+                .getValues(), 5);

         // the measurement of the track is taken at the POCA to the best estimate

-        Matrix m_k = new Matrix(SpaceMomentum2Parameters(
-                x, p, particle.origin(), t.getCharge(), B_z).getValues()
-                , 5);
-        Matrix c_k0 =  h.minus(A_k.times(x_prev).plus(B_k.times(q0)));
-        
-        // Now for the real filtering equations

+        Matrix m_k = new Matrix(SpaceMomentum2Parameters(x, p, particle.origin(), t.getCharge(), B_z).getValues(), 5);
+        Matrix c_k0 = h.minus(A_k.times(x_prev).plus(B_k.times(q0)));
+
+        // Now for the real filtering equations

         Matrix G_k = t.getErrorMatrix().inverse();
         Matrix W_k = (B_k.transpose().times(G_k.times(B_k))).inverse();

-        Matrix G_kB = G_k.minus((G_k.times(B_k.times(W_k.times(B_k.transpose().times(G_k))))));

+        Matrix G_kB = G_k.minus( (G_k.times(B_k.times(W_k.times(B_k.transpose().times(G_k))))));

 
         // cov(x)

-//        System.out.printf("C_k before: %g\t%g\t%g\n", C_prev.get(0, 0), C_prev.get(1, 1), C_prev.get(2, 2));

+        // System.out.printf("C_k before: %g\t%g\t%g\n", C_prev.get(0, 0), C_prev.get(1, 1), C_prev.get(2, 2));

         Matrix C_k = C_prev.inverse().plus(A_k.transpose().times(G_kB.times(A_k))).inverse();

-//        System.out.printf("C_k after: %g\t%g\t%g\n", C_k.get(0, 0), C_k.get(1, 1), C_k.get(2, 2));
-        Matrix x_k = C_k.times(C_prev.inverse().times(x_prev).plus(
-                 A_k.transpose().times(G_kB).times(m_k.minus(c_k0))));

+        // System.out.printf("C_k after: %g\t%g\t%g\n", C_k.get(0, 0), C_k.get(1, 1), C_k.get(2, 2));
+        Matrix x_k = C_k.times(C_prev.inverse().times(x_prev).plus(A_k.transpose().times(G_kB).times(m_k.minus(c_k0))));

         Matrix q_k = W_k.times(B_k.transpose().times(G_k.times(m_k.minus(c_k0.plus(A_k.times(x_k))))));

         // cov(q)

-        Matrix D_k = W_k.plus(W_k.times(B_k.transpose().times(G_k.times(A_k.times(C_k.times(A_k.transpose().times(G_k.times(B_k.times(W_k)))))))));
-//        System.out.printf("D_k: %g\t%g\t%g\n", D_k.get(0, 0), D_k.get(1, 1), D_k.get(2, 2));
-        // cov(x, q)
-        Matrix E_k = C_k.times(A_k.transpose().times(G_k.times(B_k.times(W_k)))).uminus();
-

         // now the chi2
         Matrix r_k = h.minus(c_k0.plus(A_k.times(x_k).plus(B_k.times(q_k))));

-//        System.out.printf("chi2: %s + %s", r_k.transpose().times(G_k.times(r_k)), x_k.minus(x_prev).transpose().times(C_prev.inverse().times(x_k.minus(x_prev))));

+        // System.out.printf("chi2: %s + %s", r_k.transpose().times(G_k.times(r_k)),
+        // x_k.minus(x_prev).transpose().times(C_prev.inverse().times(x_k.minus(x_prev))));

         double chi2_kf = r_k.transpose().times(G_k.times(r_k)).get(0, 0)

-            + x_k.minus(x_prev).transpose().times(C_prev.inverse().times(x_k.minus(x_prev))).get(0, 0);
-		
-        double chi2 = chi2_prev + chi2_kf;
-//		System.out.printf("before: %s", x_prev);

+                + x_k.minus(x_prev).transpose().times(C_prev.inverse().times(x_k.minus(x_prev))).get(0, 0);
+
+        chi2_prev += chi2_kf;
+        // System.out.printf("before: %s", x_prev);

         x_prev = x_k;

-//        System.out.printf("after: %s", x_prev);

+        // System.out.printf("after: %s", x_prev);

         C_prev = C_k;
         particle.addTrack(t, chi2_kf);
         particle._origin = new CartesianPoint(x_k.getRowPackedCopy());

-//        System.out.println(particle._origin);
-        chi2_prev = chi2;

+        // System.out.println(particle._origin);
+//        if (fullFit) {
+//            Matrix D_k = W_k.plus(W_k.times(B_k.transpose().times(
+//                    G_k.times(A_k.times(C_k.times(A_k.transpose().times(G_k.times(B_k.times(W_k)))))))));
+//            // System.out.printf("D_k: %g\t%g\t%g\n", D_k.get(0, 0), D_k.get(1, 1), D_k.get(2, 2));
+//            // cov(x, q)
+//            Matrix E_k = C_k.times(A_k.transpose().times(G_k.times(B_k.times(W_k)))).uminus();
+//        }
+

         return chi2_kf;

-	}
-	
-	private double smoothe(Track t) {
-	    _swimmer.setTrack(t);
-	    // at the moment, particle.origin is the best estimate so far
-	    double alpha = _swimmer.getTrackLengthToPoint(particle.origin());
-	    SpacePoint p = _swimmer.getMomentumAtLength(alpha);
-	    // x is the POCA of the current track to the best estimate
-	    Matrix q0 = new Matrix(p.getCartesianArray(), 3);
-            SpacePoint x = _swimmer.getPointAtLength(alpha);
-        
-	    Matrix A_k = getSpatialDerivativeMatrix(particle.origin(), p, t.getCharge(), B_z);
-	    Matrix B_k = getMomentumDerivativeMatrix(particle.origin(), p, t.getCharge(), B_z);

+    }
+
+    private double smoothe(Track t, boolean fullFit) {
+        _swimmer.setTrack(t);
+        // at the moment, particle.origin is the best estimate so far
+        double alpha = _swimmer.getTrackLengthToPoint(particle.origin());
+        SpacePoint p = _swimmer.getMomentumAtLength(alpha);
+        // x is the POCA of the current track to the best estimate
+        Matrix q0 = new Matrix(p.getCartesianArray(), 3);
+        SpacePoint x = _swimmer.getPointAtLength(alpha);
+
+        Matrix A_k = getSpatialDerivativeMatrix(particle.origin(), p, t.getCharge(), B_z);
+        Matrix B_k = getMomentumDerivativeMatrix(particle.origin(), p, t.getCharge(), B_z);
+
+        Matrix G_k = t.getErrorMatrix().inverse();
+        Matrix W_k = B_k.transpose().times(G_k.times(B_k)).uminus();
+        // the measurement of the track is taken at the POCA to the best estimate
+        Matrix m_k = new Matrix(SpaceMomentum2Parameters(x, p, particle.origin(), t.getCharge(), B_z).getValues(), 5);
+        Matrix c_k0 = m_k.minus(A_k.times(x_prev).plus(B_k.times(q0)));
+
+        if (fullFit) {
+            Matrix q_kN = W_k.times(B_k.transpose().times(G_k)).times(m_k.minus(c_k0.plus(A_k.times(x_prev))));
+            Matrix D_kN = W_k.plus(W_k.times(B_k.transpose().times(
+                    G_k.times(A_k.times(C_prev.times(A_k.transpose().times(G_k.times(B_k.times(W_k)))))))));
+            Matrix E_kN = C_prev.times(A_k.transpose().times(G_k.times(B_k.times(W_k)))).uminus();
+        }

-	    Matrix G_k = t.getErrorMatrix().inverse();
-	    Matrix W_k = B_k.transpose().times(G_k.times(B_k)).uminus();
-	    // the measurement of the track is taken at the POCA to the best estimate
-	    Matrix m_k = new Matrix(SpaceMomentum2Parameters(
-                x, p, particle.origin(), t.getCharge(), B_z).getValues()
-                , 5);
-	    Matrix c_k0 =  m_k.minus(A_k.times(x_prev).plus(B_k.times(q0)));
-
-	    Matrix q_kN = W_k.times(B_k.transpose().times(G_k)).times(m_k.minus(c_k0.plus(A_k.times(x_prev))));
-	    // Matrix D_kN = W_k.plus(W_k.times(B_k.transpose().times(G_k.times(A_k.times(C_prev.times(A_k.transpose().times(G_k.times(B_k.times(W_k)))))))));
-	    // Matrix E_kN = C_prev.times(A_k.transpose().times(G_k.times(B_k.times(W_k)))).uminus();
-	    return 0;
-	}
-
-
-//	// returns y = A.x
-//	static double[] multiply(Matrix A, double[] x) {
-//		if (A.getColumnDimension() != x.length)
-//			throw new IllegalArgumentException("dimensions do not match");
-//		double[] result = new double[A.getRowDimension()];
-//		
-//		for (int i=0; i<A.getRowDimension(); i++) {
-//			for (int j=0; j<A.getColumnDimension(); j++) {
-//				result[i] += A.get(i, j) * x[j];
-//			}
-//		}
-//		return result;
-//	}
-//	
-//	// for notational convenience
-//	static Matrix multiply(Matrix a, Matrix b) {
-//		return a.times(b);
-//	}
-	
-	public static Matrix getMomentumDerivativeMatrix(SpacePoint x, SpacePoint p, int charge, double Bz) {
-        double field = Bz*fieldConversion;
-    	double pt = p.rxy();
-    	double px0 = p.x() - charge*field;
-    	double py0 = p.y() + charge*field;
-    	double pt0 = sqrt(px0*px0 + py0*py0);
-    	double phi = atan2(p.y(), p.x());
-    	double phi0 = atan2(py0, px0);
-    	double l = (phi-phi0)*pt/(charge*field);
-    
-        double[] dTanLambda_dp = new double[] {
-                -p.z()*p.x() / (pt*pt*pt)
-                , -p.z()*p.y() / (pt*pt*pt)
-                , 1/pt
-        };
-        double[] dPhi_dp = new double[] {
-                -py0 / (pt0*pt0)
-                , px0 / (pt0*pt0)
-                , 0
-        };
-        double[] dOmega_dp = new double[] {
-                -charge*field*p.x() / (pt*pt*pt)
-                , -charge*field*p.y() / (pt*pt*pt)
-                , 0
-        };
-        double[] dD0_dp = new double[] {
-                -py0/pt0
-                , px0/pt0
-                , 0
-        };
-        double[] dZ_dp = new double[] {
-                -p.z() / (field*charge) * (p.y()/(pt*pt) - py0/(pt0*pt0))
-                , -p.z() / (field*charge) * (p.x()/(pt*pt) - px0/(pt0*pt0))
-                , -l/pt
-        };        
-    	return new Matrix(new double[][] {
-                dD0_dp, dPhi_dp, dOmega_dp, dZ_dp, dTanLambda_dp
-                });

+        return 0;
+    }
+
+    // // returns y = A.x
+    // static double[] multiply(Matrix A, double[] x) {
+    // if (A.getColumnDimension() != x.length)
+    // throw new IllegalArgumentException("dimensions do not match");
+    // double[] result = new double[A.getRowDimension()];
+    //		
+    // for (int i=0; i<A.getRowDimension(); i++) {
+    // for (int j=0; j<A.getColumnDimension(); j++) {
+    // result[i] += A.get(i, j) * x[j];
+    // }
+    // }
+    // return result;
+    // }
+    //	
+    // // for notational convenience
+    // static Matrix multiply(Matrix a, Matrix b) {
+    // return a.times(b);
+    // }
+
+    public static Matrix getMomentumDerivativeMatrix(SpacePoint x, SpacePoint p, int charge, double Bz) {
+        double field = Bz * fieldConversion;
+        double pt = p.rxy();
+        double px0 = p.x() - charge * field;
+        double py0 = p.y() + charge * field;
+        double pt0 = sqrt(px0 * px0 + py0 * py0);
+        double phi = atan2(p.y(), p.x());
+        double phi0 = atan2(py0, px0);
+        double l = (phi - phi0) * pt / (charge * field);
+
+        double[] dTanLambda_dp = new double[] { -p.z() * p.x() / (pt * pt * pt), -p.z() * p.y() / (pt * pt * pt),
+                1 / pt };
+        double[] dPhi_dp = new double[] { -py0 / (pt0 * pt0), px0 / (pt0 * pt0), 0 };
+        double[] dOmega_dp = new double[] { -charge * field * p.x() / (pt * pt * pt),
+                -charge * field * p.y() / (pt * pt * pt), 0 };
+        double[] dD0_dp = new double[] { -py0 / pt0, px0 / pt0, 0 };
+        double[] dZ_dp = new double[] { -p.z() / (field * charge) * (p.y() / (pt * pt) - py0 / (pt0 * pt0)),
+                -p.z() / (field * charge) * (p.x() / (pt * pt) - px0 / (pt0 * pt0)), -l / pt };
+        return new Matrix(new double[][] { dD0_dp, dPhi_dp, dOmega_dp, dZ_dp, dTanLambda_dp });

     }
 
     /**
      * Calculates the derivatives of the measurement vector with respect to cartesian spatial coordinates.

+     *

      * @param x
      * @param p
      * @param charge
      * @return the derivatives Matrix. Assumes an ordering of ??? in the measurement vector
      */
     public static Matrix getSpatialDerivativeMatrix(SpacePoint x, SpacePoint p, int charge, double Bz) {

-        double field = Bz*fieldConversion;
-    
-        double px0 = p.x() - charge*field;
-    	double py0 = p.y() + charge*field;
-    	double pt0 = sqrt(px0*px0 + py0*py0);
-

+        double field = Bz * fieldConversion;
+
+        double px0 = p.x() - charge * field;
+        double py0 = p.y() + charge * field;
+        double pt0 = sqrt(px0 * px0 + py0 * py0);
+

         // be very explicit about the derivatives
         // dr = (dx, dy, dz)^T

-        double[] dTanLambda_dr = new double[] {
-                0, 0, 0
-        };
-        
-        double[] dPhi_dr = new double[] {
-                px0*field*charge / (pt0*pt0)
-                , py0*field*charge / (pt0*pt0)
-                , 0
-        };
-        
-        double[] dOmega_dr = new double[] {
-                0, 0, 0
-        };
-        
-        double[] dD0_dr = new double[] {
-                -py0 / pt0
-                , px0 / pt0
-                , 0
-        };
-        
-        double[] dZ_dr = new double[] {
-                -p.z() * px0 / (pt0*pt0)
-                , -p.z() * py0 / (pt0*pt0)
-                , 1
-        };
-        
-    	return new Matrix(new double[][] {
-                dD0_dr, dPhi_dr, dOmega_dr, dZ_dr, dTanLambda_dr
-                });
-    }
-	
-	// for notational convenience
-	static Matrix add(Matrix a, Matrix b) {
-		return a.plus(b);
-	}
-	
-	// matrix A_c in the Grab and Luchsinger paper
-	static Matrix chargedSpatialLinearity(Track t) {
-		
-		return new Matrix(0, 0);
-	}
-	
-	static Matrix chargedMomentumLinearity(Track t) {
-		return new Matrix(0, 0);
-	}
-	
-	static Matrix calcSpatialLinearityMatrix(Track t) {
-		return new Matrix(5, 5);
-	}
-	
-	static Matrix calcMomentumLinearityMatrix(Track t) {
-		return new Matrix(5, 5);
-	}

+        double[] dTanLambda_dr = new double[] { 0, 0, 0 };
+
+        double[] dPhi_dr = new double[] { px0 * field * charge / (pt0 * pt0), py0 * field * charge / (pt0 * pt0), 0 };
+
+        double[] dOmega_dr = new double[] { 0, 0, 0 };
+
+        double[] dD0_dr = new double[] { -py0 / pt0, px0 / pt0, 0 };
+
+        double[] dZ_dr = new double[] { -p.z() * px0 / (pt0 * pt0), -p.z() * py0 / (pt0 * pt0), 1 };
+
+        return new Matrix(new double[][] { dD0_dr, dPhi_dr, dOmega_dr, dZ_dr, dTanLambda_dr });
+    }
+
+    // for notational convenience
+    static Matrix add(Matrix a, Matrix b) {
+        return a.plus(b);
+    }
+
+    // matrix A_c in the Grab and Luchsinger paper
+    static Matrix chargedSpatialLinearity(Track t) {
+
+        return new Matrix(0, 0);
+    }
+
+    static Matrix chargedMomentumLinearity(Track t) {
+        return new Matrix(0, 0);
+    }
+
+    static Matrix calcSpatialLinearityMatrix(Track t) {
+        return new Matrix(5, 5);
+    }
+
+    static Matrix calcMomentumLinearityMatrix(Track t) {
+        return new Matrix(5, 5);
+    }
+
+    public double getDelta_chi2() {
+        return delta_chi2;
+    }
+
+    public void setDelta_chi2(double d) {
+        delta_chi2 = d;
+    }

Commit in `lcsim` on MAIN
`src/org/lcsim/contrib/JanStrube/standalone/TrackComparison.java`	+46		added 1.1
`/PrimaryParticleDaughterFitter.java`	+56		added 1.1
`/PrimaryParticleFinder.java`	+31		added 1.1
`/MainLoop.java`	+6	-4	1.3 -> 1.4
`/FitterTestDriver_ReconTrack.java`	+44	-27	1.1 -> 1.2
`/FitterTestDriver.java`	+10	-10	1.2 -> 1.3
`/.cvsignore`	+2		1.1 -> 1.2
`src/org/lcsim/spacegeom/SpacePoint.java`	+4	-7	1.7 -> 1.8
`/CartesianVector.java`	+5	-2	1.2 -> 1.3
`/CartesianPoint.java`	+2	-3	1.5 -> 1.6
`src/org/lcsim/contrib/JanStrube/vtxFitter/Fitter.java`	+225	-238	1.10 -> 1.11
`/Vertex.java`	+29	-2	1.4 -> 1.5
`/vertexing.lyx`	+19	-50	1.8 -> 1.9
`/vertexing.pdf`	[binary]		1.4 -> 1.5
`src/org/lcsim/contrib/JanStrube/tracking/Track.java`	+15	-1	1.3 -> 1.4
`/NewTrack.java`	+9	-1	1.6 -> 1.7
`test/org/lcsim/contrib/JanStrube/tracking/FitterTest.java`	+8	-7	1.2 -> 1.3
	+511	-352