Skip to content

fix: correct calculated doca in KF tracking #1108

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

Draft
tongtongcao wants to merge 1 commit into
base: development
Choose a base branch
from
+105 −47
Draft

fix: correct calculated doca in KF tracking #1108

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
20 changes: 15 additions & 5 deletions common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AMeasVecs.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
import org.jlab.geom.prim.Line3D;
import org.jlab.geom.prim.Point3D;
import org.jlab.geom.prim.Transformation3D;
import org.jlab.geom.prim.Vector3D;

/**
*
Expand Down Expand Up @@ -50,7 +51,9 @@ public double[] dhDoca(int k, StateVec stateVec) {

Surface surf = this.measurements.get(stateVec.k).surface;
Point3D point = new Point3D(stateVec.x, stateVec.y, stateVec.z);
double h = hDoca(point, surf.wireLine[0]);
Vector3D dir = new Vector3D(stateVec.tx, stateVec.ty, 1);
Line3D line = new Line3D(point, dir);
double h = hDoca(line, surf.wireLine[0]);

double signMeas = 1;
double sign = 1;
Expand All @@ -66,7 +69,7 @@ public double[] dhDoca(int k, StateVec stateVec) {

//USE THE DOUBLE HIT
if(surf.doca[1]!=-99) {
h = hDoca(point, surf.wireLine[1]);
h = hDoca(line, surf.wireLine[1]);

signMeas = Math.signum(surf.doca[1]);
sign = Math.signum(h);
Expand All @@ -78,13 +81,20 @@ public double[] dhDoca(int k, StateVec stateVec) {
}

// Return a signed doca for DC
public double hDoca(Point3D point, Line3D wireLine) {
// Suppose that trajectory is line at the given layer, which is defined by the state vector with point(x, y, z) and dir(tx, ty, 1)
public double hDoca(Line3D trajLine, Line3D wireLine) {

// Define sign of calculated doca to be consistent with LR definition of measured doca
Line3D WL = new Line3D();
WL.copy(wireLine);
WL.copy(WL.distance(point));
WL.copy(WL.distance(trajLine.origin()));

return WL.length()*Math.signum(WL.direction().x());
// Get a line, which is commonly perpendicular to trajectory line and wire line
Line3D cpLine = new Line3D();
cpLine.copy(trajLine);
cpLine.copy(cpLine.distance(wireLine));

return cpLine.length()*Math.signum(WL.direction().x());
}

public double dh(int k, StateVec stateVec) {
Expand Down
70 changes: 45 additions & 25 deletions ...s/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/KFitter.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
import org.jlab.clas.tracking.utilities.RungeKuttaDoca;
import org.jlab.clas.tracking.utilities.MatrixOps.Libr;
import org.jlab.geom.prim.Point3D;
import org.jlab.geom.prim.Vector3D;
import org.jlab.geom.prim.Line3D;
import org.jlab.jnp.matrix.*;

/**
Expand Down Expand Up @@ -592,7 +594,7 @@ private boolean filter(int k, boolean forward, double annealingFactor) {

double[] K = new double[5];
double V = effectiveVar;
double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), mVec.surface.wireLine[0]);
double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), sVec.tx, sVec.ty, mVec.surface.wireLine[0]);
Matrix CaInv = this.filterCovMat(H, sVec.CM, V);
if (CaInv != null) {
Matrix5x5.copy(CaInv, cMat);
Expand All @@ -607,7 +609,9 @@ private boolean filter(int k, boolean forward, double annealingFactor) {
}

Point3D point = new Point3D(sVec.x, sVec.y, mVec.surface.measPoint.z());
double h = mv.hDoca(point, mVec.surface.wireLine[0]);
Vector3D dir = new Vector3D(sVec.tx, sVec.ty, 1);
Line3D line = new Line3D(point, dir);
double h = mv.hDoca(line, mVec.surface.wireLine[0]);

c2 = (effectiveDoca - h) * (effectiveDoca - h) / V;

Expand All @@ -623,7 +627,9 @@ private boolean filter(int k, boolean forward, double annealingFactor) {
+ K[4] * (effectiveDoca - h);

Point3D pointFiltered = new Point3D(x_filt, y_filt, mVec.surface.measPoint.z());
double h0 = mv.hDoca(pointFiltered, mVec.surface.wireLine[0]);
Vector3D dirFiltered = new Vector3D(tx_filt, ty_filt, 1);
Line3D lineFiltered = new Line3D(pointFiltered, dirFiltered);
double h0 = mv.hDoca(lineFiltered, mVec.surface.wireLine[0]);

double residual = effectiveDoca - h0;
updatedWeights_singleHit = daf.calc_updatedWeight_singleHit(residual, annealingFactor);
Expand All @@ -644,7 +650,7 @@ private boolean filter(int k, boolean forward, double annealingFactor) {

double[] K = new double[5];
double V = effectiveVar;
double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), mVec.surface.wireLine[indexReferenceWire]);
double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), sVec.tx, sVec.ty, mVec.surface.wireLine[indexReferenceWire]);
Matrix CaInv = this.filterCovMat(H, sVec.CM, V);
if (CaInv != null) {
Matrix5x5.copy(CaInv, cMat);
Expand All @@ -659,7 +665,9 @@ private boolean filter(int k, boolean forward, double annealingFactor) {
}

Point3D point = new Point3D(sVec.x, sVec.y, mVec.surface.measPoint.z());
double h = mv.hDoca(point, mVec.surface.wireLine[indexReferenceWire]);
Vector3D dir = new Vector3D(sVec.tx, sVec.ty, 1);
Line3D line = new Line3D(point, dir);
double h = mv.hDoca(line, mVec.surface.wireLine[indexReferenceWire]);

c2 = (effectiveDoca - h) * (effectiveDoca - h) / V;

Expand All @@ -675,8 +683,10 @@ private boolean filter(int k, boolean forward, double annealingFactor) {
+ K[4] * (effectiveDoca - h);

Point3D pointFiltered = new Point3D(x_filt, y_filt, mVec.surface.measPoint.z());
double h0 = mv.hDoca(pointFiltered, mVec.surface.wireLine[0]);
double h1 = mv.hDoca(pointFiltered, mVec.surface.wireLine[1]);
Vector3D dirFiltered = new Vector3D(tx_filt, ty_filt, 1);
Line3D lineFiltered = new Line3D(pointFiltered, dirFiltered);
double h0 = mv.hDoca(lineFiltered, mVec.surface.wireLine[0]);
double h1 = mv.hDoca(lineFiltered, mVec.surface.wireLine[1]);
double[] residuals = {mVec.surface.doca[0] - h0, mVec.surface.doca[1] - h1};
updatedWeights_doubleHits = daf.calc_updatedWeights_doubleHits(residuals, annealingFactor);
}
Expand Down Expand Up @@ -725,7 +735,7 @@ private boolean filter(int k, boolean forward) {

double[] K = new double[5];
double V = mVec.surface.unc[0] * KFScale;
double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), mVec.surface.wireLine[0]);
double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), sVec.tx, sVec.ty, mVec.surface.wireLine[0]);
Matrix CaInv = this.filterCovMat(H, sVec.CM, V);
Matrix cMat = new Matrix();
if (CaInv != null) {
Expand All @@ -741,7 +751,9 @@ private boolean filter(int k, boolean forward) {
}

Point3D point = new Point3D(sVec.x, sVec.y, mVec.surface.measPoint.z());
double h = mv.hDoca(point, mVec.surface.wireLine[0]);
Vector3D dir = new Vector3D(sVec.tx, sVec.ty, 1);
Line3D line = new Line3D(point, dir);
double h = mv.hDoca(line, mVec.surface.wireLine[0]);

double signMeas = 1;
double sign = 1;
Expand Down Expand Up @@ -774,8 +786,7 @@ private boolean filter(int k, boolean forward) {
if (mVec.surface.doca[1] != -99) {
// now filter using the other Hit
V = mVec.surface.unc[1] * KFScale;
H = mv.H(x_filt, y_filt, mVec.surface.measPoint.z(),
mVec.surface.wireLine[1]);
H = mv.H(x_filt, y_filt, mVec.surface.measPoint.z(), tx_filt, ty_filt, mVec.surface.wireLine[1]);
CaInv = this.filterCovMat(H, cMat, V);
if (CaInv != null) {
for (int i = 0; i < 5; i++) {
Expand All @@ -791,8 +802,9 @@ private boolean filter(int k, boolean forward) {
}

Point3D point2 = new Point3D(x_filt, y_filt, mVec.surface.measPoint.z());

h = mv.hDoca(point2, mVec.surface.wireLine[1]);
Vector3D dir2 = new Vector3D(tx_filt, ty_filt, 1);
Line3D line2 = new Line3D(point2, dir2);
h = mv.hDoca(line2, mVec.surface.wireLine[1]);

signMeas = Math.signum(mVec.surface.doca[1]);
sign = Math.signum(h);
Expand Down Expand Up @@ -889,7 +901,9 @@ public void calcFinalChisq(int sector, boolean nofilter) {
double V0 = mv.measurements.get(0).surface.unc[0];

Point3D point = new Point3D(svc.x, svc.y, mv.measurements.get(0).surface.measPoint.z());
double h0 = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[0]);
Vector3D dir = new Vector3D(svc.tx, svc.ty, 1);
Line3D line = new Line3D(point, dir);
double h0 = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[0]);

svc.setProjector(mv.measurements.get(0).surface.wireLine[0].origin().x());
svc.setProjectorDoca(h0);
Expand All @@ -900,7 +914,7 @@ public void calcFinalChisq(int sector, boolean nofilter) {
//USE THE DOUBLE HIT
if (mv.measurements.get(0).surface.doca[1] != -99) {
V0 = mv.measurements.get(0).surface.unc[1];
h0 = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[1]);
h0 = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[1]);
res = (mv.measurements.get(0).surface.doca[1] - h0);
chi2 += (mv.measurements.get(0).surface.doca[1] - h0) * (mv.measurements.get(0).surface.doca[1] - h0) / V0;
nRj[mv.measurements.get(0).region - 1] += res * res / mv.measurements.get(0).error;
Expand All @@ -922,8 +936,10 @@ public void calcFinalChisq(int sector, boolean nofilter) {
double V = mv.measurements.get(k1 + 1).surface.unc[0];

point = new Point3D(sv.transported(forward).get(k1 + 1).x, sv.transported(forward).get(k1 + 1).y, mv.measurements.get(k1 + 1).surface.measPoint.z());
dir = new Vector3D(sv.transported(forward).get(k1 + 1).tx, sv.transported(forward).get(k1 + 1).ty, 1);
line = new Line3D(point, dir);

double h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[0]);
double h = mv.hDoca(line, mv.measurements.get(k1 + 1).surface.wireLine[0]);
svc = sv.transported(forward).get(k1 + 1);
path += (forward ? 1 : -1) * svc.deltaPath;
svc.setPathLength(path);
Expand All @@ -936,7 +952,7 @@ public void calcFinalChisq(int sector, boolean nofilter) {
//USE THE DOUBLE HIT
if (mv.measurements.get(k1 + 1).surface.doca[1] != -99) {
V = mv.measurements.get(k1 + 1).surface.unc[1];
h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[1]);
h = mv.hDoca(line, mv.measurements.get(k1 + 1).surface.wireLine[1]);
res = (mv.measurements.get(k1 + 1).surface.doca[1] - h);
chi2 += (mv.measurements.get(k1 + 1).surface.doca[1] - h) * (mv.measurements.get(k1 + 1).surface.doca[1] - h) / V;
nRj[mv.measurements.get(k1 + 1).region - 1] += res * res / V;
Expand Down Expand Up @@ -982,6 +998,8 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
svc.setPathLength(path);

Point3D point = new Point3D(svc.x, svc.y, mv.measurements.get(0).surface.measPoint.z());
Vector3D dir = new Vector3D(svc.tx, svc.ty, 1);
Line3D line = new Line3D(point, dir);
if(mv.measurements.get(0).surface.doca[1] == -99) {
StateVec sVecPreviousFiltered = sv.filtered(true).get(0);
double daf_weight = 1;
Expand All @@ -995,7 +1013,7 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
double effectiveDoca = daf.get_EffectiveDoca();
double effectiveVar = daf.get_EffectiveVar();

double h = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[0]);
double h = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[0]);
double res = (effectiveDoca - h);
chi2 += res*res / effectiveVar;
ndfDAF += daf_weight;
Expand All @@ -1020,20 +1038,20 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
double effectiveVar = daf.get_EffectiveVar();
int indexReferenceWire = daf.get_IndexReferenceWire();

double h = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[indexReferenceWire]);
double h = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[indexReferenceWire]);
double res = (effectiveDoca - h);
chi2 += res*res / effectiveVar;
ndfDAF += (daf_weights[0] + daf_weights[1]);

h = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[0]);
h = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[0]);
svc.setProjectorDoca(h);
svc.setProjector(mv.measurements.get(0).surface.wireLine[0].origin().x());
svc.setFinalDAFWeight(daf_weights[0]);
svc.setIsDoubleHit(true);
kfStateVecsAlongTrajectory.add(svc);

StateVec svc2 = sv.new StateVec(svc);
h = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[1]);
h = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[1]);
svc2.setProjectorDoca(h);
svc2.setProjector(mv.measurements.get(0).surface.wireLine[1].origin().x());
svc2.setFinalDAFWeight(daf_weights[1]);
Expand All @@ -1054,6 +1072,8 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
svc.setPathLength(path);

point = new Point3D(sv.transported(forward).get(k1 + 1).x, sv.transported(forward).get(k1 + 1).y, mv.measurements.get(k1 + 1).surface.measPoint.z());
dir = new Vector3D(sv.transported(forward).get(k1 + 1).tx, sv.transported(forward).get(k1 + 1).ty, 1);
line = new Line3D(point, dir);
if(mv.measurements.get(k1 + 1).surface.doca[1] == -99) {
StateVec sVecPreviousFiltered = sv.filtered(true).get(k1 + 1);
double daf_weight = 1;
Expand All @@ -1067,7 +1087,7 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
double effectiveDoca = daf.get_EffectiveDoca();
double effectiveVar = daf.get_EffectiveVar();

double h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[0]);
double h = mv.hDoca(line, mv.measurements.get(k1 + 1).surface.wireLine[0]);
double res = (effectiveDoca - h);
chi2 += res*res / effectiveVar;
ndfDAF += daf_weight;
Expand All @@ -1092,20 +1112,20 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
double effectiveVar = daf.get_EffectiveVar();
int indexReferenceWire = daf.get_IndexReferenceWire();

double h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[indexReferenceWire]);
double h = mv.hDoca(line, mv.measurements.get(k1 + 1).surface.wireLine[indexReferenceWire]);
double res = (effectiveDoca - h);
chi2 += res*res / effectiveVar;
ndfDAF += (daf_weights[0] + daf_weights[1]);

h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[0]);
h = mv.hDoca(line, mv.measurements.get(k1 + 1).surface.wireLine[0]);
svc.setProjectorDoca(h);
svc.setProjector(mv.measurements.get(k1 + 1).surface.wireLine[0].origin().x());
svc.setFinalDAFWeight(daf_weights[0]);
svc.setIsDoubleHit(true);
kfStateVecsAlongTrajectory.add(svc);

StateVec svc2 = sv.new StateVec(svc);
h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[1]);
h = mv.hDoca(line, mv.measurements.get(k1 + 1).surface.wireLine[1]);
svc2.setProjectorDoca(h);
svc2.setProjector(mv.measurements.get(k1 + 1).surface.wireLine[1].origin().x());
svc2.setFinalDAFWeight(daf_weights[1]);
Expand Down
Loading