diff -N Fitter.java
--- /dev/null	1 Jan 1970 00:00:00 -0000
+++ Fitter.java	26 Jun 2006 02:54:31 -0000	1.1
@@ -0,0 +1,117 @@

+package vtxFitter;
+
+import org.lcsim.event.ReconstructedParticle;
+
+import Jama.Matrix;
+
+public class Fitter {
+
+	ReconstructedParticle particle;
+	// convergence criterion
+	double deltaChi2 = 1e-4;
+	
+	// position
+	double[] x_k, x_prev, x;
+	// covariance for x
+	Matrix C_k, C_prev, C;
+	// momentum
+	double[] q_k, q_prev, q;
+	// covariance for momentum
+	Matrix D_k, D_prev, D;
+	//covariance for x and q
+	Matrix E_k, E_prev, E;
+	// track measrement
+	double[] m_k, m_prev, m;
+	// covariance for track measurement
+	Matrix V_k, V_prev, V;
+	// inverse of covariance matrix
+	Matrix G_k, G_prev, G;
+	Matrix G_B;
+	
+	// linearity factors of the measeurement eq.
+	// in x
+	Matrix A_k, A_prev, A;
+	// and q
+	Matrix B_k, B_prev, B;
+	Matrix W_k;
+	
+	// residual
+	double[] c_k0;
+	
+	/**
+	 * 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(ReconstructedParticle p) {
+		this(p
+			, new double[3]
+            , new double[][] {{10000, 0, 0}, {0, 10000, 0}, {0, 0, 10000}});
+	}
+
+	public Fitter(ReconstructedParticle p, double[] x0, double[][] C0) {
+		particle = p;
+		x = x0;
+		C = new Matrix(C0);
+	}
+	
+	private void filter() {
+		x = multiply(C_k
+				, plus(multiply(C_prev.inverse(), x_prev)
+				    , multiply(A_k.transpose().times(G_B)
+				    		, minus(m_k, c_k0))));
+		
+		q = multiply(W_k.times(B_k.transpose().times(G_k)), minus(m_k, plus(c_k0, multiply(A_k, x_k))));
+		C = C_prev.inverse().plus(A_k.transpose().times(G_B.times(A_k))).inverse();
+		D = 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)))))))));
+		E = C_k.times(A_k.transpose().times(G_k.times(B_k.times(W_k)))).times(-1);
+		
+		W_k = (B_k.transpose().times(G_k.times(B_k))).times(-1);
+		G_B = G_k.minus((G_k.times(B_k.times(W_k.times(B_k.transpose().times(G_k))))));
+		
+	}
+	
+	// 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;
+	}
+	
+	static double[] minus(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[] plus(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;
+	}
+}

diff -N FitterTest.java
--- /dev/null	1 Jan 1970 00:00:00 -0000
+++ FitterTest.java	26 Jun 2006 02:54:31 -0000	1.1
@@ -0,0 +1,56 @@

+package vtxFitter;
+
+import Jama.Matrix;
+import junit.framework.TestCase;
+import static vtxFitter.Fitter.minus;
+import static vtxFitter.Fitter.plus;
+import static vtxFitter.Fitter.multiply;
+
+public class FitterTest extends TestCase {
+	Matrix A;
+	double[] x;
+	double[] y;
+	protected void setUp() throws Exception {
+		super.setUp();
+		A = new Matrix(2, 3, 2.5);
+		x = new double[] {1, 2, 3};
+		y = x.clone();
+	}
+
+	protected void tearDown() throws Exception {
+		super.tearDown();
+	}
+
+	/*
+	 * Test method for 'vtxFitter.Fitter.multiply(Matrix, double[])'
+	 */
+	public void testMultiply() {
+		double[] r = multiply(A, x);
+		assertEquals(r.length, A.getRowDimension());
+		assertEquals(r[0], 15.);
+		assertEquals(r[1], 15.);
+	}
+
+	/*
+	 * Test method for 'vtxFitter.Fitter.minus(double[], double[])'
+	 */
+	public void testMinus() {
+		double[] r = minus(x, y);
+		assertEquals(r.length, x.length);
+		assertEquals(r[0], 0.);
+		assertEquals(r[1], 0.);
+		assertEquals(r[2], 0.);
+	}
+
+	/*
+	 * Test method for 'vtxFitter.Fitter.plus(double[], double[])'
+	 */
+	public void testPlus() {
+		double[] r = plus(x, y);
+		assertEquals(r.length, x.length);
+		assertEquals(r[0], 2.);
+		assertEquals(r[1], 4.);
+		assertEquals(r[2], 6.);
+	}
+
+}