lcsim/src/org/lcsim/recon/vertexing/zvtop4/fitter
diff -N VertexFitter.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ VertexFitter.java 20 Mar 2006 07:42:04 -0000 1.1
@@ -0,0 +1,911 @@
+package org.lcsim.recon.vertexing.zvtop4.fitter;
+
+// Performs a Kalman fit to a list of tracks and returns
+// a Vertex object
+import static java.lang.Math.sin;
+import static java.lang.Math.cos;
+import static java.lang.Math.tan;
+import static java.lang.Math.atan;
+import static java.lang.Math.atan2;
+import static java.lang.Math.sqrt;
+import static java.lang.Math.PI;
+import static org.lcsim.recon.tracking.trfutil.TRFMath.fmod1;
+
+public class VertexFitter {
+ // the value of the magnetic field in the vicinity of the vertex
+ // default is a constant field along the z axis
+ private double _bField;
+
+ // constructor
+ public VertexFitter(double bField) {
+ _bField = bField;
+ }
+
+ // fitter
+ public Vertex fit(int ntrk, boolean fitwb, boolean[] invtx, double[][] par, double[][] wgt, double[] xyz) {
+ return pxfvtx(ntrk, fitwb, invtx, par, wgt, xyz);
+ }
+
+ private double[] pxmi5(double[] wgt) {
+
+ // ***********************************************************************
+ // * *
+ // * *
+ // * inversion of a (5x5) symmetric positive matrix (cholesky method) *
+ // * internal computation in double precision *
+ // * check on positivity (ierr=2 if wgt not positive) *
+ // * *
+ // ***********************************************************************
+ // *
+ double[] cov = new double[15];
+ double t11, t12, t13, t14, t15;
+ double t22, t23, t24, t25;
+ double t33, t34, t35;
+ double t44, t45;
+ double t55;
+ double s12, s13, s14, s15;
+ double s23, s24, s25;
+ double s34, s35;
+ double s45;
+ if (wgt[0] < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t11 = 1. / sqrt(wgt[0]);
+ s12 = wgt[1] * t11;
+ s13 = wgt[3] * t11;
+ s14 = wgt[6] * t11;
+ s15 = wgt[10] * t11;
+ //
+ t22 = wgt[2] - s12 * s12;
+ if (t22 < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t22 = 1. / sqrt(t22);
+ s23 = (wgt[4] - s12 * s13) * t22;
+ s24 = (wgt[7] - s12 * s14) * t22;
+ s25 = (wgt[11] - s12 * s15) * t22;
+ //
+ t33 = wgt[5] - s13 * s13 - s23 * s23;
+ if (t33 < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t33 = 1. / sqrt(t33);
+ s34 = (wgt[8] - s13 * s14 - s23 * s24) * t33;
+ s35 = (wgt[12] - s13 * s15 - s23 * s25) * t33;
+ //
+ t44 = wgt[9] - s14 * s14 - s24 * s24 - s34 * s34;
+ if (t44 < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t44 = 1. / sqrt(t44);
+ s45 = (wgt[13] - s14 * s15 - s24 * s25 - s34 * s35) * t44;
+ //
+ t55 = wgt[14] - s15 * s15 - s25 * s25 - s35 * s35 - s45 * s45;
+ if (t55 < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t55 = 1. / sqrt(t55);
+ //
+ t45 = -t44 * (s45 * t55);
+ t34 = -t33 * (s34 * t44);
+ t35 = -t33 * (s34 * t45 + s35 * t55);
+ t23 = -t22 * (s23 * t33);
+ t24 = -t22 * (s23 * t34 + s24 * t44);
+ t25 = -t22 * (s23 * t35 + s24 * t45 + s25 * t55);
+ t12 = -t11 * (s12 * t22);
+ t13 = -t11 * (s12 * t23 + s13 * t33);
+ t14 = -t11 * (s12 * t24 + s13 * t34 + s14 * t44);
+ t15 = -t11 * (s12 * t25 + s13 * t35 + s14 * t45 + s15 * t55);
+ //
+ cov[0] = t11 * t11 + t12 * t12 + t13 * t13 + t14 * t14 + t15 * t15;
+ cov[1] = t12 * t22 + t13 * t23 + t14 * t24 + t15 * t25;
+ cov[2] = t22 * t22 + t23 * t23 + t24 * t24 + t25 * t25;
+ cov[3] = t13 * t33 + t14 * t34 + t15 * t35;
+ cov[4] = t23 * t33 + t24 * t34 + t25 * t35;
+ cov[5] = t33 * t33 + t34 * t34 + t35 * t35;
+ cov[6] = t14 * t44 + t15 * t45;
+ cov[7] = t24 * t44 + t25 * t45;
+ cov[8] = t34 * t44 + t35 * t45;
+ cov[9] = t44 * t44 + t45 * t45;
+ cov[10] = t15 * t55;
+ cov[11] = t25 * t55;
+ cov[12] = t35 * t55;
+ cov[13] = t45 * t55;
+ cov[14] = t55 * t55;
+ return cov;
+ }
+
+ private double fwdch2(double[] wgt, double d1, double d2, double d3, double d4, double d5) {
+ // ***********************************************************************
+ // * *
+ // * computation of dt*wgt*d *
+ // * wgt is a (5x5) symmetric matrix ; d is a 5-vector *
+ // * *
+ // ***********************************************************************
+ // *
+ return wgt[0]
+ * d1
+ * d1
+ + wgt[2]
+ * d2
+ * d2
+ + wgt[5]
+ * d3
+ * d3
+ + wgt[9]
+ * d4
+ * d4
+ + wgt[14]
+ * d5
+ * d5
+ + 2
+ * (d2 * d1 * wgt[1] + d3 * (d1 * wgt[3] + d2 * wgt[4]) + d4 * (d1 * wgt[6] + d2 * wgt[7] + d3 * wgt[8]) + d5
+ * (d1 * wgt[10] + d2 * wgt[11] + d3 * wgt[12] + d4 * wgt[13]));
+ }
+
+ // ************************************************************************
+ // * *
+ public Perigee perigee(double[] rawdat, double xb, double yb) {
+ // * *
+ // ************************************************************************
+ // name : perigee
+ //
+ // created : 30-jun-1998 author : norman a. graf
+ // based on pxfxpe 7-jun-1989 author : p. billoir (cdf)
+ //
+ // function : compute from input data (with cylindrical or cartesian
+ // coordinates)
+ // the "perigee" parameters (point of closest approach to the
+ // reference axis) and the covariance matrix on them
+ // for error propagation, an approximation is made, assuming
+ // that the r coordinate of reference point is small w.r.t.
+ // the radius of curvature
+ // no account is taken for multiple scattering, so that the
+ // error matrix is underestimated if the data reference point
+ // is outside the beam pipe (except at high momentum)
+ //
+ // references : none
+ //
+ // arguments : input : dat(1:21) : tix array
+ // dat(1:3) : coord. of a point on the track
+ // if "cylindrical" coord. : r,r*phi,z
+ // if "cartesian" coord. : x,y,z
+ // dat(4:5) : theta, phi of the tangent at this point
+ // dat[5] : 1/p with geometrical sign
+ // dat(7:21) : covariance matrix on 5 "free" param. :
+ // rphi,z,theta,phi,1/p if "cylind." coord. (r fixed)
+ // x,y,theta,phi,1/p if "cartes." coord. (z fixed)
+ //
+ // xb,yb : coordinates of the reference axis
+ //
+ // output : par(1:5) : "perigee" parameters
+ // 1 : epsilon (impact par. in xy projection, with sign)
+ // 2 : z coordinate
+ // 3 : theta angle
+ // 4 : phi angle
+ // 5 : 1/r (r = radius of curvature, with sign)
+ // cov(1:15) : covariance matrix of par
+ // wgt(1:15) : weight matrix of par (inverse of cov)
+ //
+ // errors : ierr = 1 : incorrect data in tkr array
+ // (for example not cylindrical coordinates)
+ // 2 : cov not positive (wgt not computed)
+ //
+ // ************************************************************************
+ //
+ //
+ // conversion from d0 cylindrical coordinates to mine...
+ //
+ double sthet, sthet2, cthet, gamma;
+ double d0der11, d0der43, d0der45, d0der55;
+ // double[] rawdat = new double [21];
+ //
+ double cosf, sinf, r0, rcosb, d11, d12, d21, d22, cov1, cov3, cov4, cov7, cov11;
+ // to define the magnetic field - values to be provided :
+ // FIXME this needs to be replaced by the official org.lcsim constant
+ double consa = 0.0003;
+ double bmag = _bField;
+ double consb = consa * bmag;
+
+ // consa = velocity of light (here in GeV/(mm.T) ; depends on units)
+ // bmag = magnetic field along z axis
+ //
+ //
+ double[] par = new double[5];
+ double[] cov = new double[15];
+ double[] wgt = new double[15];
+ double sgn;
+ double[] dat = new double[21];
+ //
+ // local variables
+ // __________________
+ //
+ int ierr, ii;
+ double capphi, cotth, dphi, rdphi, rtrk, xc, x0, yc, y0;
+ double der1, der2, der11, der14, der15, der23, der45;
+ //
+ //
+ // executable statements
+ // ________________________
+ //
+ double twopi = 2. * PI;
+ double halfpi = PI / 2.;
+
+ int icypl = 1;
+ // icypl = 1 if "cylindrical" coordinates, 0 if "plane" coordinates
+ //
+ // convert here...
+ //
+ //
+ // ::: parameters r, rphi, z, theta, phi, 1/p
+ //
+ dat[0] = rawdat[0]; // r
+ dat[1] = rawdat[0] * rawdat[1]; // r*phi
+ dat[2] = rawdat[2]; // z
+ dat[3] = atan(1. / rawdat[4]); // theta
+ if (rawdat[4] < 0)
+ dat[3] = dat[3] + PI;
+ dat[4] = rawdat[1] + rawdat[3]; // phi (recall beta= phi(dir)-phi(pos) )
+ dat[5] = -rawdat[5] * sin(dat[3]); // 1/p (recall 1/pt = 1/(p*sin(theta))
+ //
+ // ::: covariance matrix
+ // ::: there are seven terms in the conversion from
+ //
+ // phi rphi
+ // z z
+ // alfa to theta
+ // cot(theta) phi
+ // q/pt 1/p
+ //
+ // d0der14 = 1
+ // d0der22 = 1
+ // d0der34 = -1
+ //
+ sthet = sin(dat[3]);
+ sthet2 = sthet * sthet;
+ cthet = cos(dat[3]);
+ gamma = -rawdat[5] * cthet * sthet2;
+
+ d0der11 = rawdat[0];
+ d0der43 = -sthet2;
+ d0der45 = -rawdat[5] * cthet * sthet2;
+ d0der55 = sthet;
+ //
+ dat[6] = rawdat[6] * rawdat[0] * rawdat[0];
+ dat[7] = rawdat[7] * rawdat[0];
+ dat[8] = rawdat[8];
+ dat[9] = -rawdat[12] * rawdat[0] * sthet2;
+ dat[10] = -rawdat[13] * sthet2;
+ dat[11] = rawdat[15] * sthet2 * sthet2;
+ dat[12] = (rawdat[6] - rawdat[9]) * rawdat[0];
+ dat[13] = rawdat[7] - rawdat[10];
+ dat[14] = (rawdat[14] - rawdat[12]) * sthet2;
+ dat[15] = rawdat[6] - 2 * rawdat[9] + rawdat[11];
+ dat[16] = - (rawdat[16] * sthet + rawdat[12] * gamma) * rawdat[0];
+ dat[17] = -rawdat[17] * sthet - rawdat[13] * gamma;
+ dat[18] = (rawdat[19] * sthet + rawdat[15] * gamma) * sthet2;
+ dat[19] = (rawdat[14] - rawdat[12]) * gamma + (rawdat[18] - rawdat[16]) * sthet;
+ dat[20] = (rawdat[15] * gamma + 2 * rawdat[19] * sthet) * gamma + rawdat[20] * sthet2;
+
+ //
+ // icypl = 1 if "cylindrical" coordinates, 0 if "plane" coordinates
+ //
+ if ( (icypl == 1 && dat[0] == 0.) || dat[3] == 0. || dat[5] == 0.) {
+ System.out.println("******error in perigee*******");
+ throw new IllegalArgumentException("Choke!");
+ }
+
+ //
+ // now computation of "perigee" parameters
+ //
+ capphi = dat[1] / dat[0];
+ if (icypl == 1) {
+ x0 = dat[0] * cos(capphi) - xb;
+ y0 = dat[0] * sin(capphi) - yb;
+ } else {
+ x0 = dat[0] - xb;
+ y0 = dat[1] - yb;
+ }
+ rtrk = sin(dat[3]) / (consb * dat[5]);
+ cosf = cos(dat[4]);
+ sinf = sin(dat[4]);
+ xc = x0 - rtrk * sinf;
+ yc = y0 + rtrk * cosf;
+ // epsilon (impact parameter in xy projection, with geometrical sign)
+ sgn = 1.;
+ if (rtrk < 0.)
+ sgn = -1.;
+ par[0] = rtrk - sgn * sqrt(xc * xc + yc * yc);
+ // phi at perigee (range 0,2*PI)
+ par[3] = atan2(yc, xc) + PI + sgn * halfpi;
+ if (par[3] < 0.)
+ par[3] = par[3] + twopi;
+ else if (par[3] > twopi)
+ par[3] = par[3] - twopi;
+ // variation of phi from reference point of tkr to perigee (range -pi,+pi)
+ dphi = fmod1(par[3] - dat[4] + twopi + PI, twopi) - PI;
+ rdphi = rtrk * dphi;
+ cotth = 1. / tan(dat[3]);
+ // z of perigee
+ par[1] = dat[2] + cotth * rdphi;
+ // theta and 1/r at perigee (unchanged)
+ par[2] = dat[3];
+ par[4] = 1. / rtrk;
+ //
+ // computation of covariance matrix
+ //
+ for (ii = 0; ii < 15; ++ii) {
+ cov[ii] = dat[6 + ii];
+ }
+ //
+ // transformation from 1/p to 1/r = consb * (1/p) / sin(theta)
+ //
+ der1 = -cotth * par[4];
+ der2 = par[4] / dat[5];
+ cov[14] = der1 * der1 * cov[5] + 2 * der1 * der2 * cov[12] + der2 * der2 * cov[14];
+ cov[10] = der1 * cov[3] + der2 * cov[10];
+ cov[11] = der1 * cov[4] + der2 * cov[11];
+ cov[12] = der1 * cov[5] + der2 * cov[12];
+ cov[13] = der1 * cov[8] + der2 * cov[13];
+ //
+ // if cartesian coordinates transformation from (x,y) to (r*phi,z)
+ // (neglecting curvature effects )
+ //
+ if (icypl == 0) {
+ r0 = sqrt(dat[0] * dat[0] + dat[1] * dat[1]);
+ rcosb = dat[0] * cosf + dat[1] * sinf;
+ d11 = -r0 * sinf / rcosb;
+ d12 = r0 * cosf / rcosb;
+ d21 = -dat[0] * cotth / rcosb;
+ d22 = -dat[1] * cotth / rcosb;
+ cov1 = d11 * d11 * cov[0] + 2 * d11 * d12 * cov[1] + d12 * d12 * cov[2];
+ cov3 = d21 * d21 * cov[0] + 2 * d21 * d22 * cov[1] + d22 * d22 * cov[2];
+ cov[1] = d11 * d21 * cov[0] + (d12 * d21 + d11 * d22) * cov[1] + d12 * d22 * cov[2];
+ cov[0] = cov1;
+ cov[2] = cov3;
+ cov4 = d11 * cov[3] + d12 * cov[4];
+ cov[4] = d21 * cov[3] + d22 * cov[4];
+ cov[3] = cov4;
+ cov7 = d11 * cov[6] + d12 * cov[7];
+ cov[7] = d21 * cov[6] + d22 * cov[7];
+ cov[6] = cov7;
+ cov11 = d11 * cov[10] + d12 * cov[11];
+ cov[11] = d21 * cov[10] + d22 * cov[11];
+ cov[10] = cov11;
+ }
+ //
+ // transformation from (r*phi,z0,theta,phi0,1/r)
+ // to (epsilon,zp,theta,phip,1/r)
+ //
+ // approximation for derivatives d(epsilon)/d(r*phi), d(epsilon)/d(phi0),
+ // d(epsilon)/d(1/r), d(zp)/d(z0) and d(phip)/d(1/r)
+ // the other ones are exactly or approximately 1 (diagonal terms)
+ // or 0 (non-diagonal terms)
+ //
+ der11 = rdphi / sqrt(x0 * x0 + y0 * y0);
+ der14 = -rdphi;
+ der15 = -rdphi * rdphi / 2.;
+ der23 = - (1. + cotth * cotth) * rdphi;
+ der45 = rdphi;
+ //
+ cov[0] = der11
+ * der11
+ * cov[0]
+ + 2
+ * der11
+ * (der14 * cov[6] + der15 * cov[10])
+ + der14
+ * (der14 * cov[9] + 2 * der15 * cov[13])
+ + der15
+ * der15
+ * cov[14];
+ cov[1] = der11
+ * (cov[1] + der23 * cov[3])
+ + der14
+ * (cov[7] + der23 * cov[8])
+ + der15
+ * (cov[11] + der23 * cov[12]);
+ cov[2] = cov[2] + 2 * der23 * cov[4] + der23 * der23 * cov[5];
+ cov[3] = der11 * cov[3] + der14 * cov[8] + der15 * cov[12];
+ cov[4] = cov[4] + der23 * cov[5];
+ cov[6] = der11
+ * (cov[6] + der45 * cov[10])
+ + der14
+ * (cov[9] + der45 * cov[13])
+ + der15
+ * (cov[13] + der45 * cov[14]);
+ cov[7] = cov[7] + der23 * cov[8] + der45 * (cov[11] + der23 * cov[12]);
+ cov[8] = cov[8] + der45 * cov[12];
+ cov[9] = cov[9] + 2 * der45 * cov[13] + der45 * der45 * cov[14];
+ cov[10] = der11 * cov[10] + der14 * cov[13] + der15 * cov[14];
+ cov[11] = cov[11] + der23 * cov[12];
+ cov[13] = cov[13] + der45 * cov[14];
+ //
+ // invert matrix cov to get wgt
+ //
+ wgt = pxmi5(cov);
+ //
+ return new Perigee(par, cov, wgt);
+
+ }
+
+ private double[] pxmi3(double[] wgt) {
+ // ***********************************************************************
+ // * *
+ // * *
+ // * inversion of a (3x3) symmetric positive matrix (cholesky method) *
+ // * internal computation in double precision *
+ // * check on positivity (ierr=2 if wgt not positive) *
+ // * cov (output) may overwrite wgt (input) *
+ // * *
+ // ***********************************************************************
+ // *
+
+ double[] cov = new double[6];
+ double t11, t12, t13;
+ double t21, t22, t23;
+ double t33;
+ double s12, s13;
+ double s23;
+
+ if (wgt[0] < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t11 = 1. / sqrt(wgt[0]);
+ s12 = wgt[1] * t11;
+ s13 = wgt[3] * t11;
+ //
+ t22 = wgt[2] - s12 * s12;
+ if (t22 < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t22 = 1. / sqrt(t22);
+ s23 = (wgt[4] - s12 * s13) * t22;
+ //
+ t33 = wgt[5] - s13 * s13 - s23 * s23;
+ if (t33 < 0.)
+ throw new IllegalArgumentException("Bad weight matrix!");
+
+ t33 = 1. / sqrt(t33);
+ //
+ t23 = -t22 * (s23 * t33);
+ t12 = -t11 * (s12 * t22);
+ t13 = -t11 * (s12 * t23 + s13 * t33);
+ //
+ cov[0] = t11 * t11 + t12 * t12 + t13 * t13;
+ cov[1] = t12 * t22 + t13 * t23;
+ cov[2] = t22 * t22 + t23 * t23;
+ cov[3] = t13 * t33;
+ cov[4] = t23 * t33;
+ cov[5] = t33 * t33;
+ return cov;
+ }
+
+ // ************************************************************************
+ // * *
+ public Vertex pxfvtx(int ntrk, boolean fitwb, boolean[] invtx, double[][] par, double[][] wgt, double[] xyz) {
+ // * *
+ // ************************************************************************
+ // * name : pxfvtx
+ // *
+ // * created : 5-jan-1989 author : p. billoir (cdf)
+ // *
+ // * function : perform a full vertex vertex fit with weighted mean
+ // * of tracks considered as ellipsoids in 5d space of
+ // * "perigee" parameters (epsilon,zp,theta,phi,1/r)
+ // *
+ // ******** 1-12-90 changes in pxfvtx routine : p. billoir
+ // * - to allow the use of the beam spot constraint (logical var. fitwb)
+ // * - to know the contribution of individual tracks to the total chi2.
+ // *
+ // * references : none
+ // *
+ // * arguments : input : ntrk : number of tracks
+ // * fitwb = .true. if we want the beam spot
+ // * constraint
+ // * invtx(i)=.true. to use the track i in the
+ // * vertex fit
+ // * par(1:5,1:ntrk) : "perigee" parameters
+ // * 1 : epsilon (impact par. in xy projection, with sign)
+ // * 2 : z coordinate
+ // * 3 : theta angle
+ // * 4 : phi angle
+ // * 5 : 1/r (r = radius of curvature, with sign)
+ // * wgt(1:15,1:ntrk) : weight matrix on par
+ // * xyz(1:3) : approximate coordinates of the vertex
+ // *
+ // * output : xyzf(1:3) : fitted coordinates of the vertex
+ // * parf(1:3,1:ntrk) : fitted parameters at the vertex
+ // * 1 : theta
+ // * 2 : phi
+ // * 3 : 1/r
+ // * vcov(1:6) : covariance matrix on xyz(vertex)
+ // * tcov(1:6,1:ntrk) : covariance matrix on parf
+ // * chi2 : chi2 of the fit
+ // * chi2tr(i)=contribution of track i to the
+ // * total chi2
+ // *
+ // * errors : ierr = 1 : too many tracks (ntrk > ntrmax)
+ // * ierr = 2 : covariance or weight matrix not positive
+ // *
+ // *************************************************************************
+ // *
+ // *
+ // * "beam spot" description (common to be provided by the user)
+ // * beamoy(1-3) are the mean beam spot positions in x,y and z.
+ // * sigxbe,sigybe,sigzbe are the beam spread in x,y,z
+ // common/pxcpro/beamoy[2],sigxbe,sigybe,sigzbe
+ // double beamoy,sigxbe,sigybe,sigzbe
+ // *
+ // formal parameters
+ // *____________________
+ // *
+ // int i,j,ierr,ntrk,ntrmax
+ // *
+ int ntrmax = 1000;
+
+ double[] xyzf = new double[3];
+ double[][] parf = new double[3][ntrk];
+ double[] vcov = new double[6];
+ double[][] tcov = new double[6][ntrk];
+ double chi2;
+
+ // *
+ // * local variables
+ // *__________________
+ // *
+ double[] wa = new double[6];
+ double[][] wb = new double[9][ntrk];
+ double[][] wc = new double[6][ntrk];
+ double[][] wci = new double[6][ntrk];
+ double[][] wbci = new double[9][ntrk];
+ double[] tv = new double[3];
+ double[][] tt = new double[3][ntrk];
+ double[] dxyz = new double[3];
+ double[] phiv = new double[ntrk];
+ double[] eps = new double[ntrk];
+ double[] zp = new double[ntrk];
+ double[] deps = new double[ntrk];
+ double[] dzp = new double[ntrk];
+ double cotth, cosf, sinf, uu, vv, d11, d12, d21, d22, d41, d42, e12, e13, e21, e22, e23, e43, dw11, dw12, dw13, dw14, dw15, dw21, dw22, dw23, dw24, dw25, dw31, dw32, dw33, dw34, dw35, ew11, ew12, ew13, ew14, ew15, ew21, ew22, ew23, ew24, ew25, ew31, ew32, ew33, ew34, ew35, chi2i, epsf, zpf, phif;
+
+ double fwdch2;
+ double[] chi2tr = new double[ntrk];
+
+ // needs to be set
+ double sigxbe = .1;
+ double sigybe = .1;
+ double sigzbe = .1;
+ double beamoy[] = { 0., 0., 0. };
+ //
+ // *
+ // * executable statements
+ // *________________________
+ // *
+
+ // *
+ // * loop over the tracks
+ // *
+ chi2i = 0.;
+ for (int i = 0; i < ntrk; ++i) {
+ if (invtx[i]) {
+ // *
+ // * starting conditions :
+ // * "perigee" parameters eps and zp if the trajectory goes through xyz
+ // * and its theta,phi,1/r at perigee are equal to the values at input
+ // *
+ cotth = 1. / tan(par[2][i]);
+ uu = xyz[0] * cos(par[3][i]) + xyz[1] * sin(par[3][i]);
+ vv = xyz[1] * cos(par[3][i]) - xyz[0] * sin(par[3][i]);
+ eps[i] = -vv + .5 * uu * uu * par[4][i];
+ zp[i] = xyz[2] - uu * cotth;
+ // * phi at vertex with these parameters
+ phiv[i] = par[3][i] + uu * par[4][i];
+ cosf = cos(phiv[i]);
+ sinf = sin(phiv[i]);
+ // *
+ // * contribution of this track to chi2 with initial values
+ // *
+ deps[i] = par[0][i] - eps[i];
+ dzp[i] = par[1][i] - zp[i];
+ chi2i = chi2i
+ + wgt[0][i]
+ * deps[i]
+ * deps[i]
+ + 2
+ * wgt[1][i]
+ * deps[i]
+ * dzp[i]
+ + wgt[2][i]
+ * dzp[i]
+ * dzp[i];
+ // *
+ // * derivatives (deriv1) of perigee param. w.r.t. x,y,z (vertex)
+ d11 = sinf;
+ d12 = -cosf;
+ d21 = -cosf * cotth;
+ d22 = -sinf * cotth;
+ d41 = -cosf * par[4][i];
+ d42 = -sinf * par[4][i];
+ // *
+ // * matrix dw = (deriv1)t * weight
+ dw11 = d11 * wgt[0][i] + d21 * wgt[1][i] + d41 * wgt[6][i];
+ dw12 = d11 * wgt[1][i] + d21 * wgt[2][i] + d41 * wgt[7][i];
+ dw13 = d11 * wgt[3][i] + d21 * wgt[4][i] + d41 * wgt[8][i];
+ dw14 = d11 * wgt[6][i] + d21 * wgt[7][i] + d41 * wgt[9][i];
+ dw15 = d11 * wgt[10][i] + d21 * wgt[11][i] + d41 * wgt[13][i];
+ dw21 = d12 * wgt[0][i] + d22 * wgt[1][i] + d42 * wgt[6][i];
+ dw22 = d12 * wgt[1][i] + d22 * wgt[2][i] + d42 * wgt[7][i];
+ dw23 = d12 * wgt[3][i] + d22 * wgt[4][i] + d42 * wgt[8][i];
+ dw24 = d12 * wgt[6][i] + d22 * wgt[7][i] + d42 * wgt[9][i];
+ dw25 = d12 * wgt[10][i] + d22 * wgt[11][i] + d42 * wgt[13][i];
+ dw31 = wgt[1][i];
+ dw32 = wgt[2][i];
+ dw33 = wgt[4][i];
+ dw34 = wgt[7][i];
+ dw35 = wgt[11][i];
+ // *
+ // * summation of dw * dpar to vector tv
+ tv[0] = tv[0] + dw11 * deps[i] + dw12 * dzp[i];
+ tv[1] = tv[1] + dw21 * deps[i] + dw22 * dzp[i];
+ tv[2] = tv[2] + dw31 * deps[i] + dw32 * dzp[i];
+ // *
+ // * derivatives (deriv2) of perigee param. w.r.t. theta,phi,1/r (vertex)
+ e12 = uu;
+ e13 = -.5 * uu * uu;
+ e21 = -uu * (1. + cotth * cotth);
+ e22 = -vv * cotth;
+ e23 = uu * vv * cotth;
+ e43 = -uu;
+ // *
+ // * matrix ew = (deriv2)t * weight
+ ew11 = e21 * wgt[1][i] + wgt[3][i];
+ ew12 = e21 * wgt[2][i] + wgt[4][i];
+ ew13 = e21 * wgt[4][i] + wgt[5][i];
+ ew14 = e21 * wgt[7][i] + wgt[8][i];
+ ew15 = e21 * wgt[11][i] + wgt[12][i];
+ ew21 = e12 * wgt[0][i] + e22 * wgt[1][i] + wgt[6][i];
+ ew22 = e12 * wgt[1][i] + e22 * wgt[2][i] + wgt[7][i];
+ ew23 = e12 * wgt[3][i] + e22 * wgt[4][i] + wgt[8][i];
+ ew24 = e12 * wgt[6][i] + e22 * wgt[7][i] + wgt[9][i];
+ ew25 = e12 * wgt[10][i] + e22 * wgt[11][i] + wgt[13][i];
+ ew31 = e13 * wgt[0][i] + e23 * wgt[1][i] + e43 * wgt[6][i] + wgt[10][i];
+ ew32 = e13 * wgt[1][i] + e23 * wgt[2][i] + e43 * wgt[7][i] + wgt[11][i];
+ ew33 = e13 * wgt[3][i] + e23 * wgt[4][i] + e43 * wgt[8][i] + wgt[12][i];
+ ew34 = e13 * wgt[6][i] + e23 * wgt[7][i] + e43 * wgt[9][i] + wgt[13][i];
+ ew35 = e13 * wgt[10][i] + e23 * wgt[11][i] + e43 * wgt[13][i] + wgt[14][i];
+ // *
+ // * computation of vector tt = ew * dpar
+ tt[0][i] = ew11 * deps[i] + ew12 * dzp[i];
+ tt[1][i] = ew21 * deps[i] + ew22 * dzp[i];
+ tt[2][i] = ew31 * deps[i] + ew32 * dzp[i];
+ // *
+ // * summation of (deriv1)t * weight * (deriv1) to matrix wa
+ wa[0] = wa[0] + dw11 * d11 + dw12 * d21 + dw14 * d41;
+ wa[1] = wa[1] + dw11 * d12 + dw12 * d22 + dw14 * d42;
+ wa[2] = wa[2] + dw21 * d12 + dw22 * d22 + dw24 * d42;
+ wa[3] = wa[3] + dw12;
+ wa[4] = wa[4] + dw22;
+ wa[5] = wa[5] + dw32;
+ // *
+ // * computation of matrix wb = (deriv1)t * weight * (deriv2)
+ wb[0][i] = dw12 * e21 + dw13;
+ wb[1][i] = dw22 * e21 + dw23;
+ wb[2][i] = dw32 * e21 + dw33;
+ wb[3][i] = dw11 * e12 + dw12 * e22 + dw14;
+ wb[4][i] = dw21 * e12 + dw22 * e22 + dw24;
+ wb[5][i] = dw31 * e12 + dw32 * e22 + dw34;
+ wb[6][i] = dw11 * e13 + dw12 * e23 + dw14 * e43 + dw15;
+ wb[7][i] = dw21 * e13 + dw22 * e23 + dw24 * e43 + dw25;
+ wb[8][i] = dw31 * e13 + dw32 * e23 + dw34 * e43 + dw35;
+ // *
+ // * computation of matrix wc = (deriv2)t * weight * (deriv2)
+ wc[0][i] = ew12 * e21 + ew13;
+ wc[1][i] = ew22 * e21 + ew23;
+ wc[3][i] = ew32 * e21 + ew33;
+ wc[2][i] = ew21 * e12 + ew22 * e22 + ew24;
+ wc[4][i] = ew31 * e12 + ew32 * e22 + ew34;
+ wc[5][i] = ew31 * e13 + ew32 * e23 + ew34 * e43 + ew35;
+ // *
+ // * computation of matrices wci = (wc)**(-1) and wbci = wb * wci
+
+ // Not very efficient here...
+ double[] tmp = new double[6];
+ for (int j = 0; j < 6; ++j) {
+ tmp[j] = wc[j][i];
+ System.out.println("tmp[" + j + "]= " + tmp[j]);
+ }
+ double[] tmpinv = new double[6];
+ tmpinv = pxmi3(tmp);
+ for (int j = 0; j < 6; ++j) {
+ wci[j][i] = tmpinv[j];
+ }
+ // wci[0][i] = pxmi3(wc[0][i]);
+
+ wbci[0][i] = wb[0][i] * wci[0][i] + wb[3][i] * wci[1][i] + wb[6][i] * wci[3][i];
+ wbci[1][i] = wb[1][i] * wci[0][i] + wb[4][i] * wci[1][i] + wb[7][i] * wci[3][i];
+ wbci[2][i] = wb[2][i] * wci[0][i] + wb[5][i] * wci[1][i] + wb[8][i] * wci[3][i];
+ wbci[3][i] = wb[0][i] * wci[1][i] + wb[3][i] * wci[2][i] + wb[6][i] * wci[4][i];
+ wbci[4][i] = wb[1][i] * wci[1][i] + wb[4][i] * wci[2][i] + wb[7][i] * wci[4][i];
+ wbci[5][i] = wb[2][i] * wci[1][i] + wb[5][i] * wci[2][i] + wb[8][i] * wci[4][i];
+ wbci[6][i] = wb[0][i] * wci[3][i] + wb[3][i] * wci[4][i] + wb[6][i] * wci[5][i];
+ wbci[7][i] = wb[1][i] * wci[3][i] + wb[4][i] * wci[4][i] + wb[7][i] * wci[5][i];
+ wbci[8][i] = wb[2][i] * wci[3][i] + wb[5][i] * wci[4][i] + wb[8][i] * wci[5][i];
+ // *
+ // * subtraction of wbci * (wb)t from matrix wa
+ wa[0] = wa[0] - wbci[0][i] * wb[0][i] - wbci[3][i] * wb[3][i] - wbci[6][i] * wb[6][i];
+ wa[1] = wa[1] - wbci[0][i] * wb[1][i] - wbci[3][i] * wb[4][i] - wbci[6][i] * wb[7][i];
+ wa[2] = wa[2] - wbci[1][i] * wb[1][i] - wbci[4][i] * wb[4][i] - wbci[7][i] * wb[7][i];
+ wa[3] = wa[3] - wbci[0][i] * wb[2][i] - wbci[3][i] * wb[5][i] - wbci[6][i] * wb[8][i];
+ wa[4] = wa[4] - wbci[1][i] * wb[2][i] - wbci[4][i] * wb[5][i] - wbci[7][i] * wb[8][i];
+ wa[5] = wa[5] - wbci[2][i] * wb[2][i] - wbci[5][i] * wb[5][i] - wbci[8][i] * wb[8][i];
+ // *
+ // * subtraction of wbci * tt from vector tv
+ tv[0] = tv[0] - wbci[0][i] * tt[0][i] - wbci[3][i] * tt[1][i] - wbci[6][i] * tt[2][i];
+ tv[1] = tv[1] - wbci[1][i] * tt[0][i] - wbci[4][i] * tt[1][i] - wbci[7][i] * tt[2][i];
+ tv[2] = tv[2] - wbci[2][i] * tt[0][i] - wbci[5][i] * tt[1][i] - wbci[8][i] * tt[2][i];
+ // *
+ }// check on whether to use track
+ }// loop over tracks
+
+ // beam constraint
+ if (fitwb) {
+ wa[0] += 1. / (sigxbe * sigxbe);
+ wa[2] += 1. / (sigxbe * sigybe);
+ wa[5] += 1. / (sigzbe * sigzbe);
+ tv[0] += (beamoy[0] - xyz[0]) / (sigxbe * sigxbe);
+ tv[1] += (beamoy[1] - xyz[1]) / (sigybe * sigybe);
+ tv[2] += (beamoy[2] - xyz[2]) / (sigzbe * sigzbe);
+ }
+ // *
+ // * solution of the linear system
+ // *
+ // * covariance matrix on vertex
+ vcov = pxmi3(wa);
+ for (int ii = 0; ii < vcov.length; ++ii) {
+ System.out.println("vcov[" + ii + "]= " + vcov[ii]);
+ }
+ //
+ // *
+ // * corrections to vertex coordinates
+ dxyz[0] = vcov[0] * tv[0] + vcov[1] * tv[1] + vcov[3] * tv[2];
+ dxyz[1] = vcov[1] * tv[0] + vcov[2] * tv[1] + vcov[4] * tv[2];
+ dxyz[2] = vcov[3] * tv[0] + vcov[4] * tv[1] + vcov[5] * tv[2];
+ xyzf[0] = xyz[0] + dxyz[0];
+ xyzf[1] = xyz[1] + dxyz[1];
+ xyzf[2] = xyz[2] + dxyz[2];
+ // *
+ // * corrections to track parameters and covariance matrices
+ // *
+ for (int i = 0; i < ntrk; ++i) {
+ if (invtx[i]) {
+ // *
+ // * variation on par is wci * tt - (wbci)t * dxyz
+ parf[0][i] = par[2][i]
+ + wci[0][i]
+ * tt[0][i]
+ + wci[1][i]
+ * tt[1][i]
+ + wci[3][i]
+ * tt[2][i]
+ - wbci[0][i]
+ * dxyz[0]
+ - wbci[1][i]
+ * dxyz[1]
+ - wbci[2][i]
+ * dxyz[2];
+ parf[1][i] = phiv[i]
+ + wci[1][i]
+ * tt[0][i]
+ + wci[2][i]
+ * tt[1][i]
+ + wci[4][i]
+ * tt[2][i]
+ - wbci[3][i]
+ * dxyz[0]
+ - wbci[4][i]
+ * dxyz[1]
+ - wbci[5][i]
+ * dxyz[2];
+ parf[2][i] = par[4][i]
+ + wci[3][i]
+ * tt[0][i]
+ + wci[4][i]
+ * tt[1][i]
+ + wci[5][i]
+ * tt[2][i]
+ - wbci[6][i]
+ * dxyz[0]
+ - wbci[7][i]
+ * dxyz[1]
+ - wbci[8][i]
+ * dxyz[2];
+ // *
+ // * covariance matrix of par is wci + (wbci)t * vcov * wbci
+ tcov[0][i] = wci[0][i]
+ + wbci[0][i]
+ * (vcov[0] * wbci[0][i] + vcov[1] * wbci[1][i] + vcov[3] * wbci[2][i])
+ + wbci[1][i]
+ * (vcov[1] * wbci[0][i] + vcov[2] * wbci[1][i] + vcov[4] * wbci[2][i])
+ + wbci[2][i]
+ * (vcov[3] * wbci[0][i] + vcov[4] * wbci[1][i] + vcov[5] * wbci[2][i]);
+
+ tcov[1][i] = wci[1][i]
+ + wbci[0][i]
+ * (vcov[0] * wbci[3][i] + vcov[1] * wbci[4][i] + vcov[3] * wbci[5][i])
+ + wbci[1][i]
+ * (vcov[1] * wbci[3][i] + vcov[2] * wbci[4][i] + vcov[4] * wbci[5][i])
+ + wbci[2][i]
+ * (vcov[3] * wbci[3][i] + vcov[4] * wbci[4][i] + vcov[5] * wbci[5][i]);
+
+ tcov[2][i] = wci[2][i]
+ + wbci[3][i]
+ * (vcov[0] * wbci[3][i] + vcov[1] * wbci[4][i] + vcov[3] * wbci[5][i])
+ + wbci[4][i]
+ * (vcov[1] * wbci[3][i] + vcov[2] * wbci[4][i] + vcov[4] * wbci[5][i])
+ + wbci[5][i]
+ * (vcov[3] * wbci[3][i] + vcov[4] * wbci[4][i] + vcov[5] * wbci[5][i]);
+
+ tcov[3][i] = wci[3][i]
+ + wbci[0][i]
+ * (vcov[0] * wbci[6][i] + vcov[1] * wbci[7][i] + vcov[3] * wbci[8][i])
+ + wbci[1][i]
+ * (vcov[1] * wbci[6][i] + vcov[2] * wbci[7][i] + vcov[4] * wbci[8][i])
+ + wbci[2][i]
+ * (vcov[3] * wbci[6][i] + vcov[4] * wbci[7][i] + vcov[5] * wbci[8][i]);
+
+ tcov[4][i] = wci[4][i]
+ + wbci[3][i]
+ * (vcov[0] * wbci[6][i] + vcov[1] * wbci[7][i] + vcov[3] * wbci[8][i])
+ + wbci[4][i]
+ * (vcov[1] * wbci[6][i] + vcov[2] * wbci[7][i] + vcov[4] * wbci[8][i])
+ + wbci[5][i]
+ * (vcov[3] * wbci[6][i] + vcov[4] * wbci[7][i] + vcov[5] * wbci[8][i]);
+
+ tcov[5][i] = wci[5][i]
+ + wbci[6][i]
+ * (vcov[0] * wbci[6][i] + vcov[1] * wbci[7][i] + vcov[3] * wbci[8][i])
+ + wbci[7][i]
+ * (vcov[1] * wbci[6][i] + vcov[2] * wbci[7][i] + vcov[4] * wbci[8][i])
+ + wbci[8][i]
+ * (vcov[3] * wbci[6][i] + vcov[4] * wbci[7][i] + vcov[5] * wbci[8][i]);
+
+ // *
+ } // check on whether to use track
+ } // loop over tracks
+
+ // *
+ // * chi2 with fitted values
+ // *
+ chi2 = 0.;
+ if (fitwb)
+ chi2 += ( (xyzf[0] - beamoy[0]) / sigxbe)
+ * ( (xyzf[0] - beamoy[0]) / sigxbe)
+ + ( (xyzf[1] - beamoy[1]) / sigybe)
+ * ( (xyzf[1] - beamoy[1]) / sigybe)
+ + ( (xyzf[2] - beamoy[2]) / sigzbe)
+ * ( (xyzf[2] - beamoy[2]) / sigzbe);
+
+ for (int i = 0; i < ntrk; ++i) {
+ if (invtx[i]) {
+ uu = xyzf[0] * cos(parf[1][i]) + xyzf[1] * sin(parf[1][i]);
+ vv = xyzf[1] * cos(parf[1][i]) - xyzf[0] * sin(parf[1][i]);
+ epsf = -vv - .5 * uu * uu * parf[2][i];
+ zpf = xyzf[2] - uu / tan(parf[0][i]);
+ phif = parf[1][i] - uu * parf[2][i];
+ // not very efficient here...
+ double[] tmp = new double[15];
+ for (int j = 0; j < 15; ++j) {
+ tmp[j] = wgt[j][i];
+ }
+ chi2tr[i] = fwdch2(tmp, epsf - par[0][i], zpf - par[1][i], parf[0][i] - par[2][i], phif - par[3][i],
+ parf[2][i] - par[4][i]);
+ chi2 += chi2tr[i];
+ }
+ }
+ System.out.println("chi2= " + chi2);
+ return new Vertex(ntrk, xyzf, parf, vcov, tcov, chi2, chi2tr);
+
+ }
+
+}
lcsim/test/org/lcsim/recon/vertexing/zvtop4/fitter
diff -N VertexFitterTest.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ VertexFitterTest.java 20 Mar 2006 07:42:05 -0000 1.1
@@ -0,0 +1,132 @@
+/**
+ *
+ */
+package org.lcsim.recon.vertexing.zvtop4.fitter;
+
+import Jama.Matrix;
+import java.io.File;
+import java.net.URL;
+import java.util.ArrayList;
+import java.util.List;
+
+import org.lcsim.event.EventHeader;
+import org.lcsim.event.Track;
+import org.lcsim.mc.fast.MCFast;
+import org.lcsim.util.Driver;
+import org.lcsim.util.aida.AIDA;
+import org.lcsim.util.cache.FileCache;
+import org.lcsim.util.loop.LCSimLoop;
+
+import junit.framework.TestCase;
+import static java.lang.Math.PI;
+import static java.lang.Math.atan;
+
+/**
+ * @author jstrube
+ *
+ */
+
+public class VertexFitterTest extends TestCase {
+ private Track track1;
+ private Track track2;
+ private LCSimLoop loop;
+ private URL location;
+ private FileCache cache;
+ private File trackFile;
+ private VertexFitter fitter;
+ private AIDA aida = AIDA.defaultInstance();
+ protected void setUp() throws Exception {
+ super.setUp();
+ loop = new LCSimLoop();
+ // contains psi- mu mu events
+ location = new URL("ftp://ftp-lcd.slac.stanford.edu/lcd/ILC/ILC500/Zgamma/stdhep/pythia/pythiaZgamma.stdhep");
+ cache = new FileCache();
+ trackFile = cache.getCachedFile(location);
+ loop.setStdhepRecordSource(trackFile, "sidaug05");
+ fitter = new VertexFitter(5);
+ }
+
+ public void testFit() throws org.freehep.record.loop.LoopException, java.io.IOException {
+ loop.add(new MCFast());
+ loop.add(new TrackFitDriver());
+ loop.loop(-1);
+ }
+ private class TrackFitDriver extends Driver {
+ private List<double[]> weights = new ArrayList<double[]>();
+ private List<double[]> trackParams = new ArrayList<double[]>();
+ public void process(EventHeader event) {
+ trackParams.clear();
+ weights.clear();
+ List<Track> tracks = event.getTracks();
+ for (Track iTrack : tracks) {
+ Matrix olderrors = new Matrix(iTrack.getErrorMatrix());
+ olderrors.print(5, 3);
+ double[] params = iTrack.getTrackParameters();
+ System.err.println();
+ Matrix jacobi = new Matrix(getJacobi(params));
+ double theta = PI/2 - atan(params[4]);
+ double[] newparams = new double[]{params[0], params[3], theta, params[1], params[2]};
+ trackParams.add(newparams);
+ double[][] weightArray = jacobi.arrayTimes(olderrors).times(jacobi.transpose()).getArray();
+ weights.add(flattenWeights(weightArray));
+ }
+ System.err.println("fitting");
+ boolean[] inVtx = new boolean[] {true, true};
+ double[] initialPosition = new double[] {0, 0, 0};
+
+ for (double[] x : trackParams) {
+ System.err.print("Track Parameters: ");
+ for (double y : x) {
+ System.err.printf("%f\t", y);
+ }
+ System.err.println();
+ }
+ System.err.printf("Check: Parameter 4 of track 2: %.3f\n", trackParams.toArray(new double[][]{})[1][3]);
+// for (double[] x : trackParams.toArray(new double[][]{})) {
+// System.err.print("Track Parameters: ");
+// for (double y : x) {
+// System.err.printf("%f\t", y);
+// }
+// System.err.println();
+// }
+
+ for (double[] x : weights) {
+ System.err.print("Track weights: ");
+ for (double y : x) {
+ System.err.printf("%f\t", y);
+ }
+ System.err.println();
+ }
+ Matrix newTrackParams = new Matrix(trackParams.toArray(new double[][]{}));
+ Matrix newWeights = new Matrix(weights.toArray(new double[][]{}));
+ Vertex v = fitter.fit(tracks.size(), false, inVtx, newTrackParams.transpose().getArray(), newWeights.transpose().getArray(), initialPosition);
+ aida.histogram1D("vtx_chi2").fill(v._chi2);
+ }
+
+ private double[][] getJacobi(double[] old) {
+ double[][] jacobi = new double[][]{
+ {old[0], 0, 0, 0, 0}
+ , {0, 0, 0, old[1], 0}
+ , {0, 0, 0, 0, old[2]}
+ , {0, old[3], 0, 0, 0}
+ , {0, 0, 1/(Math.cos(old[4])*Math.cos(old[4])), 0, 0}};
+ return jacobi;
+ }
+
+ private double[] flattenWeights(double[][] weightMatrix) {
+ double[] result = new double[15];
+ int count = 0;
+ for (int i=0; i<5; ++i)
+ for (int j=i; j<5; ++j)
+ result[count++] = weightMatrix[i][j];
+ return result;
+ }
+ }
+
+
+ protected void tearDown() throws Exception {
+ super.tearDown();
+ aida.saveAs("x.aida");
+ loop.dispose();
+ }
+}