10 added files
lcsim/src/org/lcsim/contrib/KFFitter
diff -N History
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ History 13 Dec 2006 22:45:11 -0000 1.1
@@ -0,0 +1,16 @@
+## History file
+## Kalman Filter Fitter (KFFitter)
+## Please summarize changes to the KFFitter code here.
+## Most recent first please.
+
+
+13 December 2006 Fred Blanc V01-00-00
+ - Create KFFitter package. Included files:
+ SODFittedCir.java
+ - KFFitterDriver.java is the main driver
+ - Example code is given in test/TestKFF.java
+ - The default functionality is: 1) loop over tracks reconstructed
+ with the SODTrackFinder package, 2) apply the Kalman Filter
+ at sites defined as measurement points, or material points
+ (material points are not yet used in the first version of the
+ code).
lcsim/src/org/lcsim/contrib/KFFitter
diff -N KFConstants.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ KFConstants.java 13 Dec 2006 22:45:12 -0000 1.1
@@ -0,0 +1,22 @@
+package org.lcsim.contrib.KFFitter;
+
+public class KFConstants
+{
+
+ public KFConstants()
+ {
+ }
+
+ public double Pi() {return pi;}
+ public double twoPi() {return twopi;}
+ public double vtxResolution() {return vtxres;}
+ public double sodResolution() {return sodres;}
+ public double pointThreeTimesB() {return ptconst;}
+
+ private static double pi = 3.14159265359;
+ private static double twopi = 6.283185307;
+ private static double vtxres = 0.0005;
+ private static double sodres = 0.0007;
+ private static double ptconst = 0.015; // = 0.3*B
+
+}
lcsim/src/org/lcsim/contrib/KFFitter
diff -N KFFitterDriver.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ KFFitterDriver.java 13 Dec 2006 22:45:12 -0000 1.1
@@ -0,0 +1,93 @@
+package org.lcsim.contrib.KFFitter;
+
+import java.util.*;
+import java.io.*;
+import org.lcsim.event.EventHeader;
+import org.lcsim.util.Driver;
+import org.lcsim.event.EventHeader.LCMetaData;
+import org.lcsim.event.TrackerHit;
+import org.lcsim.event.SimTrackerHit;
+import org.lcsim.event.MCParticle;
+import org.lcsim.geometry.TrackerIDDecoder;
+import org.lcsim.util.aida.AIDA;
+
+import org.lcsim.contrib.SODTracker.SODTrackFinderDriver;
+import org.lcsim.contrib.SODTracker.SODTrack;
+
+import org.lcsim.contrib.KFFitter.KFTrack;
+
+public class KFFitterDriver extends Driver
+{
+ private AIDA aida = AIDA.defaultInstance();
+ public KFFitterDriver()
+ {
+ add(new SODTrackFinderDriver());
+ }
+
+ public void process(EventHeader event)
+ {
+ super.process(event); // this takes care that the child Drivers are loaded and processed.
+
+ List<SODTrack> SODTrackList = (List<SODTrack>) event.get("SODTracks");
+ for ( SODTrack SODtrk : SODTrackList ) {
+
+ double trkPT = SODtrk.getPT();
+ System.out.print("Driver: trk pT = "); System.out.println(trkPT);
+ System.out.print("Driver: SOD track = ");
+ System.out.print(SODtrk.getTrackParameter(0)); System.out.print(" ");
+ System.out.print(SODtrk.getTrackParameter(1)); System.out.print(" ");
+ System.out.print(SODtrk.getTrackParameter(2)); System.out.print(" ");
+ System.out.print(SODtrk.getTrackParameter(3)); System.out.print(" ");
+ System.out.print(SODtrk.getTrackParameter(4)); System.out.print(" ");
+ System.out.println(SODtrk.getCharge());
+
+ KFTrack track = new KFTrack();
+ track.defineSeedTrack(SODtrk);
+
+ List<SimTrackerHit> hits = (List<SimTrackerHit>) SODtrk.getHits();
+ int nbHits = hits.size();
+ System.out.print("Driver: SOD hit list size = "); System.out.println(nbHits);
+
+ double doca = 0.1*SODtrk.getTrackParameter(0);
+ System.out.print("Driver: SOD doca = "); System.out.print(nbHits); System.out.println(" cm");
+
+ double chi2 = SODtrk.getChi2();
+ System.out.print("Driver: SOD chi2 = "); System.out.print(chi2);
+ aida.cloud1D("chi2").fill(chi2);
+ double ndf = nbHits;
+ if(ndf>0.) { double r = chi2/ndf; aida.cloud1D("chi2 per ndf").fill(r); }
+
+ //if(Math.abs(doca)<0.01) {
+ if(chi2<100.) {
+ for ( SimTrackerHit hit : hits ) {
+ track.addHit(hit);
+ }
+ System.out.print("Driver: track params before fit = ");
+ track.getTrackParameters().printFitParams();
+ track.fit();
+ System.out.println();
+
+ double KFpt = track.getFittedPtAtPOCA();
+
+ // fill histograms
+ aida.cloud1D("KFpt all").fill(KFpt);
+ aida.cloud1D("SODpt all").fill(trkPT);
+ aida.cloud2D("pt (helix vs KF) all").fill(KFpt,trkPT);
+
+ if(Math.abs(KFpt-20.0)<0.1) {
+ aida.cloud1D("KFpt").fill(track.getFittedPtAtPOCA());
+ }
+ if(Math.abs(trkPT-20.0)<0.1) {
+ aida.cloud1D("SODpt").fill(trkPT);
+ }
+ if(Math.abs(KFpt-20.0)<0.1&&Math.abs(trkPT-20.0)<0.1) {
+ aida.cloud2D("pt (helix vs KF)").fill(KFpt,trkPT);
+ }
+
+ } else {
+ System.out.println("Driver: skip this track");
+ System.out.println();
+ }
+ }
+ }
+}
lcsim/src/org/lcsim/contrib/KFFitter
diff -N KFMain.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ KFMain.java 13 Dec 2006 22:45:12 -0000 1.1
@@ -0,0 +1,96 @@
+package org.lcsim.contrib.KFFitter;
+
+import java.util.*;
+import org.lcsim.util.Driver;
+import org.lcsim.event.*;
+
+import org.lcsim.util.loop.*;
+import java.io.File;
+
+public class KFMain extends Driver
+{
+ private int nEvents = 0;
+
+ public KFMain() throws Exception
+ {
+ this(new String[0]);
+ }
+
+ public KFMain(String[] args) throws Exception
+ {
+ add(new KFFitterDriver());
+ }
+
+ public void process(EventHeader event)
+ {
+ super.process(event);
+ if (nEvents%10==0) {
+ System.out.println("done processing event "+nEvents);
+ }
+ ++nEvents;
+ }
+
+ public void mainCopy(String[] args) throws Exception
+ {
+ if (args.length >= 1 &&
+ args[0].matches("^\\s*[/\\.\\w-_\\+]+\\.slcio")) {
+ KFMain driver = this;
+ convert(args[0],driver);
+ }
+ else {
+ System.out.println("usage: java KFMain <input.slcio> <nb events>");
+ return;
+ }
+ }
+
+ public static void main(String[] args) throws Exception
+ {
+ if (args.length >= 1 &&
+ args[0].matches("^\\s*[/\\.\\w-_\\+]+\\.slcio")) {
+ KFMain driver = new KFMain(args);
+ int nbEventsToProcess = -1;
+ if (args.length == 2 && args[1].matches("[0-9]+")) { // matches a number at least 1 time
+ nbEventsToProcess = Integer.parseInt(args[1]);
+ System.out.print("Processing "); System.out.print(nbEventsToProcess); System.out.println(" events");
+ }
+ convert(args[0],driver,nbEventsToProcess);
+ }
+ else {
+ System.out.println("usage: java KFMain <input.slcio> <nb events>");
+ return;
+ }
+ }
+
+ private static void convert(String iFile) throws Exception
+ {
+ convert(iFile, new KFMain());
+ }
+
+ private static void convert(String iFile, KFMain driver) throws Exception
+ {
+ int nLoop = -1;
+ convert(iFile, new KFMain(), nLoop);
+ }
+
+ private static void convert(String iFile, KFMain driver, int nbLoops) throws Exception
+ {
+ // Setup LCSim event loop
+ LCSimLoop loop = new LCSimLoop();
+
+ File input = new File(iFile);
+
+ loop.setLCIORecordSource(input);
+ loop.add(driver);
+ // loop.add(new LCIODriver(output));
+
+ loop.loop(nbLoops);
+ loop.dispose();
+
+ System.out.println("Processed "+driver.getNEvents()+" events.");
+ }
+
+ public int getNEvents()
+ {
+ return nEvents;
+ }
+}
lcsim/src/org/lcsim/contrib/KFFitter
diff -N KFMatrix.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ KFMatrix.java 13 Dec 2006 22:45:12 -0000 1.1
@@ -0,0 +1,27 @@
+package org.lcsim.contrib.KFFitter;
+
+//import org.lcsim.spacegeom.Matrix;
+import Jama.Matrix;
+
+public class KFMatrix
+{
+
+ public KFMatrix(int n)
+ {
+ _order = n;
+ }
+
+ public void invert()
+ {
+ }
+
+ public KFMatrix multiply(KFMatrix mat2)
+ {
+ KFMatrix prod = new KFMatrix(5);
+ return prod;
+ }
+
+ private int _order;
+ private Matrix _matrix;
+
+}
lcsim/src/org/lcsim/contrib/KFFitter
diff -N KFPoint.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ KFPoint.java 13 Dec 2006 22:45:12 -0000 1.1
@@ -0,0 +1,89 @@
+package org.lcsim.contrib.KFFitter;
+
+import Jama.Matrix;
+
+public class KFPoint
+{
+
+ public KFPoint()
+ {
+ _x = 0.;
+ _y = 0.;
+ _z = 0.;
+ _r = 0.;
+ _phi = 0.;
+
+ _rphiErr = 0.;
+ _zErr = 0.;
+
+ }
+
+ public void makePoint(double X, double Y, double Z)
+ {
+ _x = X;
+ _y = Y;
+ _z = Z;
+
+ _r = Math.sqrt(_x*_x+_y*_y);
+ //System.out.print("KFPoint: hit radius = "); System.out.println(_r);
+ _phi = Math.atan2(_y,_x);
+
+ _rphiErr = 0.0007; if(_r<10.){_rphiErr = 0.0005;}
+ _zErr = 0.005; //if(_r>10.){_zErr = 1000.;}
+
+ //System.out.print("KFPoint: phi, rphiErr = ");
+ //System.out.print(_phi); System.out.print(" ");
+ //System.out.println(_rphiErr);
+
+ }
+
+ public double getX() { return _x; }
+ public double getY() { return _y; }
+ public double getZ() { return _z; }
+ public double getR() { return _r; }
+ public double getPhi() { return _phi; }
+
+ public double getRPhiErr() { return _rphiErr;}
+ public double getZErr() { return _zErr;}
+
+ public Matrix getPhiZErrorMatrix()
+ {
+ double[][] vals = {{_rphiErr*_rphiErr/(_r*_r),0.},{0.,_zErr*_zErr}};
+ //double[][] vals = {{_rphiErr*_rphiErr,0.},{0.,_zErr*_zErr}};
+ Matrix errMat = new Matrix(vals);
+ return errMat;
+ }
+
+ public Matrix getXYZErrorMatrix()
+ {
+ double s2 = _rphiErr*_rphiErr;
+
+ double xx = Math.sin(_phi)*Math.sin(_phi)*s2;
+ double xy = -Math.sin(_phi)*Math.cos(_phi)*s2;
+ double yx = xy;
+ double yy = Math.cos(_phi)*Math.cos(_phi)*s2;
+
+ double zz = _zErr*_zErr;
+
+ //System.out.print("KFPoint: calc Cov = ");
+ //System.out.print(_phi); System.out.print(" ");
+ //System.out.print(_rphiErr); System.out.print(" ");
+ //System.out.print(xx); System.out.print(" ");
+ //System.out.println(xy);
+
+ double[][] vals = {{xx,xy,0.},{yx,yy,0.},{0.,0.,zz}};
+ Matrix errMat = new Matrix(vals);
+ //System.out.println("KFPoint: XYZ cov matrix");
+ //errMat.print(6,10);
+ return errMat;
+ }
+
+ private double _x;
+ private double _y;
+ private double _z;
+
+ private double _r;
+ private double _phi;
+ private double _rphiErr;
+ private double _zErr;
+}
lcsim/src/org/lcsim/contrib/KFFitter
diff -N KFSite.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ KFSite.java 13 Dec 2006 22:45:12 -0000 1.1
@@ -0,0 +1,162 @@
+package org.lcsim.contrib.KFFitter;
+
+import org.lcsim.event.SimTrackerHit;
+import org.lcsim.event.TrackerHit;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import Jama.Matrix;
+
+public class KFSite
+{
+
+ public KFSite()
+ {
+ _siteType = 1; // 1=hit
+ //_siteType = 2; // 2=material
+ //_siteType = 3; // 3=B field discontinuity
+ }
+
+ public void setLocation(SimTrackerHit hit)
+ {
+ double[] pos = hit.getPoint();
+ // convert to cm
+ for(int i=0; i<3; i++) {pos[i]*=0.1;}
+ _siteLocation.makePoint(pos[0],pos[1],pos[2]);
+
+ System.out.print("KFSite: Add Pt at coord = ");
+ System.out.print(pos[0]); System.out.print(" ");
+ System.out.print(pos[1]); System.out.print(" ");
+ System.out.println(pos[2]);
+
+ //System.out.print("KFSite: read back radius = ");
+ //System.out.println(_siteLocation.getR());
+
+ //double rphierr = _siteLocation.getRPhiErr();
+ //double zerr = _siteLocation.getZErr();
+ //double[][] vals = {{rphierr,0.},{0.,zerr}};
+ _PhiZErrorMatrix = _siteLocation.getPhiZErrorMatrix();
+
+ System.out.println("KFSite: PhiZ cov matrix:");
+ _PhiZErrorMatrix.print(12,10);
+
+ _PhiZErrorMatrixInverse = _PhiZErrorMatrix.inverse();
+
+ System.out.println("KFSite: PhiZ cov matrix inverted:");
+ _PhiZErrorMatrixInverse.print(12,10);
+
+ }
+
+ public void setTrackParOut(KFTrackParameters trk)
+ {
+ _trackParamOutward = trk;
+ }
+
+ public void setTrackParIn(KFTrackParameters trk)
+ {
+ _trackParamInward = trk;
+ }
+
+ public KFTrackParameters filter(KFTrackParameters trk)
+ {
+
+ // define matrix relating the measured parameters (phi,z) at radius R and the
+ // state vector (fit parameters). In the current setup, the state vector is (phi,z,px,py,pz)
+ double[][] vals = {{1.,0.,0.,0.,0.},{0.,1.,0.,0.,0.}};
+ Matrix H = new Matrix(vals);
+
+ Matrix HT = H.transpose();
+ System.out.println("KFSite: H:");
+ H.print(12,10);
+ System.out.println("KFSite: HT:");
+ HT.print(12,10);
+
+ Matrix trkCov = trk.getCovMatrixAtRadiusR();
+
+ System.out.println("KFSite: trkCov:");
+ trkCov.print(15,4);
+
+ Matrix trkCovInverse = trkCov.inverse();
+
+ System.out.println("KFSite: trkCovInverse:");
+ trkCovInverse.print(15,4);
+
+ // Check matrix inversion
+ Matrix checkInv = trkCov.times(trkCovInverse);
+ System.out.println("KFSite: trkCov*trkCovInverse:");
+ checkInv.print(15,4);
+
+ double[] tpp = trk.getPFitParameters();
+ Matrix stateVect = new Matrix(tpp,5);
+
+ System.out.println("KFSite: stateVect:");
+ stateVect.print(12,10);
+
+ //double[] mp = {_siteLocation.getPhi()*_siteLocation.getR(),_siteLocation.getZ()};
+ double[] mp = {_siteLocation.getPhi(),_siteLocation.getZ()};
+ //double[] mp = {_siteLocation.getPhi(),0.}; // fix me
+ Matrix measVect = new Matrix(mp,2);
+
+ System.out.println("KFSite: measVect:");
+ measVect.print(12,10);
+
+ Matrix cv1 = _PhiZErrorMatrixInverse.times(H);
+ Matrix cv2 = HT.times(cv1);
+ Matrix filteredCovInv = trkCovInverse.plus(cv2);
+ Matrix filteredCov = filteredCovInv.inverse();
+
+ System.out.println("KFSite: Filtered Cov:");
+ filteredCov.print(15,12);
+
+ Matrix tmp1 = trkCovInverse.times(stateVect);
+ Matrix tmp2 = HT.times(_PhiZErrorMatrixInverse).times(measVect);
+ Matrix tmp3 = tmp1.plus(tmp2);
+ Matrix filteredTrkPar = filteredCov.times(tmp3);
+
+ System.out.println("KFSite: Filtered State Vector:");
+ filteredTrkPar.print(6,6);
+
+ double[][] par = filteredTrkPar.getArrayCopy();
+ int charge = trk.getCharge();
+ double phi = par[0][0];
+ double x1 = _siteLocation.getR()*Math.cos(phi);
+ double x2 = _siteLocation.getR()*Math.sin(phi);
+ double x3 = par[1][0];
+ double p1 = par[2][0];
+ double p2 = par[3][0];
+ double p3 = par[4][0];
+
+ System.out.print("KFSite: Fitted Pt = "); System.out.println(Math.sqrt(p1*p1+p2*p2));
+
+ //double p1 = par[0][0];
+ //double p2 = par[1][0];
+ //double p3 = par[2][0];
+ //double p4 = par[3][0];
+ //double p5 = par[4][0];
+
+ KFTrackParameters filtTrk = new KFTrackParameters();
+
+ //filtTrk.setTrackFitParameters(charge,p1,p2,p3,p4,p5);
+ //filtTrk.setPointToRadius(_siteLocation.getR());
+ filtTrk.setTrackPointParameters(charge,x1,x2,x3,p1,p2,p3);
+ filtTrk.setErrorMatrixAtRadiusR(filteredCov);
+
+ return filtTrk;
+ }
+
+ public KFPoint getSiteLocation() {return _siteLocation;}
+
+ private KFTrackParameters _trackParamOutward;
+ private KFTrackParameters _trackParamOutwardCombined;
+ private KFTrackParameters _trackParamInward;
+ private KFTrackParameters _trackParamInwardCombined;
+
+ private KFPoint _siteLocation = new KFPoint();
+
+ private Matrix _PhiZErrorMatrix = new Matrix(3,3);
+ private Matrix _PhiZErrorMatrixInverse = new Matrix(3,3);
+
+ private int _siteType;
+
+}
lcsim/src/org/lcsim/contrib/KFFitter
diff -N KFTrack.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ KFTrack.java 13 Dec 2006 22:45:12 -0000 1.1
@@ -0,0 +1,163 @@
+package org.lcsim.contrib.KFFitter;
+
+import org.lcsim.event.Track;
+import org.lcsim.event.SimTrackerHit;
+import org.lcsim.event.TrackerHit;
+import org.lcsim.event.MCParticle;
+import org.lcsim.contrib.SODTracker.SODTrack;
+
+import java.util.ArrayList;
+import java.util.*;
+
+import Jama.Matrix;
+
+public class KFTrack
+{
+ public KFTrack()
+ {
+ _siteList = new java.util.LinkedList();
+ }
+
+ public void addHit(SimTrackerHit hit)
+ {
+ // define site for this hit
+ KFSite kfhit = new KFSite();
+ kfhit.setLocation(hit);
+ _siteList.add(kfhit);
+ //System.out.print("KFTrack: read back radius = ");
+ //System.out.println(kfhit.getSiteLocation().getR());
+
+ }
+
+ public void defineSeedTrack(SODTrack seedTrack)
+ {
+ // load track parameters d0,omega,...
+ double d = seedTrack.getTrackParameter(0)/10.; // divided by 10 to get cm
+ double phi = seedTrack.getTrackParameter(1);
+ double omega = seedTrack.getTrackParameter(2)*(-10.); // mult. by -10 to get cm-1
+ //omega = 2.*omega; // fix me
+ double z = seedTrack.getTrackParameter(3)/10.; // divided by 10 to get cm
+ double s = seedTrack.getTrackParameter(4);
+ //double z = 0.; // fix me
+ //double s = 0.; // fix me
+
+ int charge = 1; if(omega>0.) {charge = -1;}
+
+ System.out.print("KFTrack: define track par = ");
+ System.out.print(d); System.out.print(" ");
+ System.out.print(phi); System.out.print(" ");
+ System.out.print(omega); System.out.print(" ");
+ System.out.print(z); System.out.print(" ");
+ System.out.println(s);
+
+ _seedTrack.setTrackFitParameters(charge,d,phi,omega,z,s);
+
+ }
+
+ public void fit()
+ {
+ // initialize track param covariance matrix
+ double[][] diag = new double[5][5];
+ diag[0][0] = 0.1;
+ diag[1][1] = 10.;
+ for(int i=2; i<5; i++) {diag[i][i] = 100.;}
+ Matrix initCov = new Matrix(diag);
+ System.out.println("KFTrack: init cov matrix");
+ initCov.print(6,2);
+ //_seedTrack.setErrorMatrixAtRadiusR(initCov);
+
+ System.out.print("KFTrack: ");
+ _seedTrack.printFitParams();
+
+ // initialize track parameters for outward fit
+ KFTrackParameters currentTrackOut = new KFTrackParameters(_seedTrack);
+ // determine track params at fixed small radius
+ //_seedTrack.setPointToPOCA();
+ double refRadius = 1.0;
+ currentTrackOut.setPointToRadius(refRadius);
+ // initialize error matrix
+ currentTrackOut.setErrorMatrixAtRadiusR(initCov);
+
+
+ // initialize track parameters for inward fit
+ KFTrackParameters currentTrackIn = new KFTrackParameters(_seedTrack);
+ // determine track params at fixed small radius
+ refRadius = 130.0;
+ currentTrackIn.setPointToRadius(refRadius);
+ // initialize error matrix
+ currentTrackIn.setErrorMatrixAtRadiusR(initCov);
+
+ ListIterator hits = _siteList.listIterator();
+
+ //***************
+ // Fit inside-out
+ //***************
+ System.out.println("KFTrack: Fitting Track outward");
+
+ for (; hits.hasNext();){
+
+ KFSite hit = (KFSite)hits.next();
+ KFPoint siteLocation = new KFPoint();
+ siteLocation = hit.getSiteLocation();
+ double R = siteLocation.getR();
+ System.out.print("KFTrack: hit radius = "); System.out.println(R);
+
+ System.out.print("KFTrack: current pars = ");
+ currentTrackOut.printPFitParams();
+
+ // propagate track to radius of hit
+ KFTrackParameters predictedTrack = currentTrackOut.makePredictionAtRadius(R);
+
+ System.out.print("KFTrack: predicted pars = ");
+ predictedTrack.printPFitParams();
+
+ // filter predicted track parameters with hit at radius R
+ currentTrackOut = hit.filter(predictedTrack);
+
+ // record track parameters in KFSite (fix me)
+
+ }
+
+ //***************
+ // Fit outside-in
+ //***************
+ System.out.println("KFTrack: Fitting Track inward");
+
+ for (; hits.hasPrevious();){
+
+ KFSite hit = (KFSite)hits.previous();
+ KFPoint siteLocation = new KFPoint();
+ siteLocation = hit.getSiteLocation();
+ double R = siteLocation.getR();
+ System.out.print("KFTrack: hit radius = "); System.out.println(R);
+
+ System.out.print("KFTrack: current pars = ");
+ currentTrackIn.printPFitParams();
+
+ // propagate track to radius of hit
+ KFTrackParameters predictedTrack = currentTrackIn.makePredictionAtRadius(R);
+
+ System.out.print("KFTrack: predicted pars = ");
+ predictedTrack.printPFitParams();
+
+ // filter predicted track parameters with hit at radius R
+ currentTrackIn = hit.filter(predictedTrack);
+
+ // record track parameters in KFSite (fix me)
+
+ }
+
+ _fittedPtAtPOCA = currentTrackIn.getPt();
+ System.out.print("KFTrack: Final fitted pt = ");
+ System.out.println(_fittedPtAtPOCA);
+ }
+
+ public KFTrackParameters getTrackParameters() { return _seedTrack; }
+ public double getFittedPtAtPOCA() { return _fittedPtAtPOCA; }
+
+ private java.util.LinkedList _siteList;
+ private KFTrackParameters _seedTrack = new KFTrackParameters();
+
+ private double _fittedPtAtPOCA;
+
+}
lcsim/src/org/lcsim/contrib/KFFitter
diff -N KFTrackParameters.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ KFTrackParameters.java 13 Dec 2006 22:45:12 -0000 1.1
@@ -0,0 +1,417 @@
+package org.lcsim.contrib.KFFitter;
+
+import Jama.Matrix;
+
+public class KFTrackParameters
+{
+
+ KFTrackParameters()
+ {
+
+ }
+
+ KFTrackParameters(KFTrackParameters trk)
+ {
+ double[] par = trk.getFitParameters();
+ int chg = trk.getCharge();
+ this.setTrackFitParameters(chg, par[0], par[1], par[2], par[3], par[4]);
+
+ double[] ptp = trk.getPointParameters();
+ for(int i=0; i<6; i++) { this._pointParams[i] = ptp[i]; }
+ _x = ptp[0]; _y = ptp[1]; _z = ptp[2]; _px = ptp[3]; _py = ptp[4]; _pz = ptp[5];
+ _r = Math.sqrt(_x*_x+_y*_y);
+ //_phi = 0.; if(_x!=0.&&_y!=0) { _phi = _r*Math.atan2(_y,_x); }
+ _phi = 0.; if(_x!=0.&&_y!=0) { _phi = Math.atan2(_y,_x); }
+ this._pFitParams[0] = _phi;
+ for(int i=2; i<6; i++) { this._pFitParams[i-1] = ptp[i]; }
+
+ this._FitErrorMatrix = trk.getFitCovMatrix();
+ this._ErrorMatrixAtRadiusR = trk.getCovMatrixAtRadiusR();
+ this._ErrorMatrixAtRadiusRprime = trk.getCovMatrixAtRadiusRprime();
+ }
+
+ private int _charge = 0;
+
+ private double _d0 = 0., _phi0 = 0., _omega = 0., _z0 = 0., _s = 0.;
+ private double[] _fitParams = new double[] {0.,0.,0.,0.,0.};
+
+ private double _p = 0., _pt = 0., _px0 = 0., _py0 = 0., _pz0 = 0.;
+ private double _x0 = 0., _y0 = 0.;
+ private double[] _pointOriginParams = new double[] {0.,0.,0.,0.,0.,0.};
+
+ private double _xc = 0., _yc = 0., _rc = 0.;
+ private double[] _helixParams = new double[] {0.,0.,0.,0.,0.};
+
+ private double _px = 0., _py = 0., _pz = 0.;
+ private double _x = 0., _y = 0., _z = 0., _r, _phi = 0.;
+ private double[] _pointParams = new double[] {0.,0.,0.,0.,0.,0.};
+ private double[] _pFitParams = new double[] {0.,0.,0.,0.,0.};
+
+ public void setTrackFitParameters(int ch, double dzero, double phizero, double omeg, double zzero, double spar)
+ {
+ _charge = ch;
+ _d0 = dzero;
+ _phi0 = phizero;
+ _omega = omeg; if(_omega==0.) {System.out.println("Null curvature!");}
+ _z0 = zzero;
+ _s = spar;
+
+ _fitParams[0] = _d0;
+ _fitParams[1] = _phi0;
+ _fitParams[2] = _omega;
+ _fitParams[3] = _z0;
+ _fitParams[4] = _s;
+
+ System.out.print("KFTrackParameters: setting fit pars = ");
+ this.printFitParams();
+
+ makePointFromFitRepresentation();
+ makeHelixFromPointRepresentation();
+ }
+
+ public void setTrackPointParameters(int ch, double x, double y, double z, double px, double py, double pz)
+ {
+ _charge = ch;
+ _x = x;
+ _y = y;
+ _r = Math.sqrt(_x*_x+_y*_y);
+ //_phi = 0.; if(_x!=0.&&_y!=0) { _phi = _r*Math.atan2(_y,_x); }
+ _phi = 0.; if(_x!=0.&&_y!=0) { _phi = Math.atan2(_y,_x); }
+ _z = z;
+ _px = px;
+ _py = py;
+ _pz = pz;
+
+ _pointParams[0] = _x;
+ _pointParams[1] = _y;
+ _pointParams[2] = _z;
+ _pointParams[3] = _px;
+ _pointParams[4] = _py;
+ _pointParams[5] = _pz;
+
+ _pFitParams[0] = _phi;
+ _pFitParams[1] = _z;
+ _pFitParams[2] = _px;
+ _pFitParams[3] = _py;
+ _pFitParams[4] = _pz;
+
+ _pt = Math.sqrt(_px*_px + _py*_py);
+ _p = Math.sqrt(_px*_px + _py*_py + _pz*_pz);
+
+ _xc = _x+_charge*_py/_cst.pointThreeTimesB();
+ _yc = _y-_charge*_px/_cst.pointThreeTimesB();
+ _rc = Math.sqrt((_x-_xc)*(_x-_xc)+(_y-_yc)*(_y-_yc));
+ _pt = Math.sqrt(_px*_px+_py*_py); // = _rc/_cst.pointThreeTimesB();
+ _s = _pz/_pt;
+
+ double r0 = Math.sqrt(_xc*_xc+_yc*_yc);
+ double alpha = Math.acos((r0*r0-_x*_xc-_y*_yc)/(r0*_rc));
+ double z0Plus = z+alpha*_pz/_cst.pointThreeTimesB();
+ double z0Minus = z-alpha*_pz/_cst.pointThreeTimesB();
+ if((_z-z0Plus)*_pz>=0.) {
+ _z0 = z0Plus;
+ } else {
+ _z0 = z0Minus;
+ }
+
+ _helixParams[0] = _xc;
+ _helixParams[1] = _yc;
+ _helixParams[2] = _rc;
+ _helixParams[3] = _z0;
+ _helixParams[4] = _s;
+
+ makeFitFromHelixRepresentation();
+ makePointFromFitRepresentation();
+ }
+
+ private void makePointFromFitRepresentation()
+ {
+ _pt = _cst.pointThreeTimesB()/Math.abs(_omega);
+ _px0 = _pt*Math.cos(_phi0);
+ _py0 = _pt*Math.sin(_phi0);
+ _pz0 = _pt*_s;
+ _p = Math.sqrt(_px0*_px0 + _py0*_py0 + _pz0*_pz0);
+
+ _x0 = _d0*_charge*Math.sin(_phi0);
+ _y0 = -_d0*_charge*Math.cos(_phi0);
+
+ _pointOriginParams[0] = _x0;
+ _pointOriginParams[1] = _y0;
+ _pointOriginParams[2] = _z0;
+ _pointOriginParams[3] = _px0;
+ _pointOriginParams[4] = _py0;
+ _pointOriginParams[5] = _pz0;
+ }
+
+ private void makeHelixFromPointRepresentation()
+ {
+ _xc = _x0+_charge*_py0/_cst.pointThreeTimesB();
+ _yc = _y0-_charge*_px0/_cst.pointThreeTimesB();
+ _rc = Math.sqrt((_x0-_xc)*(_x0-_xc)+(_y0-_yc)*(_y0-_yc));
+ _pt = Math.sqrt(_px0*_px0+_py0*_py0); // = _rc/_cst.pointThreeTimesB();
+ _s = _pz0/_pt;
+ //_z0 = _z0; // valid only if point is POCA => fix me!
+
+ _helixParams[0] = _xc;
+ _helixParams[1] = _yc;
+ _helixParams[2] = _rc;
+ _helixParams[3] = _z0;
+ _helixParams[4] = _s;
+
+ }
+
+ private void makeFitFromHelixRepresentation()
+ {
+ _d0 = Math.sqrt(_xc*_xc+_yc*_yc)-_rc;
+ _phi0 = Math.atan2(_xc,-_yc);
+ _omega = -_charge/_rc;
+ //_z0 = _z0;
+ //_s = _s;
+
+ _fitParams[0] = _d0;
+ _fitParams[1] = _phi0;
+ _fitParams[2] = _omega;
+ _fitParams[3] = _z0;
+ _fitParams[4] = _s;
+ }
+
+ public KFTrackParameters makePredictionAtRadius(double newRadius) {
+
+ System.out.print("KFTrackParameters: pre-propagate "); this.printFitParams();
+
+ KFTrackParameters newTrack = new KFTrackParameters();
+ newTrack = propagate(newRadius);
+
+ System.out.print("KFTrackParameters: propagated "); newTrack.printFitParams();
+
+ // propagate error matrix
+ Matrix jacobian = getJacobian(newRadius);
+ Matrix transposedJacobian = jacobian.transpose();
+ this._ErrorMatrixAtRadiusRprime = jacobian.times(_ErrorMatrixAtRadiusR).times(transposedJacobian);
+ newTrack.setErrorMatrixAtRadiusR(_ErrorMatrixAtRadiusRprime);
+
+ System.out.println("KFTrackParameters: Error matrix at radius R:");
+ _ErrorMatrixAtRadiusR.print(15,12);
+
+ System.out.println("KFTrackParameters: Error matrix at radius Rprime:");
+ _ErrorMatrixAtRadiusRprime.print(15,12);
+
+ System.out.println("KFTrackParameters: Jacobian matrix at radius R:");
+ jacobian.print(15,12);
+
+ // test linearity of jacobian
+ Matrix stateVec = new Matrix(_pFitParams,5);
+ Matrix pred = jacobian.times(stateVec);
+ pred.print(10,5);
+
+ return newTrack;
+ }
+
+ public KFTrackParameters propagate(double r)
+ {
+ // propagate trajectory to radius r
+ double r0Sqr = _xc*_xc+_yc*_yc;
+ double deltaSqr = 0.5*(r*r+r0Sqr-_rc*_rc);
+ //double Delta = deltaSqr*deltaSqr*(_xc*_xc-r0Sqr)+r0Sqr*r*r*_yc*_yc;
+ double Delta = (r0Sqr*r*r-deltaSqr*deltaSqr)*_yc*_yc;
+ double sqrtDelta = 0;
+ if(Delta>=0) {
+ sqrtDelta = Math.sqrt(Delta);
+ } else {
+ System.out.println("Negative sqrt");
+ }
+ double xPlus = (deltaSqr*_xc + sqrtDelta)/r0Sqr;
+ double xMinus = (deltaSqr*_xc - sqrtDelta)/r0Sqr;
+
+ double yPlus = (deltaSqr-xPlus*_xc)/_yc;
+ double yMinus = (deltaSqr-xMinus*_xc)/_yc;
+
+ double rCurrent = Math.sqrt(_x*_x+_y*_y);
+ double testPlus = _px*(xPlus-_x)+_py*(yPlus-_y);
+ double testMinus = _px*(xMinus-_x)+_py*(yMinus-_y);
+
+ double xNew = _x;
+ double yNew = _y;
+ if(testPlus>testMinus) {
+ xNew = xPlus;
+ yNew = yPlus;
+ } else {
+ xNew = xMinus;
+ yNew = yMinus;
+ }
+
+ //System.out.print("Old radius = "); System.out.println(_rc);
+ //double newRC = Math.sqrt((xNew-_xc)*(xNew-_xc)+(yNew-_yc)*(yNew-_yc));
+ //System.out.print("New radius = "); System.out.println(newRC);
+
+ double cosAlpha = ((_x-_xc)*(xNew-_xc)+(_y-_yc)*(yNew-_yc))/(_rc*_rc);
+ double sinAlpha = ((_x-_xc)*(yNew-_yc)-(_y-_yc)*(xNew-_xc))/(_rc*_rc);
+ double alpha = Math.atan2(sinAlpha,cosAlpha);
+
+ //System.out.print("xc,yc = "); System.out.print(_xc); System.out.print(" "); System.out.println(_yc);
+ //System.out.print("x,y = "); System.out.print(_x); System.out.print(" "); System.out.println(_y);
+ //System.out.print("xNew,yNew = "); System.out.print(xNew); System.out.print(" "); System.out.println(yNew);
+ //System.out.print("cos,sin = "); System.out.print(cosAlpha); System.out.print(" "); System.out.println(sinAlpha);
+ //System.out.print("alpha = "); System.out.println(alpha);
+
+ double zNew = _z + alpha*_pz/_cst.pointThreeTimesB();
+
+ double pxNew = _px*Math.cos(alpha) - _py*Math.sin(alpha);
+ double pyNew = _px*Math.sin(alpha) + _py*Math.cos(alpha);
+ double pzNew = _pz;
+
+ KFTrackParameters newTrackPoint = new KFTrackParameters();
+ newTrackPoint.setTrackPointParameters(_charge, xNew, yNew, zNew, pxNew, pyNew, pzNew);
+
+ return newTrackPoint;
+
+ }
+
+ private Matrix getJacobian(double r)
+ {
+ // calculate the jacobian (dphi/dphi, dphi/dz, ...)
+ double[][] Jacobian = new double[5][5];
+
+ //double[] variations = new double[] {0.00001,0.001,0.0000001,0.001,0.001};
+ double[] variations = new double[] {0.001,0.001,0.0001,0.0001,0.0001};
+
+ double[] fitPars = new double[5];
+ for(int k=0; k<5; k++) { fitPars[k] = _pFitParams[k]; }
+
+ for(int j=0; j<5; j++) {
+
+ double p1p = fitPars[0]; double p1m = fitPars[0];
+ if(j==0) { p1p += 0.5*variations[0]; p1m -= 0.5*variations[0]; }
+ double p2p = fitPars[1]; double p2m = fitPars[1];
+ if(j==1) { p2p += 0.5*variations[1]; p2m -= 0.5*variations[1]; }
+ double p3p = fitPars[2]; double p3m = fitPars[2];
+ if(j==2) { p3p += 0.5*variations[2]; p3m -= 0.5*variations[2]; }
+ double p4p = fitPars[3]; double p4m = fitPars[3];
+ if(j==3) { p4p += 0.5*variations[3]; p4m -= 0.5*variations[3]; }
+ double p5p = fitPars[4]; double p5m = fitPars[4];
+ if(j==4) { p5p += 0.5*variations[4]; p5m -= 0.5*variations[4]; }
+
+ double xp = _r*Math.cos(p1p); double yp = _r*Math.sin(p1p);
+ double xm = _r*Math.cos(p1m); double ym = _r*Math.sin(p1m);
+
+ System.out.print("KFTrackParameters d0/phi0/omega...Plus= ");
+ System.out.print(p1p); System.out.print(" (");
+ System.out.print(xp); System.out.print(" , ");
+ System.out.print(yp); System.out.print(") ");
+ System.out.print(p2p); System.out.print(" ");
+ System.out.print(p3p); System.out.print(" ");
+ System.out.print(p4p); System.out.print(" ");
+ System.out.println(p5p);
+
+ System.out.print("KFTrackParameters d0/phi0/omega...Minus= ");
+ System.out.print(p1m); System.out.print(" (");
+ System.out.print(xm); System.out.print(" , ");
+ System.out.print(ym); System.out.print(") ");
+ System.out.print(p2m); System.out.print(" ");
+ System.out.print(p3m); System.out.print(" ");
+ System.out.print(p4m); System.out.print(" ");
+ System.out.println(p5m);
+
+ KFTrackParameters trkVarPlus = new KFTrackParameters();
+ trkVarPlus.setTrackPointParameters(_charge, xp, yp, p2p, p3p, p4p, p5p);
+ KFTrackParameters trkVarPlusPropagated = trkVarPlus.propagate(r);
+ double[] trkVarPlusParam = trkVarPlusPropagated.getPFitParameters();
+ KFTrackParameters trkVarMinus = new KFTrackParameters();
+ trkVarMinus.setTrackPointParameters(_charge, xm, ym, p2m, p3m, p4m, p5m);
+ KFTrackParameters trkVarMinusPropagated = trkVarMinus.propagate(r);
+ double[] trkVarMinusParam = trkVarMinusPropagated.getPFitParameters();
+
+ for(int i=0; i<5; i++) {
+ //Jacobian[j][i] = (trkVarPlusParam[i]-trkVarMinusParam[i])/variations[j];
+ Jacobian[i][j] = (trkVarPlusParam[i]-trkVarMinusParam[i])/variations[j];
+ }
+ }
+
+
+ Matrix jacobianMatrix = new Matrix(Jacobian);
+
+ return jacobianMatrix;
+
+ }
+
+ public void setPointToRadius(double r) {
+ this.setPointToPOCA();
+ KFTrackParameters trk = propagate(r);
+ double[] nP = trk.getPointParameters();
+ for(int i=0; i<6; i++) { _pointParams[i] = nP[i]; }
+ _x = nP[0]; _y = nP[1]; _z = nP[2]; _px = nP[3]; _py = nP[4]; _pz = nP[5];
+ _r = Math.sqrt(_x*_x+_y*_y);
+ //_phi = 0.; if(_x!=0.&&_y!=0) { _phi = _r*Math.atan2(_y,_x); }
+ _phi = 0.; if(_x!=0.&&_y!=0) { _phi = Math.atan2(_y,_x); }
+ _pFitParams[0] = _phi;
+ for(int i=2; i<6; i++) { _pFitParams[i-1] = nP[i]; }
+ }
+
+ public void setPointToPOCA() {
+ for(int i=0; i<6; i++) { _pointParams[i] = _pointOriginParams[i]; }
+ _x = _x0; _y = _y0; _z = _z0; _px = _px0; _py = _py0; _pz = _pz0;
+ _r = Math.sqrt(_x*_x+_y*_y);
+ //_phi = 0.; if(_x!=0.&&_y!=0) { _phi = _r*Math.atan2(_y,_x); }
+ _phi = 0.; if(_x!=0.&&_y!=0) { _phi = Math.atan2(_y,_x); }
+ _pFitParams[0] = _phi;
+ for(int i=2; i<6; i++) { _pFitParams[i-1] = _pointOriginParams[i]; }
+ }
+
+ public double[] getFitParameters() {return _fitParams;}
+ public double[] getPointOriginParameters() {return _pointOriginParams;}
+ public double[] getPointParameters() {return _pointParams;}
+ public double[] getPFitParameters() {return _pFitParams;}
+ public double[] getHelixParameters() {return _helixParams;}
+
+ public int getCharge() {return _charge;}
+ public double getPt() {return _pt;}
+
+ public void setErrorMatrixAtRadiusR(Matrix cov) { _ErrorMatrixAtRadiusR = cov; }
+
+ public Matrix getFitCovMatrix() {return _FitErrorMatrix;}
+ public Matrix getCovMatrixAtRadiusR() {return _ErrorMatrixAtRadiusR;}
+ public Matrix getCovMatrixAtRadiusRprime() {return _ErrorMatrixAtRadiusRprime;}
+
+ private Matrix _FitErrorMatrix = new Matrix(5,5);
+ private Matrix _ErrorMatrixAtRadiusR = new Matrix(5,5);
+ private Matrix _ErrorMatrixAtRadiusRprime = new Matrix(5,5);
+ private KFConstants _cst = new KFConstants();
+
+ public void resetTrackParameters() {
+ int i;
+ _charge = 0;
+ _d0 = 0.; _phi0 = 0.; _omega = 0.; _z0 = 0.; _s = 0.;
+ for(i=0;i<5;i++){_fitParams[i]=0.;};
+
+ _p = 0.; _pt = 0.; _px0 = 0.; _py0 = 0.; _pz0 = 0.;
+ _x0 = 0.; _y0 = 0.;
+ for(i=0;i<6;i++){_pointOriginParams[i]=0.;};
+
+ _xc = 0.; _yc = 0.; _rc = 0.;
+ for(i=0;i<5;i++){_helixParams[i]=0.;};
+
+ _px = 0.; _py = 0.; _pz = 0.;
+ _x = 0.; _y = 0.; _z = 0.; _r = 0.; _phi = 0.;
+ for(i=0;i<6;i++){_pointParams[i]=0.;};
+ for(i=0;i<5;i++){_pFitParams[i]=0.;};
+
+ }
+
+ public void printPFitParams() {
+ System.out.print("PFit params = ");
+ System.out.print(_pFitParams[0]); System.out.print(" ");
+ System.out.print(_pFitParams[1]); System.out.print(" ");
+ System.out.print(_pFitParams[2]); System.out.print(" ");
+ System.out.print(_pFitParams[3]); System.out.print(" ");
+ System.out.println(_pFitParams[4]);
+ }
+
+ public void printFitParams() {
+ System.out.print("Fit params = ");
+ System.out.print(_fitParams[0]); System.out.print(" ");
+ System.out.print(_fitParams[1]); System.out.print(" ");
+ System.out.print(_fitParams[2]); System.out.print(" ");
+ System.out.print(_fitParams[3]); System.out.print(" ");
+ System.out.println(_fitParams[4]);
+ }
+}
lcsim/src/org/lcsim/contrib/KFFitter/test
diff -N TestKFF.java
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ TestKFF.java 13 Dec 2006 22:45:12 -0000 1.1
@@ -0,0 +1,30 @@
+import java.util.List;
+
+import java.util.*;
+import java.io.*;
+import org.lcsim.event.EventHeader;
+import org.lcsim.event.MCParticle;
+import org.lcsim.event.TrackerHit;
+import org.lcsim.util.Driver;
+import org.lcsim.util.aida.AIDA;
+import org.lcsim.contrib.KFFitter.KFFitterDriver;
+
+
+public class TestKFF extends Driver {
+ private AIDA aida = AIDA.defaultInstance();
+ public TestKFF(){
+ add(new KFFitterDriver());
+ // try{
+ // FileOutputStream fos = new FileOutputStream("output.dat");
+ // PrintStream ps = new PrintStream(fos);
+ // System.setOut(ps);
+ // }
+ // catch(FileNotFoundException x){
+ // System.out.println("blow me!! output.dat doesn't exist");
+ // x.printStackTrace();
+ // }
+ };
+ public void process(EventHeader event) {
+ super.process(event); // this takes care that the child Drivers are loaded and processed.
+ }
+}
CVSspam 0.2.8
