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

Fitter.java 1.5 -> 1.6

diff -u -r1.5 -r1.6
--- Fitter.java	4 Jul 2006 01:41:48 -0000	1.5
+++ Fitter.java	7 Aug 2006 21:30:30 -0000	1.6
@@ -1,47 +1,42 @@

 package org.lcsim.contrib.JanStrube.vtxFitter;
 /**

- * @version $Id: Fitter.java,v 1.5 2006/07/04 01:41:48 jstrube Exp $

+ * @version $Id: Fitter.java,v 1.6 2006/08/07 21:30:30 jstrube Exp $

*/

-import org.lcsim.event.ReconstructedParticle;
-import org.lcsim.event.Track;

+import static java.lang.Math.atan2;
+import static java.lang.Math.sqrt;
+import static org.lcsim.contrib.JanStrube.tracking.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 org.lcsim.contrib.JanStrube.tracking.HelixSwimmer;
+import org.lcsim.contrib.JanStrube.tracking.Track;
+import org.lcsim.spacegeom.CartesianPoint;
+import org.lcsim.spacegeom.SpacePoint;

 
 import Jama.Matrix;
 
 public class Fitter {

-	ReconstructedParticle particle;

+    // The Vertex object
+	Vertex particle;
+    
+    Vertex unfittedParticle;

 	// convergence criterion
 	double deltaChi2 = 1e-4;

+    double B_z;

 	// position

-	double[] x_prev;

+	Matrix x_prev;

 	// covariance for x

-	Matrix C_prev;
-	// momentum
-	double[] q_prev;
-	// covariance for momentum
-	Matrix D_prev;
-	//covariance for x and q
-	Matrix E_prev;
-	// covariance for track measurement
-	Matrix V_prev;
-	// inverse of covariance matrix
-	Matrix G_prev;
-	
-	// linearity factors of the measeurement eq.
-	// in x
-	Matrix A_k, A_prev, A;
-	// and q
-	Matrix B_k, B_prev, B;
-	
-	// residual
-	double[] c_k0;

+	Matrix C_prev;	
+    
+    HelixSwimmer _swimmer;

 	
 	// chi2
 	double chi2_prev;

-	
-	double[] r_k;
-

 	/**
 	 * Definitions:
 	 * x_k = estimate of the vertex position after using the information of k tracks

@@ -57,56 +52,116 @@

 	 * Gk = Vk^-1 weightmatrix of particle k
 	 * @param p
 	 */

-	public Fitter(ReconstructedParticle p) {

+	public Fitter(Vertex p, double bz) {

 		this(p

-			, new double[3]
-            , new double[][] {{10000, 0, 0}, {0, 10000, 0}, {0, 0, 10000}});

+            , bz
+            , new Matrix(new double[][] {{10000, 0, 0}, {0, 10000, 0}, {0, 0, 10000}})
+        );

-	public Fitter(ReconstructedParticle p, double[] x0, double[][] C0) {
-		particle = p;
-		x_prev = x0;
-		C_prev = new Matrix(C0);

+	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 void fit() {
-		for (Track t : particle.getTracks()) {

+	public Vertex fit() {
+        particle = new Vertex();
+        particle.setOrigin(unfittedParticle.origin());
+        
+		for (Track t : unfittedParticle.getTracks()) {

 			filter(t);
 		}
 		for (Track t : particle.getTracks()) {
 			smoothe(t);
 		}

+        return particle;

+    
+    // The problem is that double[] is a column vector...
+    // Turn everything into a Matrix

 	private double filter(Track t) {

-        Matrix G_k = new Matrix(t.getErrorMatrix()).inverse();
-		double[] m_k = t.getTrackParameters();
-
-		Matrix W_k = (B_k.transpose().times(G_k.times(B_k))).uminus();
-		Matrix G_B = G_k.minus((G_k.times(B_k.times(W_k.times(B_k.transpose().times(G_k))))));
-		Matrix C_k = C_prev.inverse().plus(A_k.transpose().times(G_B.times(A_k))).inverse();
-		
-		double[] x_k = C_k.times(add(C_prev.inverse().times(x_prev)
-				    , A_k.transpose().times(G_B).times(subtract(m_k, c_k0))));

+        // First, get the Matrices at the point of linearization x0, p0
+        // a good choice for x0 is the previous best estimate and
+        // p0 is the momentum of the track at the POCA to x0
+        _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
+        
+        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);
+        // 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 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))))));

+        // cov(x)
+        Matrix C_k = C_prev.inverse().plus(A_k.transpose().times(G_kB.times(A_k))).inverse();
+        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)))))))));
+        // 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))));
+        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[] q_k = W_k.times(B_k.transpose().times(G_k)).times(subtract(m_k, add(c_k0, A_k.times(x_k))));
-		Matrix D_k = W_k.plus(W_k.times(B_k.transpose().plus(G_k.times(A_k.times(C_k.times(A_k.transpose().times(G_k.times(B_k.times(W_k)))))))));
-		Matrix E_k = C_k.times(A_k.transpose().times(G_k.times(B_k.times(W_k)))).uminus();
-		
-		
-		double chi2 = chi2_prev + dot(r_k, G_k.times(r_k)) + dot(subtract(x_k, x_prev), C_prev.inverse().times(subtract(x_k, x_prev)));

