diff --git a/tracking/src/main/java/org/hps/recon/tracking/kalman/KalTrack.java b/tracking/src/main/java/org/hps/recon/tracking/kalman/KalTrack.java index 55bfee0754..50d4107fb5 100644 --- a/tracking/src/main/java/org/hps/recon/tracking/kalman/KalTrack.java +++ b/tracking/src/main/java/org/hps/recon/tracking/kalman/KalTrack.java @@ -27,6 +27,7 @@ public class KalTrack { public int ID; public int nHits; public double chi2; + private double reducedChi2; ArrayList SiteList; // call the corresponding functions to create and access the following two maps @@ -35,6 +36,7 @@ public class KalTrack { Map millipedeMap; Map lyrMap; public int eventNumber; + public boolean bad; HelixState helixAtOrigin; private boolean propagated; private RotMatrix Rot; @@ -50,7 +52,7 @@ public class KalTrack { private double time; double tMin; double tMax; - final static boolean debug = false; + static final boolean debug = false; private KalmanParams kPar; private double chi2incVtx; private static DMatrixRMaj tempV; @@ -59,15 +61,18 @@ public class KalTrack { private static boolean initialized; private double [] arcLength; private static LinearSolverDense solver; + static int [] nBadCov = {0, 0}; KalTrack(int evtNumb, int tkID, ArrayList SiteList, ArrayList yScat, ArrayList XLscat, KalmanParams kPar) { // System.out.format("KalTrack constructor chi2=%10.6f\n", chi2); eventNumber = evtNumb; + bad = false; this.yScat = yScat; this.XLscat = XLscat; this.kPar = kPar; ID = tkID; arcLength = null; + //debug = (evtNumb == 217481); if (!initialized) { logger = Logger.getLogger(KalTrack.class.getName()); @@ -98,6 +103,7 @@ public class KalTrack { logger.log(Level.WARNING, String.format("Event %d: site is missing smoothed state vector for layer %d detector %d", eventNumber, site.m.Layer, site.m.detector)); logger.log(Level.WARNING, site.toString("bad site")); + bad = true; continue; } this.SiteList.add(site); @@ -125,6 +131,7 @@ public class KalTrack { tMax = -9.9e9; this.chi2 = 0.; this.nHits = 0; + if (debug) System.out.format("KalTrack: event %d, creating track %d\n", evtNumb, ID); for (MeasurementSite site : this.SiteList) { if (site.hitID < 0) continue; nHits++; @@ -132,14 +139,17 @@ public class KalTrack { tMin = Math.min(tMin, site.m.hits.get(site.hitID).time); tMax = Math.max(tMax, site.m.hits.get(site.hitID).time); this.chi2 += site.chi2inc; + if (debug) System.out.format(" Layer %d, chi^2 increment=%10.5f, a=%s\n", site.m.Layer, site.chi2inc, site.aS.helix.a.toString()); } time = time/(double)nHits; + reducedChi2 = chi2/(double)nHits; lyrMap = null; millipedeMap = null; interceptVects = null; interceptMomVects = null; if (nHits < 5) { // This should never happen logger.log(Level.WARNING, "KalTrack error: not enough hits ("+nHits+") on the candidate track (ID::"+ID+") for event "+eventNumber); + bad = true; //for (MeasurementSite site : SiteList) logger.log(Level.FINE, site.toString("in KalTrack input list")); //logger.log(Level.FINE, String.format("KalTrack error in event %d: not enough hits on track %d: ",evtNumb,tkID)); //String str=""; @@ -366,7 +376,7 @@ public void print(String s) { } String toString(String s) { - String str = String.format("\n KalTrack %s: Event %d, ID=%d, %d hits, chi^2=%10.5f, t=%5.1f from %5.1f to %5.1f\n", s, eventNumber, ID, nHits, chi2, time, tMin, tMax); + String str = String.format("\n KalTrack %s: Event %d, ID=%d, %d hits, chi^2=%10.5f, t=%5.1f from %5.1f to %5.1f, bad=%b\n", s, eventNumber, ID, nHits, chi2, time, tMin, tMax, bad); if (propagated) { str=str+String.format(" B-field at the origin=%10.6f, direction=%8.6f %8.6f %8.6f\n", Bmag, tB.v[0], tB.v[1], tB.v[2]); str=str+helixAtOrigin.toString("helix state for a pivot at the origin")+"\n"; @@ -615,7 +625,7 @@ public HelixState originConstraint(double [] vtx, double [][] vtxCov) { SquareMatrix Cov = helixAtOrigin.Rot.rotate(new SquareMatrix(3,vtxCov)); Vec X0 = helixAtOrigin.X0; double phi = phiDOCA(helixAtOrigin.a, v, X0, alpha); - if (debug) { // Test the DOCA algorithm +/* if (debug) { // Test the DOCA algorithm Vec rDoca = HelixState.atPhi(X0, helixAtOrigin.a, phi, alpha); System.out.format("originConstraint: phi of DOCA=%10.5e\n", phi); rDoca.print(" DOCA point"); @@ -633,7 +643,7 @@ public HelixState originConstraint(double [] vtx, double [][] vtxCov) { double deriv = dfDOCAdPhi(phi, helixAtOrigin.a, v, X0, alpha); double df2 = deriv * delPhi; System.out.format("Test of fDOCA derivative: df exact = %11.7f; df from derivative = %11.7f\n", df1, df2); - } + }*/ double [][] H = buildH(helixAtOrigin.a, v, X0, phi, alpha); Vec pntDOCA = HelixState.atPhi(X0, helixAtOrigin.a, phi, alpha); if (debug) { @@ -984,6 +994,7 @@ public boolean removeHit(MeasurementSite site, double mxChi2Inc, double mxTdif) } chi2 -= site.chi2inc; nHits--; + reducedChi2 = chi2/(double)nHits; int oldID = site.hitID; site.removeHit(); // Check whether there might be another hit available @@ -1103,62 +1114,107 @@ public int addHits(ArrayList data, double mxResid, double mxChi2inc, d } // re-fit the track - public boolean fit(int nIterations) { + public boolean fit(boolean keep) { + // keep = true if there might be another recursion after dropping more hits double chi2s = 0.; - for (int iteration = 0; iteration < nIterations; iteration++) { - if (debug) System.out.format("KalTrack.fit: starting filtering for iteration %d\n", iteration); - StateVector sH = SiteList.get(0).aS; + if (debug) System.out.format("Entering KalTrack.fit for event %d, track %d\n", eventNumber, ID); + StateVector sH = SiteList.get(0).aS.copy(); + boolean badC = KalmanPatRecHPS.negativeCov(sH.helix.C); + if (badC) { + if (debug) System.out.format("KalTrack.fit: negative starting covariance, event %d track %d\n", eventNumber, ID); + KalmanPatRecHPS.setInitCov(sH.helix.C, sH.helix.a, false); + } else { CommonOps_DDRM.scale(100., sH.helix.C); // Blow up the initial covariance matrix to avoid double counting measurements - SiModule prevMod = null; - double chi2f = 0.; - for (int idx = 0; idx < SiteList.size(); idx++) { // Redo all the filter steps - MeasurementSite currentSite = SiteList.get(idx); - currentSite.predicted = false; - currentSite.filtered = false; - currentSite.smoothed = false; - currentSite.chi2inc = 0.; - currentSite.aP = null; - currentSite.aF = null; - currentSite.aS = null; - - boolean allowSharing = false; - boolean pickupHits = false; - boolean checkBounds = false; - double [] tRange = {-999., 999.}; - if (currentSite.makePrediction(sH, prevMod, currentSite.hitID, allowSharing, pickupHits, checkBounds, tRange, 0) < 0) { - if (debug) System.out.format("KalTrack.fit: event %d, track %d in iteration %d failed to make prediction!!\n", eventNumber, ID, iteration); - return false; - } - if (!currentSite.filter()) { - if (debug) System.out.format("KalTrack.fit: event %d, track %d in iteration %d failed to filter!!\n", eventNumber, ID, iteration); - return false; - } - - if (currentSite.hitID >= 0 && debug) chi2f += Math.max(currentSite.chi2inc,0.); - - sH = currentSite.aF; - prevMod = currentSite.m; - } - if (debug) System.out.format("KalTrack.fit: Iteration %d, Fit chi^2 after filtering = %12.4e\n", iteration, chi2f); + } + SiModule prevMod = null; + double chi2f = 0.; + ArrayList newSiteList = new ArrayList(SiteList.size()); + boolean badCov = false; + for (int idx = 0; idx < SiteList.size(); idx++) { // Redo all the filter steps + MeasurementSite currentSite = SiteList.get(idx); + MeasurementSite newSite = new MeasurementSite(currentSite.m.Layer, currentSite.m, kPar); - chi2s = 0.; - MeasurementSite nextSite = null; - for (int idx = SiteList.size() - 1; idx >= 0; idx--) { - MeasurementSite currentSite = SiteList.get(idx); - if (nextSite == null) { - currentSite.aS = currentSite.aF; - currentSite.smoothed = true; - } else { - currentSite.smooth(nextSite); - } - if (currentSite.hitID >= 0) chi2s += Math.max(currentSite.chi2inc,0.); - - nextSite = currentSite; + boolean allowSharing = false; + boolean pickupHits = false; + boolean checkBounds = false; + double [] tRange = {-999., 999.}; + if (newSite.makePrediction(sH, prevMod, currentSite.hitID, allowSharing, pickupHits, checkBounds, tRange, 0) < 0) { + if (debug) System.out.format("KalTrack.fit: event %d, track %d failed to make prediction at layer %d!\n", eventNumber, ID, newSite.m.Layer); + return false; + } + if (!newSite.filter()) { + if (debug) System.out.format("KalTrack.fit: event %d, track %d failed to filter!\n", eventNumber, ID); + return false; + } + if (KalmanPatRecHPS.negativeCov(currentSite.aF.helix.C)) { + if (debug) System.out.format("KalTrack: event %d, ID %d, negative covariance after filtering at layer %d\n", + eventNumber,ID,currentSite.m.Layer); + badCov = true; + KalmanPatRecHPS.fixCov(currentSite.aF.helix.C, currentSite.aF.helix.a); + } + if (debug) { + if (newSite.hitID >= 0) chi2f += Math.max(currentSite.chi2inc,0.); + } + newSite.hitID = currentSite.hitID; + sH = newSite.aF; + if (debug) System.out.format(" Layer %d hit %d filter, chi^2 increment=%10.5f, a=%s\n", + newSite.m.Layer, newSite.hitID, newSite.chi2inc, newSite.aF.helix.a.toString()); + prevMod = newSite.m; + if (keep) currentSite.chi2inc = newSite.chi2inc; // Residuals to cut out hits in next recursion + newSiteList.add(newSite); + } + if (badCov) { + nBadCov[0]++; + bad = true; + } + if (debug) System.out.format("KalTrack.fit: Track %d, Fit chi^2 after filtering = %12.4e\n", ID, chi2f); + + chi2s = 0.; + int nNewHits = 0; + badCov = false; + MeasurementSite nextSite = null; + for (int idx = newSiteList.size() - 1; idx >= 0; idx--) { + MeasurementSite currentSite = newSiteList.get(idx); + if (nextSite == null) { + currentSite.aS = currentSite.aF; + currentSite.smoothed = true; + } else { + currentSite.smooth(nextSite); + } + if (currentSite.hitID >= 0) { + chi2s += Math.max(currentSite.chi2inc,0.); + nNewHits++; + } + if (KalmanPatRecHPS.negativeCov(currentSite.aS.helix.C)) { + if (debug) System.out.format("KalTrack: event %d, ID %d, negative covariance after smoothing at layer %d\n", + eventNumber,ID,currentSite.m.Layer); + badCov = true; + KalmanPatRecHPS.fixCov(currentSite.aS.helix.C, currentSite.aS.helix.a); + } + if (debug) { + System.out.format(" Layer %d hit %d, smooth, chi^2 increment=%10.5f, a=%s\n", + currentSite.m.Layer, currentSite.hitID, currentSite.chi2inc, currentSite.aS.helix.a.toString()); + } + nextSite = currentSite; + } + if (badCov) { + nBadCov[1]++; + bad = true; + } + if (debug) System.out.format("KalTrack.fit: Track %d, Fit chi^2 after smoothing = %12.4e\n", ID, chi2s); + if (!keep) { + if (chi2s/(double)nNewHits > reducedChi2*1.2) { + if (debug) System.out.format("KalTrack.fit event %d track %d: fit chisquared=%10.5f is not an improvement. Discard new fit.\n", + eventNumber, ID, chi2s); + return false; } - if (debug) System.out.format("KalTrack.fit: Iteration %d, Fit chi^2 after smoothing = %12.4e\n", iteration, chi2s); } + SiteList = newSiteList; this.chi2 = chi2s; + this.nHits = nNewHits; + this.reducedChi2 = chi2s/(double)nNewHits; propagated = false; + if (debug) System.out.format("Exiting KalTrack.fit for event %d, track %d\n", eventNumber, ID); return true; } diff --git a/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanInterface.java b/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanInterface.java index 8d3f51c74c..7f6778c375 100644 --- a/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanInterface.java +++ b/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanInterface.java @@ -76,6 +76,7 @@ public class KalmanInterface { private static DMatrixRMaj Ft; private int maxHits; private int nBigEvents; + private int eventNumber; private static final boolean debug = false; private static final double SVTcenter = 505.57; @@ -213,6 +214,8 @@ public void summary() { System.out.format("KalmanInterface::summary: number of events with > 200 hits=%d.\n", nBigEvents); System.out.format(" Maximum event size = %d strip hits.\n", maxHits); System.out.format(" Events with > %d hits were not processed.\n", _siHitsLimit); + System.out.format(" Number of tracks with bad covariance in filterTrack= %d %d\n", KalmanPatRecHPS.nBadCov[0], KalmanPatRecHPS.nBadCov[1]); + System.out.format(" Number of tracks with bad covariance in KalTrack.fit=%d %d\n", KalTrack.nBadCov[0], KalTrack.nBadCov[1]); } // Return the reference to the parameter setting code for the driver to use @@ -390,6 +393,7 @@ public void printGBLStripClusterData(GBLStripClusterData clstr) { public List createGBLStripClusterData(KalTrack kT) { List rtnList = new ArrayList(kT.SiteList.size()); + double phiLast = 9999.; for (MeasurementSite site : kT.SiteList) { GBLStripClusterData clstr = new GBLStripClusterData(kT.SiteList.indexOf(site)); @@ -429,6 +433,10 @@ public List createGBLStripClusterData(KalTrack kT) { site.m.Layer, site.m.detector, -tanL, tanLambda, tiltAngle, tiltAngle+tanLambda); } clstr.setTrackPhi(phi); + if (phiLast < 10.) { + if (Math.abs(phi - phiLast) > 1.2) System.out.format("Big phi change in event %d\n", eventNumber); + } + phiLast = phi; clstr.setTrackLambda(FastMath.atan(tanLambda)); // Measured value in the sensor coordinates (u-value in the HPS system) @@ -751,6 +759,7 @@ public void createSiModules(List inputPlanes) { // Method to feed simulated hits into the pattern recognition, for testing private boolean fillAllSimHits(EventHeader event, IDDecoder decoder) { boolean success = false; + eventNumber = event.getEventNumber(); if (debug || event.getEventNumber() < 50) System.out.format("KalmanInterface.fillAllSimHits: entering for event %d\n", event.getEventNumber()); @@ -830,6 +839,7 @@ private boolean fillAllSimHits(EventHeader event, IDDecoder decoder) { // Method to fill all Si hits into the SiModule objects, to feed to the pattern recognition. private boolean fillAllMeasurements(EventHeader event) { boolean success = false; + eventNumber = event.getEventNumber(); // Get the collection of 1D hits String stripHitInputCollectionName = "StripClusterer_SiTrackerHitStrip1D"; @@ -932,7 +942,7 @@ private boolean fillAllMeasurements(EventHeader event) { double time = localHit.getTime(); double xStrip = -lpos[1]; // Center of strip, i.e. ~0 except in layers 0 and 1 if (xStrip > module.xExtent[1] || xStrip < module.xExtent[0]) { - logger.log(Level.FINE, String.format("Event %d Layer %d, local hit at %9.4f %9.4f, %9.4f is outside detector extents %8.3f->%8.3f %8.3f->%8.3f", + logger.log(Level.WARNING, String.format("Event %d Layer %d, local hit at %9.4f %9.4f, %9.4f is outside detector extents %8.3f->%8.3f %8.3f->%8.3f", event.getEventNumber(), module.Layer, lpos[0], lpos[1], lpos[2], module.yExtent[0], module.yExtent[1], module.xExtent[0], module.xExtent[1])); } if (debug) { @@ -1292,6 +1302,7 @@ public void plotGBLtracks(String path, EventHeader event) { // This method makes a Gnuplot file to display the Kalman tracks and hits in 3D. public void plotKalmanEvent(String path, EventHeader event, ArrayList[] patRecList) { + boolean debug = false; PrintWriter printWriter3 = null; int eventNumber = event.getEventNumber(); String fn = String.format("%shelix3_%d.gp", path, eventNumber); @@ -1344,6 +1355,11 @@ public void plotKalmanEvent(String path, EventHeader event, ArrayList[ Vec rLocal = aS.helix.atPhi(phiS); Vec rGlobal = aS.helix.toGlobal(rLocal); printWriter3.format(" %10.6f %10.6f %10.6f\n", rGlobal.v[0], rGlobal.v[1], rGlobal.v[2]); + if (debug) { + System.out.format("plotKalmanEvent %d: tk %d lyr %d phiS=%11.6f\n", event.getEventNumber(), tkr.ID, module.Layer, phiS); + rLocal.print(" local point in B frame"); + rGlobal.print(" global point"); + } // Vec rDetector = m.toLocal(rGlobal); // double vPred = rDetector.v[1]; // if (site.hitID >= 0) { @@ -1370,13 +1386,27 @@ public void plotKalmanEvent(String path, EventHeader event, ArrayList[ Vec rLocal = aS.helix.atPhi(phiS); // Position in the Bfield frame Vec rGlobal = aS.helix.toGlobal(rLocal); // Position in the global frame rLoc = module.toLocal(rGlobal); // Position in the detector frame + if (debug) { + double resid = rLoc.v[1] - mm.v; + System.out.format("plotKalmanEvent %d: tk %d lyr %d phiS=%11.6f resid= %11.8f vs %11.8f\n", + event.getEventNumber(), tkr.ID, module.Layer, phiS, resid, site.aS.r); + aS.helix.a.print(" helix parameters "); + rLocal.print(" local position in B frame"); + rGlobal.print(" global position "); + rLoc.print(" position in detector frame"); + } } else { + if (debug) { + System.out.format("plotKalmanEvent %d: tk %d lyr %d phiS is NaN.\n", event.getEventNumber(),tkr.ID, module.Layer); + aS.helix.a.print(" helix parameters "); + } rLoc = new Vec(0.,0.,0.); } } else { rLoc = module.toLocal(mm.rGlobal); // Use MC truth for the x and z coordinates in the detector frame } Vec rmG = module.toGlobal(new Vec(rLoc.v[0], mm.v, rLoc.v[2])); + if (debug) System.out.format("plotKalmanEvent %d: tk %d lyr %d rmG=%s\n", event.getEventNumber(), tkr.ID, module.Layer, rmG.toString()); printWriter3.format(" %10.6f %10.6f %10.6f\n", rmG.v[0], rmG.v[1], rmG.v[2]); } printWriter3.format("EOD\n"); diff --git a/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanParams.java b/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanParams.java index a4f9b7942e..e0f0b68e73 100644 --- a/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanParams.java +++ b/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanParams.java @@ -73,7 +73,7 @@ public void print() { System.out.format(" Minimum number of hits for a good track: %d, %d\n", minHits1[0], minHits1[1]); System.out.format(" Minimum number of stereo hits: %d %d\n", minStereo[0], minStereo[1]); System.out.format(" Minimum number of axial hits: %d\n", minAxial); - System.out.format(" Maximum chi^2 increment to add a hit to a track: %8.2f\n", mxChi2Inc); + System.out.format(" Maximum chi^2 increment to add a hit to a track, or minimum to remove a hit: %8.2f\n", mxChi2Inc); System.out.format(" Chi^2 increment threshold for removing a bad hit from a track candidate: %8.2f\n", minChi2IncBad); System.out.format(" Maximum residual, in units of detector resolution, for picking up a hit: %8.2f, %8.2f\n", mxResid[0], mxResid[1]); System.out.format(" Maximum residual, in units of detector resolution, for a hit to be shared: %8.2f\n", mxResidShare); diff --git a/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanPatRecDriver.java b/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanPatRecDriver.java index b5ea1fadb1..bae7699a55 100644 --- a/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanPatRecDriver.java +++ b/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanPatRecDriver.java @@ -352,10 +352,12 @@ private ArrayList[] prepareTrackCollections(EventHeader event, List 200.) { - System.out.format("KalmanPatRecDriver.process: run time for pattern recognition at event %d is %9.1f ms\n", evtNumb, runTime); - List striphits = event.get(TrackerHit.class, "StripClusterer_SiTrackerHitStrip1D"); - System.out.format(" Number of strip hits = %d\n", striphits.size()); + if (verbose) { + if (runTime > 200.) { + System.out.format("KalmanPatRecDriver.process: run time for pattern recognition at event %d is %9.1f ms\n", evtNumb, runTime); + List striphits = event.get(TrackerHit.class, "StripClusterer_SiTrackerHitStrip1D"); + System.out.format(" Number of strip hits = %d\n", striphits.size()); + } } if (runTime > maxTime) maxTime = runTime; nEvents++; diff --git a/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanPatRecHPS.java b/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanPatRecHPS.java index f6f1664a9f..8b2674a2d0 100644 --- a/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanPatRecHPS.java +++ b/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanPatRecHPS.java @@ -60,6 +60,7 @@ class KalmanPatRecHPS { private int firstLayer; private Plane p0; private static long startTime; + static int [] nBadCov = {0, 0}; KalmanPatRecHPS(KalmanParams kPar) { startTime = (long)0.; @@ -398,8 +399,15 @@ ArrayList kalmanPatRec(ArrayList data, int topBottom, int ev } } - DMatrixRMaj CovGuess = seed.covariance().copy(); - CommonOps_DDRM.scale(100., CovGuess); + //DMatrixRMaj CovGuess = seed.covariance().copy(); + //if (negativeCov(CovGuess)) { + // System.out.format("KalmanPatRec: starting covariance matrix is negative for event %d!\n", eventNumber); + // CovGuess.print(); + //} + //CommonOps_DDRM.scale(10., CovGuess); + DMatrixRMaj CovGuess = new DMatrixRMaj(5,5); + setInitCov(CovGuess, seed.helixParams(), true); + // Create an state vector from the input seed to initialize the Kalman filter StateVector sI = new StateVector(-1, seed.helixParams(), CovGuess, new Vec(0., 0., 0.), Bmag, tB, pivot); TrackCandidate candidateTrack = new TrackCandidate(candID, seed.hits, kPar, hitMap, eventNumber); @@ -1060,13 +1068,9 @@ ArrayList kalmanPatRec(ArrayList data, int topBottom, int ev } } } - ArrayList allTks = new ArrayList(TkrList.size()); - for (KalTrack tkr : TkrList) { - allTks.add(tkr); - } // Refit the KalTracks - Iterator iter = allTks.iterator(); + Iterator iter = TkrList.iterator(); while (iter.hasNext()) { KalTrack tkr = iter.next(); if (debug) { @@ -1079,14 +1083,16 @@ ArrayList kalmanPatRec(ArrayList data, int topBottom, int ev } // Try to add hits on layers with missing hits - tkr.addHits(data, kPar.mxResid[1], kPar.mxChi2Inc, kPar.mxTdif, debug); + int nAdded = tkr.addHits(data, kPar.mxResid[1], kPar.mxChi2Inc, kPar.mxTdif, debug); // check that there are enough hits in both views int nStereo = 0; int nAxial = 0; int nShared = 0; + boolean needRefit = (nAdded>0); for (MeasurementSite site : tkr.SiteList) { if (site.hitID < 0) continue; + if (site.aS == null) needRefit = true; SiModule m = site.m; if (!m.isStereo) nAxial++; else nStereo++; @@ -1112,7 +1118,7 @@ ArrayList kalmanPatRec(ArrayList data, int topBottom, int ev removeIt = true; } if (removeIt) { - TkrList.remove(tkr); + iter.remove(); for (MeasurementSite site : tkr.SiteList) { if (site.hitID != -1) { site.m.hits.get(site.hitID).tracks.remove(tkr); @@ -1126,52 +1132,131 @@ ArrayList kalmanPatRec(ArrayList data, int topBottom, int ev double runTime = (double)((System.nanoTime() - startTime)/1000000.); System.out.format("KalmanPatRecHPS: Call the Kalman fit for track %d at time=%10.6f ms\n", tkr.ID, runTime); } - boolean goodFit = tkr.fit(kPar.nIterations); + boolean goodFit = tkr.fit(!needRefit); if (debug) { double runTime = (double)((System.nanoTime() - startTime)/1000000.); System.out.format(" The Kalman fit is finished for track %d at time=%10.6f ms\n", tkr.ID, runTime); - } - if (goodFit) { - StateVector aS = tkr.SiteList.get(0).aS; - if (aS == null) { - if (debug) System.out.format("KalmanPatRecHPS: track %d has no smoothed state vector at site 0!\n", tkr.ID); - goodFit = false; - } - if (goodFit) { - double phi0 = aS.helix.planeIntersect(new Plane(new Vec(3,kPar.beamSpot), new Vec(0.,1.,0.))); - if (Double.isNaN(phi0)) { - if (debug) System.out.format("KalmanPatRecHPS: track %d does not intersect the origin plane!\n", tkr.ID); - } else { - if (!tkr.originHelix()) { - if (debug) System.out.format("KalmanPatRecHPS: propagating track %d to the origin failed!\n", tkr.ID); - //goodFit = false; - } - if (debug) { - double runTime = (double)((System.nanoTime() - startTime)/1000000.); - System.out.format(" The origin propagation is finished for track %d at time=%10.6f ms\n", tkr.ID, runTime); + } + // See if the fit is better if we strip off the extra hits added + if (!goodFit && needRefit) { + if (debug) System.out.format("KalmanPatRecHPS event %d: Kaltrack fit not improved for track %d, chi2=%10.5f for %d hits\n", + eventNumber, tkr.ID, tkr.chi2, tkr.nHits); + int nChange = 0; + Iterator itr = tkr.SiteList.iterator(); + while (itr.hasNext()) { + MeasurementSite site = itr.next(); + //System.out.format("KalmanPatRecHPS: Layer %d stereo=%b hit %d, chi^2 increment=%10.5f, a=%s\n", + // site.m.Layer, site.m.isStereo, site.hitID, site.chi2inc, site.aF.helix.a.toString()); + if (site.hitID < 0) continue; + if (site.aS == null) { + if (site.m.hits.get(site.hitID).tracks.contains(tkr)) { + site.m.hits.get(site.hitID).tracks.remove(tkr); } + itr.remove(); // Try getting rid of any hits that were added + nChange++; + if (site.m.isStereo) nStereo--; + else nAxial--; } } - if (goodFit) { // For tracks with few hits, include an origin constraint - if (tkr.nHits == 5) { - if (debug) System.out.format("KalmanPatRecHPS: try an origin constraint on track %d\n", tkr.ID); - HelixState constrHelix = tkr.originConstraint(vtx, vtxCov); - if (constrHelix == null) goodFit = false; - if (goodFit) { - double chi2inc = tkr.chi2incOrigin(); - if (chi2inc <= kPar.mxChi2Vtx) { - tkr.helixAtOrigin = constrHelix; - tkr.chi2 += chi2inc * tkr.nHits; - } else { - goodFit = false; + if (nChange == 0) System.out.format("KalmanPatRecHPS: no changed hits were removed from track %d???\n", tkr.ID); + if (nStereo > 2 && nAxial > 1 && nStereo+nAxial > 5) { + goodFit = tkr.fit(true); + if (debug) System.out.format("KalmanPatRecHPS event %d, result of refitting after removing new hits on track %d = %b \n", + eventNumber, tkr.ID, goodFit); + } else goodFit = false; + } + if (!goodFit) { + if (debug) System.out.format("KalmanPatRecHPS: removing KalTrack %d for bad fit!\n", tkr.ID); + iter.remove(); + for (MeasurementSite site : tkr.SiteList) { + if (site.hitID!=-1) { + if (debug) System.out.format(" removing hit %d from site on layer %d, detector %d\n", site.hitID, site.m.Layer, site.m.detector); + site.m.hits.get(site.hitID).tracks.remove(tkr); + site.removeHit(); + } + } + continue; + } + if (tkr.chi2/(double)tkr.nHits > kPar.chi2mx1[0]) { // Try removing bad hits if the fit is still lousy + ArrayList listSave = tkr.SiteList; + double chi2Save = tkr.chi2; + int nhitSave = tkr.nHits; + int nRemoved; + boolean refitted = false; + do { + if (debug) System.out.format("KalmanPatRecHPS event %d: try removing some bad hits on track %d\n", eventNumber, tkr.ID); + nRemoved = 0; + Iterator itr = tkr.SiteList.iterator(); + while (itr.hasNext()) { + MeasurementSite site = itr.next(); + if (debug) System.out.format("KalmanPatRecHPS: Layer %d stereo=%b, hit %d, chi^2 increment=%10.5f, a=%s\n", + site.m.Layer, site.m.isStereo, site.hitID, site.chi2inc, site.aF.helix.a.toString()); + if (site.hitID < 0) continue; + if (site.chi2inc > chi2Save/(double)nhitSave + 1.6*kPar.mxChi2Inc) { + if (site.m.isStereo && nStereo>3 || !site.m.isStereo && nAxial>2) { + itr.remove(); + nRemoved++; + if (debug) System.out.format("KalmanPatRecHPS: remove hit %d on layer %d of track %d\n", site.hitID, site.m.Layer, tkr.ID); + if (site.m.isStereo) nStereo--; + else nAxial--; } } } + if (debug) System.out.format("KalmanPatRecHPS: hits remaining are %d stereo and %d axial\n", nStereo, nAxial); + if (nRemoved > 0) { + goodFit = tkr.fit(true); + if (goodFit) refitted = true; + if (debug) System.out.format("KalmanPatRecHPS event %d, result of refitting after removing bad hits on track %d = %b %10.5f %d \n", + eventNumber, tkr.ID, goodFit, tkr.chi2, tkr.nHits); + } + } while (nRemoved > 0 && goodFit && tkr.chi2/(double)tkr.nHits > kPar.chi2mx1[0]); + if (!goodFit || (refitted && chi2Save/(double)nhitSave <= tkr.chi2/(double)tkr.nHits)) { // Keep the old fit + if (debug) System.out.format("KalmanPatRecHPS event %d track %d, keeping original fit.\n", eventNumber, tkr.ID); + tkr.SiteList = listSave; + tkr.nHits = nhitSave; + tkr.chi2 = chi2Save; + goodFit = true; + } + } + StateVector aS = tkr.SiteList.get(0).aS; + if (aS == null) { + if (debug) System.out.format("KalmanPatRecHPS: track %d has no smoothed state vector at site 0!\n", tkr.ID); + goodFit = false; + } + if (goodFit) { + double phi0 = aS.helix.planeIntersect(new Plane(new Vec(3,kPar.beamSpot), new Vec(0.,1.,0.))); + if (Double.isNaN(phi0)) { + if (debug) System.out.format("KalmanPatRecHPS: track %d does not intersect the origin plane!\n", tkr.ID); + } else { + if (!tkr.originHelix()) { + if (debug) System.out.format("KalmanPatRecHPS: propagating track %d to the origin failed!\n", tkr.ID); + //goodFit = false; + } + if (debug) { + double runTime = (double)((System.nanoTime() - startTime)/1000000.); + System.out.format(" The origin propagation is finished for track %d at time=%10.6f ms\n", tkr.ID, runTime); + } + } + } + if (goodFit) { // For tracks with few hits, include an origin constraint + if (tkr.nHits == 5) { + if (debug) System.out.format("KalmanPatRecHPS: try an origin constraint on track %d\n", tkr.ID); + HelixState constrHelix = tkr.originConstraint(vtx, vtxCov); + if (constrHelix == null) goodFit = false; + if (goodFit) { + double chi2inc = tkr.chi2incOrigin(); + if (chi2inc <= kPar.mxChi2Vtx) { + tkr.helixAtOrigin = constrHelix; + tkr.chi2 += chi2inc * tkr.nHits; + } else { + goodFit = false; + } + } } } if (!goodFit) { if (debug) System.out.format("KalmanPatRecHPS: removing KalTrack %d for bad fit!\n", tkr.ID); - TkrList.remove(tkr); + iter.remove(); for (MeasurementSite site : tkr.SiteList) { if (site.hitID!=-1) { if (debug) System.out.format(" removing hit %d from site on layer %d, detector %d\n", site.hitID, site.m.Layer, site.m.detector); @@ -1238,6 +1323,7 @@ private boolean removeBadHits(TrackCandidate tkr, int trial) { // Method to smooth an already filtered track candidate private void smoothTrack(TrackCandidate filteredTkr) { MeasurementSite nextSite = null; + boolean badCov = false; for (int idxS = filteredTkr.sites.size() - 1; idxS >= 0; idxS--) { MeasurementSite currentSite = filteredTkr.sites.get(idxS); if (nextSite == null) { // The outermost site with a hit is already smoothed by definition @@ -1248,10 +1334,14 @@ private void smoothTrack(TrackCandidate filteredTkr) { currentSite.smooth(nextSite); } filteredTkr.chi2s += Math.max(currentSite.chi2inc, 0.); - + if (negativeCov(currentSite.aS.helix.C)) { + badCov = true; + fixCov(currentSite.aS.helix.C, currentSite.aS.helix.a); + } nextSite = currentSite; //if (debug) currentSite.print("smoothed"); } + if (badCov) nBadCov[1]++; filteredTkr.smoothed = true; } @@ -1306,6 +1396,7 @@ private void filterTrack(TrackCandidate tkrCandidate, int lyrBegin, // layer on } else { direction = -1; } + boolean badCov = false; boolean needCleanup = false; layerLoop: for (int lyr = lyrBegin; lyr != lyrEnd + direction; lyr += direction) { SiModule mExistingHit = null; @@ -1399,6 +1490,12 @@ private void filterTrack(TrackCandidate tkrCandidate, int lyrBegin, // layer on needCleanup = true; break layerLoop; } + if (negativeCov(newSite.aF.helix.C)) { + if (debug) System.out.format("KalmanPatRecHPS.filtertrack: event %d candidate %d layer %d has negative covariance after filter step.\n", + eventNumber, tkrCandidate.ID, newSite.m.Layer); + badCov = true; + fixCov(newSite.aF.helix.C, newSite.aF.helix.a); + } if (debug) { System.out.format("KalmanPatRecHPS.filterTrack: candidate %d, completed filter at site (%d, %d, %d), chi2-inc=%8.3f\n", tkrCandidate.ID, newSite.m.Layer, newSite.m.detector, newSite.hitID, newSite.chi2inc); @@ -1443,6 +1540,7 @@ private void filterTrack(TrackCandidate tkrCandidate, int lyrBegin, // layer on tkrCandidate.good = false; } } + if (badCov) nBadCov[0]++; tkrCandidate.filtered = true; return; } @@ -1515,6 +1613,63 @@ public int compare(Pair p1, Pair p2) { } }; + static boolean negativeCov(DMatrixRMaj C) { + boolean badCov = false; + for (int i=0; i<5; ++i) { + if (C.unsafe_get(i, i) <= 0.) { + badCov = true; + break; + } + } + return badCov; + } + + static void setInitCov(DMatrixRMaj C, Vec helix, boolean newM) { + C.unsafe_set(0, 0, 0.5); + C.unsafe_set(1, 1, .00005); + C.unsafe_set(2, 2, Math.pow(0.1*helix.v[2],2)); + C.unsafe_set(3, 3, .01); + C.unsafe_set(4, 4, .00001); + if (!newM) { + for (int i=0; i<5; ++i) { + for (int j=0; j<5; ++j) { + if (i != j) C.unsafe_set(i, j, 0.); + } + } + } + } + + static void fixCov(DMatrixRMaj C, Vec helix) { + for (int i=0; i<5; ++i) { + if (C.unsafe_get(i, i) < 0.) { + for (int j=0; j<5; ++j) { + if (j != i) { + C.unsafe_set(i, j, 0.); + C.unsafe_set(j, i, 0.); + } + } + switch (i) { + case 0: + C.unsafe_set(0, 0, 0.5); + break; + case 1: + C.unsafe_set(1, 1, .00005); + break; + case 2: + C.unsafe_set(2, 2, Math.pow(0.1*helix.v[2],2)); + break; + case 3: + C.unsafe_set(3, 3, .01); + break; + case 4: + C.unsafe_set(4, 4, .00001); + break; + } + + } + } + } + // Quick check on where the seed track is heading, using only the two axial layers in the seed private boolean seedNoGood(int j, int iter) { // j must point to an axial layer in the seed. diff --git a/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanPatRecPlots.java b/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanPatRecPlots.java index 7de048ff61..9bebb66dbb 100644 --- a/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanPatRecPlots.java +++ b/tracking/src/main/java/org/hps/recon/tracking/kalman/KalmanPatRecPlots.java @@ -10,6 +10,7 @@ import java.util.logging.Level; import java.util.logging.Logger; import org.hps.recon.tracking.TrackUtils; +import org.ejml.data.DMatrixRMaj; import org.ejml.dense.row.MatrixFeatures_DDRM; import org.hps.recon.tracking.FittedRawTrackerHit; import org.hps.recon.tracking.MaterialSupervisor.SiStripPlane; @@ -150,6 +151,12 @@ class KalmanPatRecPlots { aida.histogram1D("Kalman track time range (ns)", 100, 0., 100.); aida.histogram1D("GBL number of hits",20,0.,20.); aida.histogram1D("Kalman layer hit",20,0.,20.); + aida.histogram1D("drho error estimate",50,0.,1.); + aida.histogram1D("phi0 error estimate",50,0.,.01); + aida.histogram1D("ptInv error estimate",50,0.,0.2); + aida.histogram1D("ptInv relative error estimate",50,0.,0.5); + aida.histogram1D("dz error estimate",50,0.,0.2); + aida.histogram1D("tanl error estimate",50,0.,.005); for (int lyr=0; lyr<14; ++lyr) { aida.histogram1D(String.format("Layers/Kalman missed hit residual in layer %d",lyr), 100, -1.0, 1.0); aida.histogram1D(String.format("Layers/Kalman track hit residual in layer %d",lyr), 100, -0.1, 0.1); @@ -420,6 +427,16 @@ void process(EventHeader event, List sensors, ArrayList[] logger.warning(String.format("Event %d, eigenvalue %d of covariance is negative!", event.getEventNumber(), i)); } } + if (kTk.nHits >= 10) { + DMatrixRMaj thisCov = kTk.SiteList.get(0).aS.helix.C; + double ptInv1 = kTk.SiteList.get(0).aS.helix.a.v[2]; + aida.histogram1D("drho error estimate").fill(Math.sqrt(thisCov.unsafe_get(0, 0))); + aida.histogram1D("phi0 error estimate").fill(Math.sqrt(thisCov.unsafe_get(1, 1))); + aida.histogram1D("ptInv relative error estimate").fill(Math.sqrt(thisCov.unsafe_get(2, 2))/Math.abs(ptInv1)); + aida.histogram1D("ptInv error estimate").fill(Math.sqrt(thisCov.unsafe_get(2, 2))); + aida.histogram1D("dz error estimate").fill(Math.sqrt(thisCov.unsafe_get(3, 3))); + aida.histogram1D("tanl error estimate").fill(Math.sqrt(thisCov.unsafe_get(4, 4))); + } // Histogram residuals of hits in layers with no hits on the track and with hits ArrayList mcParts = new ArrayList(); diff --git a/tracking/src/main/java/org/hps/recon/tracking/kalman/MeasurementSite.java b/tracking/src/main/java/org/hps/recon/tracking/kalman/MeasurementSite.java index e4bb2e59e4..7527d428b6 100644 --- a/tracking/src/main/java/org/hps/recon/tracking/kalman/MeasurementSite.java +++ b/tracking/src/main/java/org/hps/recon/tracking/kalman/MeasurementSite.java @@ -87,7 +87,7 @@ String toString(String s) { conFac = 1.0e12 / c; Vec Bfield = KalmanInterface.getField(m.p.X(), m.Bfield); B = Bfield.mag(); - alpha = conFac / B; // Convert from pt in GeV to curvature in mm + alpha = conFac / B; // Convert from 1/pt in 1/GeV to curvature in mm predicted = false; filtered = false; smoothed = false; @@ -204,9 +204,9 @@ int makePrediction(StateVector pS, SiModule mPs, int hitNumber, boolean sharingO arcLength = 0.; } else { double ct = pMom.unitVec().dot(mPs.p.T()); // cos(theta) at the **previous** site - double radius = Math.abs(alpha/pS.helix.a.v[2]); + double radius = alpha/pS.helix.a.v[2]; XL = mPs.thickness / radLen / Math.abs(ct); // Si scattering thickness at previous site - arcLength = radius*phi*FastMath.sqrt(1.0 + pS.helix.a.v[4] * pS.helix.a.v[4]); + arcLength = -radius*phi*FastMath.sqrt(1.0 + pS.helix.a.v[4] * pS.helix.a.v[4]); if (debug) { double dx = m.p.X().v[0]-mPs.p.X().v[0]; double dy = m.p.X().v[1]-mPs.p.X().v[1]; diff --git a/tracking/src/main/java/org/hps/recon/tracking/kalman/SeedTrack.java b/tracking/src/main/java/org/hps/recon/tracking/kalman/SeedTrack.java index 83e5797193..3a2397a9c6 100644 --- a/tracking/src/main/java/org/hps/recon/tracking/kalman/SeedTrack.java +++ b/tracking/src/main/java/org/hps/recon/tracking/kalman/SeedTrack.java @@ -269,10 +269,19 @@ private void SeedTracker(ArrayList hitList, double yOrigin, double yTarg CommonOps_DDRM.multTransB(CovA,D,Mint); C = new DMatrixRMaj(5); CommonOps_DDRM.mult(D, Mint, C); - if (debug) { - System.out.println("line/parabola to helix derivatives:"); - D.print(); - } + //boolean badC = false; + //for (int i=0; i<5; ++i) { + // if (C.unsafe_get(i, i) < 0.) { + // badC = false; + // System.out.format("SeedTrack: negative covariance matrix found!\n"); + // break; + // } + //} + //if (debug || badC) { + // System.out.println("line/parabola to helix derivatives and final covariance:"); + // D.print(); + // C.print(); + //} // Note that the non-bending plane is assumed to be y,z (B field in z // direction), and the track is assumed to start out more-or-less diff --git a/tracking/src/main/java/org/hps/recon/tracking/kalman/SquareMatrix.java b/tracking/src/main/java/org/hps/recon/tracking/kalman/SquareMatrix.java index 056525bbf6..ba48912864 100644 --- a/tracking/src/main/java/org/hps/recon/tracking/kalman/SquareMatrix.java +++ b/tracking/src/main/java/org/hps/recon/tracking/kalman/SquareMatrix.java @@ -209,7 +209,7 @@ SquareMatrix fastInvert( ) { indxc[i] = icol; if (this.M[icol][icol] == 0.0) { //System.out.format("Singular matrix in SquareMatrix.java method invert.\n"); - return this; + return null; } double pivinv = 1.0 / this.M[icol][icol]; this.M[icol][icol] = 1.0; diff --git a/tracking/src/main/java/org/hps/recon/tracking/kalman/StateVector.java b/tracking/src/main/java/org/hps/recon/tracking/kalman/StateVector.java index 7938859c20..52f14fe2c6 100644 --- a/tracking/src/main/java/org/hps/recon/tracking/kalman/StateVector.java +++ b/tracking/src/main/java/org/hps/recon/tracking/kalman/StateVector.java @@ -282,10 +282,19 @@ StateVector filter(DMatrixRMaj H, double V) { directProd(K, H, tempM); CommonOps_DDRM.scale(-1.0, tempM); CommonOps_DDRM.addEquals(tempM, U); - aPrime.helix.C = new DMatrixRMaj(5,5); CommonOps_DDRM.mult(tempM, helix.C, aPrime.helix.C); if (debug) { + System.out.format("StateVector.filter: compare covariance calculations, original one first:\n"); + // Alternate calculation of the covariance update + double R = V*(1- CommonOps_DDRM.dot(H, K)); + CommonOps_DDRM.mult(tempM, helix.C, tempV); + CommonOps_DDRM.multTransB(tempV, tempM, tempA); + directProd(K, K, tempV); + DMatrixRMaj tempZ = new DMatrixRMaj(5,5); + CommonOps_DDRM.add(tempA, R, tempV, tempZ); + aPrime.helix.C.print(); + tempZ.print(); System.out.println("filtered covariance (gain-matrix formalism) in StateVector.filter:"); aPrime.helix.C.print(); // Alternative calculation of filtered covariance (sanity check that it gives @@ -303,8 +312,6 @@ StateVector filter(DMatrixRMaj H, double V) { helix.a.print("predicted helix parameters"); aPrime.helix.a.print("filtered helix parameters (gain matrix formalism)"); } - //double R = (1 - H.dot(K))*V; - //System.out.format("StateVector.filter: R=%10.8f\n", R); return aPrime; } @@ -337,37 +344,51 @@ StateVector smooth(StateVector snS, StateVector snP) { snS.kLow, snS.kUp, snP.kLow, snP.kUp); StateVector sS = this.copy(); - // solver.setA defines the input matrix and checks whether it is singular. A copy is needed because the input gets modified. + // solver.setA defines the input matrix and checks whether it is singular. + // A copy is needed because the input gets modified. if (!solver.setA(snP.helix.C.copy())) { - logger.fine("StateVector:smooth, inversion of the covariance matrix failed"); - //snP.helix.C.print(); - //SquareMatrix invrs = KalTrack.mToS(snP.helix.C).invert(); - //invrs.print("inverse"); - //invrs.multiply(KalTrack.mToS(snP.helix.C)).print("unit matrix?"); - for (int i=0; i<5; ++i) { // Fill the inverse with something not too crazy and continue . . . - for (int j=0; j<5; ++j) { - if (i == j) { - Cinv.unsafe_set(i,j,1.0/Cinv.unsafe_get(i,j)); - } else { - Cinv.unsafe_set(i, j, 0.); + SquareMatrix invrs = KalTrack.mToS(snP.helix.C).fastInvert(); + if (invrs == null) { + logger.warning("StateVector:smooth, inversion of the covariance matrix failed"); + snP.helix.C.print(); + for (int i=0; i<5; ++i) { // Fill the inverse with something not too crazy and continue . . . + for (int j=0; j<5; ++j) { + if (i == j) { + Cinv.unsafe_set(i,j,1.0/snP.helix.C.unsafe_get(i,j)); + } else { + Cinv.unsafe_set(i, j, 0.); + snP.helix.C.unsafe_set(i, j, 0.); + } } + } + } else { + if (debug) { + KalTrack.mToS(snP.helix.C).print("singular covariance?"); + invrs.print("inverse"); + invrs.multiply(KalTrack.mToS(snP.helix.C)).print("unit matrix?"); } - } + for (int i=0; i<5; ++i) { + for (int j=0; j<5; ++j) { + Cinv.unsafe_set(i, j, invrs.M[i][j]); + } + } + } } else { solver.invert(Cinv); } - if (debug) { + + CommonOps_DDRM.multTransB(helix.C, sS.F, tempM); + CommonOps_DDRM.mult(tempM, Cinv, tempA); + + vecToM(snS.helix.a.dif(snP.helix.a), tempV); + CommonOps_DDRM.mult(tempA, tempV, tempV2); + sS.helix.a = helix.a.sum(mToVec(tempV2)); + if (debug || Math.abs(tempV2.unsafe_get(1, 0))>1.5) { System.out.println("StateVector:smooth, inverse of the covariance:"); Cinv.print("%11.6e"); CommonOps_DDRM.mult(snP.helix.C, Cinv, tempM); System.out.format("Unit matrix?? "); tempM.print(); - } - CommonOps_DDRM.multTransB(helix.C, sS.F, tempM); - CommonOps_DDRM.mult(tempM, Cinv, tempA); - - vecToM(snS.helix.a.dif(snP.helix.a), tempV); - if (debug) { System.out.format("Predicted helix covariance: "); snP.helix.C.print(); System.out.format("This helix covariance: "); @@ -380,10 +401,6 @@ StateVector smooth(StateVector snS, StateVector snP) { tempA.print(); System.out.format("Difference of helix parameters tempV: "); tempV.print(); - } - CommonOps_DDRM.mult(tempA, tempV, tempV2); - sS.helix.a = helix.a.sum(mToVec(tempV2)); - if (debug) { System.out.format("tempV2 "); tempV2.print(); sS.helix.a.print("new helix parameters"); @@ -407,8 +424,7 @@ Vec helixErrors(Vec aPrime) { Math.sqrt(tC.unsafe_get(3,3)), Math.sqrt(tC.unsafe_get(4,4))); } - // Transform the helix covariance to new pivot point (specified in local - // coordinates) + // Transform the helix covariance to new pivot point (specified in local coordinates) DMatrixRMaj covariancePivotTransform(Vec aP) { // aP are the helix parameters for the new pivot point, assumed already to be // calculated by pivotTransform() @@ -419,7 +435,7 @@ DMatrixRMaj covariancePivotTransform(Vec aP) { CommonOps_DDRM.mult(mF, tempM, tempA); return tempA; } - + // Go to and from 1D EJML matrix for a vector Vec static void vecToM(Vec a, DMatrixRMaj m) { for (int i=0; i