lcsim/src/org/lcsim/contrib/JanStrube/vtxFitter
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;
+ }
+}
lcsim/src/org/lcsim/contrib/JanStrube/vtxFitter
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.);
+ }
+
+}