+        
+        double chi2 = chi2_prev + chi2_kf;

-		r_k = subtract(m_k, add(c_k0, add(A_k.times(x_k), B_k.times(q_k))));

+        x_prev = x_k;

         C_prev = C_k;

+        particle.addTrack(t, chi2_kf);
+        particle._origin = new CartesianPoint(x_k.getRowPackedCopy());
+        chi2_prev = chi2;

         return chi2;
 	}
 	
 	private void smoothe(Track t) {

-        Matrix G_k = new Matrix(t.getErrorMatrix()).inverse();
-        Matrix W_k = (B_k.transpose().times(G_k.times(B_k))).uminus();
-		double[] m_k = t.getTrackParameters();
-		double[] q_kN = W_k.times(B_k.transpose().times(G_k)).times(subtract(m_k, add(c_k0, A_k.times(x_prev))));

+        _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)));
+
+        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();
 	}

@@ -130,26 +185,93 @@

 //		return a.times(b);
 //	}

-	static double[] subtract(double[] a, double[] b) {
-		if (a.length != b.length)
-			throw new IllegalArgumentException("dimensions do not match");
-		double[] result = new double[a.length];
-		for (int i=0; i<a.length; i++) {
-			result[i] = a[i] - b[i];
-		}
-		return result;
-	}
-	
-	static double[] add(double[] a, double[] b) {
-		if (a.length != b.length)
-			throw new IllegalArgumentException("dimensions do not match");
-		double[] result = new double[a.length];
-		for (int i=0; i<a.length; i++) {
-			result[i] = a[i] + b[i];
-		}
-		return result;
-	}

+	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);
+    			
+        // 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);

@@ -164,15 +286,6 @@

 	static Matrix chargedMomentumLinearity(Track t) {
 		return new Matrix(0, 0);
 	}

-
-	static double dot(double[] a, double[] b) {
-		if (a.length != b.length)
-			throw new IllegalArgumentException("dimensions don't match");
-		double result = 0;
-		for (int i=0; i<a.length; i++)
-			result += a[i]*b[i];
-		return result;
-	}

 	
 	static Matrix calcSpatialLinearityMatrix(Track t) {
 		return new Matrix(5, 5);

Commit in `lcsim` on MAIN
`src/org/lcsim/contrib/JanStrube/tracking/EMap.java`	+3	-3	1.2 -> 1.3
`/HelixSwimmer.java`	+40	-5	1.4 -> 1.5
`/Line.java`	+3	-3	1.1 -> 1.2
`/NewTrack.java`	+60	-9	1.4 -> 1.5
`/NewFastMCTrackFactory.java`	+3	-3	1.6 -> 1.7
`/Trajectory.java`	+3	-3	1.1 -> 1.2
`/Helix.java`	+12	-11	1.3 -> 1.4
`src/org/lcsim/contrib/JanStrube/vtxFitter/Fitter.java`	+198	-85	1.5 -> 1.6
`/Vertex.java`	+60	-19	1.1 -> 1.2
`/vertexing.lyx`	+18	-10	1.5 -> 1.6
`/VertexFactory.java`		-58	1.5 removed
`/VertexTrack.java`		-121	1.5 removed
`test/org/lcsim/contrib/JanStrube/vtxFitter/FitterTest.java`		-74	1.1 removed
`/VertexFactoryTest.java`		-53	1.1 removed
`test/org/lcsim/contrib/JanStrube/tracking/FitterTest.java`	+56		added 1.1
`/HelixSwimmerTrackConsistencyTest.java`	+10	-10	1.5 -> 1.6
`src/org/lcsim/recon/vertexing/zvtop4/VectorArithmetic.java`	+27	-3	1.12 -> 1.13
	+493	-470