Skip to content

Kalman dev #842

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
pbutti merged 4 commits into from
Apr 13, 2021
Merged

Kalman dev #842

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
443c7a9
Some extensive changes to reduce the frequency of negative covariance…
robertprestonjohnson Mar 17, 2021
6fb1ebd
Added tracking and correction of negative covariance incidents
robertprestonjohnson Apr 6, 2021
23beb7b
Fixed covariance calculation in StateVector.filter, back to the origi…
robertprestonjohnson Apr 8, 2021
f2f422e
Corrected the arc length not to depend on the curvature sign
robertprestonjohnson Apr 8, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
162 changes: 109 additions & 53 deletions tracking/src/main/java/org/hps/recon/tracking/kalman/KalTrack.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@ public class KalTrack {
public int ID;
public int nHits;
public double chi2;
private double reducedChi2;

ArrayList<MeasurementSite> SiteList;
// call the corresponding functions to create and access the following two maps
Expand All @@ -35,6 +36,7 @@ public class KalTrack {
Map<Integer, MeasurementSite> millipedeMap;
Map<Integer, MeasurementSite> lyrMap;
public int eventNumber;
public boolean bad;
HelixState helixAtOrigin;
private boolean propagated;
private RotMatrix Rot;
Expand All @@ -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;
Expand All @@ -59,15 +61,18 @@ public class KalTrack {
private static boolean initialized;
private double [] arcLength;
private static LinearSolverDense<DMatrixRMaj> solver;
static int [] nBadCov = {0, 0};

KalTrack(int evtNumb, int tkID, ArrayList<MeasurementSite> SiteList, ArrayList<Double> yScat, ArrayList<Double> 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());
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -125,21 +131,25 @@ 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++;
time += site.m.hits.get(site.hitID).time;
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="";
Expand Down Expand Up @@ -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";
Expand Down Expand Up @@ -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");
Expand All @@ -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) {
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -1103,62 +1114,107 @@ public int addHits(ArrayList<SiModule> 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<MeasurementSite> newSiteList = new ArrayList<MeasurementSite>(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;
}

Expand Down
Loading