From 443c7a9f062357d794290331b521841d47b31959 Mon Sep 17 00:00:00 2001 From: robertprestonjohnson Date: Wed, 17 Mar 2021 12:08:19 -0700 Subject: [PATCH 1/4] Some extensive changes to reduce the frequency of negative covariance. Also, improvements in refinement of already found tracks (e.g. dropping bad hits). Negative covariance still does happen, so more improvements are in the works. At least these changes prevent those tracks from becoming completely crazy. --- .../hps/recon/tracking/kalman/KalTrack.java | 139 +++++++----- .../tracking/kalman/KalmanInterface.java | 32 ++- .../recon/tracking/kalman/KalmanParams.java | 2 +- .../tracking/kalman/KalmanPatRecDriver.java | 10 +- .../tracking/kalman/KalmanPatRecHPS.java | 198 ++++++++++++++---- .../tracking/kalman/KalmanPatRecPlots.java | 17 ++ .../tracking/kalman/MeasurementSite.java | 2 +- .../hps/recon/tracking/kalman/SeedTrack.java | 17 +- .../recon/tracking/kalman/SquareMatrix.java | 2 +- .../recon/tracking/kalman/StateVector.java | 82 +++++--- .../org/hps/recon/tracking/kalman/Vec.java | 2 +- 11 files changed, 366 insertions(+), 137 deletions(-) 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..43461c6126 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 @@ -50,7 +51,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; @@ -68,6 +69,7 @@ public class KalTrack { this.kPar = kPar; ID = tkID; arcLength = null; + //debug = (evtNumb == 217481); if (!initialized) { logger = Logger.getLogger(KalTrack.class.getName()); @@ -125,6 +127,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,8 +135,10 @@ 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; @@ -615,7 +620,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 +638,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 +989,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 +1109,93 @@ 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; - 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 (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(10., sH.helix.C); // Blow up the initial covariance matrix to avoid double counting measurements + } + SiModule prevMod = null; + double chi2f = 0.; + ArrayList newSiteList = new ArrayList(SiteList.size()); + 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); + + 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 (debug) { + if (KalmanPatRecHPS.negativeCov(currentSite.aF.helix.C)) { + System.out.format("KalTrack: event %d, ID %d, negative covariance after filtering at layer %d\n", + eventNumber,ID,currentSite.m.Layer); } - - if (currentSite.hitID >= 0 && debug) chi2f += Math.max(currentSite.chi2inc,0.); - - sH = currentSite.aF; - prevMod = currentSite.m; + if (newSite.hitID >= 0) chi2f += Math.max(currentSite.chi2inc,0.); } - if (debug) System.out.format("KalTrack.fit: Iteration %d, Fit chi^2 after filtering = %12.4e\n", iteration, chi2f); - - 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); + 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 (debug) System.out.format("KalTrack.fit: Track %d, Fit chi^2 after filtering = %12.4e\n", ID, chi2f); + + chi2s = 0.; + int nNewHits = 0; + 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 (debug) { + if (KalmanPatRecHPS.negativeCov(currentSite.aS.helix.C)) { + System.out.format("KalTrack: event %d, ID %d, negative covariance after smoothing at layer %d\n", + eventNumber,ID,currentSite.m.Layer); } - if (currentSite.hitID >= 0) chi2s += Math.max(currentSite.chi2inc,0.); - - nextSite = currentSite; + 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 (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..bd878a5dbf 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; @@ -390,6 +391,8 @@ public void printGBLStripClusterData(GBLStripClusterData clstr) { public List createGBLStripClusterData(KalTrack kT) { List rtnList = new ArrayList(kT.SiteList.size()); + if (eventNumber == 210857) kT.print("dump"); + double phiLast = 9999.; for (MeasurementSite site : kT.SiteList) { GBLStripClusterData clstr = new GBLStripClusterData(kT.SiteList.indexOf(site)); @@ -429,6 +432,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) @@ -455,6 +462,7 @@ public List createGBLStripClusterData(KalTrack kT) { clstr.setScatterAngle(HelixState.projMSangle(momentum.mag(), XL)); rtnList.add(clstr); + if (eventNumber == 210857) printGBLStripClusterData(clstr); } return rtnList; } @@ -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..2fbe38059f 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 @@ -398,8 +398,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 +1067,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 +1082,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 +1117,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 +1131,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); @@ -1400,6 +1484,10 @@ private void filterTrack(TrackCandidate tkrCandidate, int lyrBegin, // layer on break layerLoop; } if (debug) { + if (negativeCov(newSite.aF.helix.C)) { + System.out.format("KalmanPatRecHPS.filtertrack: event %d candidate %d layer %d has negative covariance after filter step.\n", + eventNumber, tkrCandidate.ID, newSite.m.Layer); + } 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); } @@ -1515,6 +1603,32 @@ 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.); + } + } + } + } + // 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..0deb0c248c 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; 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..1818ea78e6 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,22 @@ 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); + + // Alternate calculation of the covariance update + //double R = (1 - H.dot(K))*V; + //System.out.format("StateVector.filter: R=%10.8f\n", R); + 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); + //aPrime.helix.C = new DMatrixRMaj(5,5); + CommonOps_DDRM.add(tempA, R, tempV, aPrime.helix.C); if (debug) { + System.out.format("StateVector.filter: compare covariance calculations, stable one first:\n"); + CommonOps_DDRM.mult(tempM, helix.C, tempV); + aPrime.helix.C.print(); + tempV.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 +315,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 +347,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 +404,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 +427,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 +438,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 Date: Tue, 6 Apr 2021 13:15:05 -0700 Subject: [PATCH 2/4] Added tracking and correction of negative covariance incidents --- .../hps/recon/tracking/kalman/KalTrack.java | 39 ++++++++++---- .../tracking/kalman/KalmanInterface.java | 4 +- .../tracking/kalman/KalmanPatRecHPS.java | 51 +++++++++++++++++-- 3 files changed, 77 insertions(+), 17 deletions(-) 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 43461c6126..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 @@ -36,6 +36,7 @@ public class KalTrack { Map millipedeMap; Map lyrMap; public int eventNumber; + public boolean bad; HelixState helixAtOrigin; private boolean propagated; private RotMatrix Rot; @@ -60,10 +61,12 @@ 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; @@ -100,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); @@ -145,6 +149,7 @@ public class KalTrack { 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=""; @@ -371,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"; @@ -1119,11 +1124,12 @@ public boolean fit(boolean keep) { 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(10., sH.helix.C); // Blow up the initial covariance matrix to avoid double counting measurements + CommonOps_DDRM.scale(100., sH.helix.C); // Blow up the initial covariance matrix to avoid double counting measurements } 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); @@ -1140,11 +1146,13 @@ public boolean fit(boolean keep) { 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 (KalmanPatRecHPS.negativeCov(currentSite.aF.helix.C)) { - System.out.format("KalTrack: event %d, ID %d, negative covariance after filtering at layer %d\n", - eventNumber,ID,currentSite.m.Layer); - } if (newSite.hitID >= 0) chi2f += Math.max(currentSite.chi2inc,0.); } newSite.hitID = currentSite.hitID; @@ -1155,10 +1163,15 @@ public boolean fit(boolean keep) { 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); @@ -1172,16 +1185,22 @@ public boolean fit(boolean keep) { 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) { - if (KalmanPatRecHPS.negativeCov(currentSite.aS.helix.C)) { - System.out.format("KalTrack: event %d, ID %d, negative covariance after smoothing at layer %d\n", - eventNumber,ID,currentSite.m.Layer); - } 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) { 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 bd878a5dbf..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 @@ -214,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 @@ -391,7 +393,6 @@ public void printGBLStripClusterData(GBLStripClusterData clstr) { public List createGBLStripClusterData(KalTrack kT) { List rtnList = new ArrayList(kT.SiteList.size()); - if (eventNumber == 210857) kT.print("dump"); double phiLast = 9999.; for (MeasurementSite site : kT.SiteList) { GBLStripClusterData clstr = new GBLStripClusterData(kT.SiteList.indexOf(site)); @@ -462,7 +463,6 @@ public List createGBLStripClusterData(KalTrack kT) { clstr.setScatterAngle(HelixState.projMSangle(momentum.mag(), XL)); rtnList.add(clstr); - if (eventNumber == 210857) printGBLStripClusterData(clstr); } return rtnList; } 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 2fbe38059f..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.; @@ -1322,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 @@ -1332,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; } @@ -1390,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; @@ -1483,11 +1490,13 @@ 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) { - if (negativeCov(newSite.aF.helix.C)) { - System.out.format("KalmanPatRecHPS.filtertrack: event %d candidate %d layer %d has negative covariance after filter step.\n", - eventNumber, tkrCandidate.ID, newSite.m.Layer); - } 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); } @@ -1531,6 +1540,7 @@ private void filterTrack(TrackCandidate tkrCandidate, int lyrBegin, // layer on tkrCandidate.good = false; } } + if (badCov) nBadCov[0]++; tkrCandidate.filtered = true; return; } @@ -1628,6 +1638,37 @@ static void setInitCov(DMatrixRMaj C, Vec helix, boolean newM) { } } } + + 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) { From 23beb7b4340a7734c6dc83fd880bff1cc49fc857 Mon Sep 17 00:00:00 2001 From: robertprestonjohnson Date: Thu, 8 Apr 2021 14:11:29 -0700 Subject: [PATCH 3/4] Fixed covariance calculation in StateVector.filter, back to the original formula. The new formula clearly wasn't working. --- .../recon/tracking/kalman/StateVector.java | 21 ++++++++----------- .../hps/recon/tracking/kalman/TestMain.java | 4 ++-- 2 files changed, 11 insertions(+), 14 deletions(-) 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 1818ea78e6..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,22 +282,19 @@ StateVector filter(DMatrixRMaj H, double V) { directProd(K, H, tempM); CommonOps_DDRM.scale(-1.0, tempM); CommonOps_DDRM.addEquals(tempM, U); - - // Alternate calculation of the covariance update - //double R = (1 - H.dot(K))*V; - //System.out.format("StateVector.filter: R=%10.8f\n", R); - 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); - //aPrime.helix.C = new DMatrixRMaj(5,5); - CommonOps_DDRM.add(tempA, R, tempV, aPrime.helix.C); + CommonOps_DDRM.mult(tempM, helix.C, aPrime.helix.C); if (debug) { - System.out.format("StateVector.filter: compare covariance calculations, stable one first:\n"); + 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(); - tempV.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 diff --git a/tracking/src/main/java/org/hps/recon/tracking/kalman/TestMain.java b/tracking/src/main/java/org/hps/recon/tracking/kalman/TestMain.java index cb52e8aa2b..3fb023ba42 100644 --- a/tracking/src/main/java/org/hps/recon/tracking/kalman/TestMain.java +++ b/tracking/src/main/java/org/hps/recon/tracking/kalman/TestMain.java @@ -55,8 +55,8 @@ public static void main(String[] args) { matInv.print(); res.print(); } - //HelixTest3 t1 = new HelixTest3(path); - PatRecTest t1 = new PatRecTest(path); + HelixTest3 t1 = new HelixTest3(path); + //PatRecTest t1 = new PatRecTest(path); } public TestMain() { From f2f422eac56b316392eba5c8de5fd2eef9171ede Mon Sep 17 00:00:00 2001 From: robertprestonjohnson Date: Thu, 8 Apr 2021 15:13:21 -0700 Subject: [PATCH 4/4] Corrected the arc length not to depend on the curvature sign --- .../java/org/hps/recon/tracking/kalman/MeasurementSite.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 0deb0c248c..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 @@ -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];