Skip to content

Commit

Permalink
fix issue for path length in FD tracking
Browse files Browse the repository at this point in the history
  • Loading branch information
@tongtongcao
tongtongcao committed Aug 20, 2024
1 parent 704a2fb commit 25557f6
Show file tree
Hide file tree
Showing 5 changed files with 146 additions and 29 deletions.
14 changes: 9 additions & 5 deletions ...s/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/KFitter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -880,9 +880,9 @@ private void calcFinalChisq(int sector, boolean nofilter) {
sv.transport(sector, k, 0, sVec, mv, this.getSwimmer(), forward);

StateVec svc = sv.transported(forward).get(0);
path += svc.deltaPath;
path += (forward ? 1 : -1) * svc.deltaPath;
svc.setPathLength(path);

double V0 = mv.measurements.get(0).surface.unc[0];

Point3D point = new Point3D(svc.x, svc.y, mv.measurements.get(0).surface.measPoint.z());
Expand Down Expand Up @@ -922,7 +922,7 @@ private void calcFinalChisq(int sector, boolean nofilter) {

double h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[0]);
svc = sv.transported(forward).get(k1 + 1);
path += svc.deltaPath;
path += (forward ? 1 : -1) * svc.deltaPath;
svc.setPathLength(path);
svc.setProjector(mv.measurements.get(k1 + 1).surface.wireLine[0].origin().x());
svc.setProjectorDoca(h);
Expand Down Expand Up @@ -975,7 +975,7 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
sv.transport(sector, k, 0, sVec, mv, this.getSwimmer(), forward);

StateVec svc = sv.transported(forward).get(0);
path += svc.deltaPath;
path += (forward ? 1 : -1) * svc.deltaPath;
svc.setPathLength(path);

Point3D point = new Point3D(svc.x, svc.y, mv.measurements.get(0).surface.measPoint.z());
Expand Down Expand Up @@ -1047,7 +1047,7 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
}

svc = sv.transported(forward).get(k1 + 1);
path += svc.deltaPath;
path += (forward ? 1 : -1) * svc.deltaPath;
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());
Expand Down Expand Up @@ -1116,6 +1116,10 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
public Matrix propagateToVtx(int sector, double Zf) {
return sv.transport(sector, finalStateVec.k, Zf, finalStateVec, mv, this.getSwimmer());
}

public double getDeltaPathToVtx(int sector, double Zf) {
return sv.getDeltaPath(sector, finalStateVec.k, Zf, finalStateVec, mv, this.getSwimmer());
}

//Todo: apply the common funciton to replace current function above
@Override
Expand Down
10 changes: 7 additions & 3 deletions ...racking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/KFitterStraight.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -426,11 +426,11 @@ private void calcFinalChisq(int sector, boolean nofilter) {
kfStateVecsAlongTrajectory = new ArrayList<>();
if (sVec != null && sVec.CM != null) {

boolean forward = false;
boolean forward = false;
sv.transport(sector, k, 0, sVec, mv, this.getSwimmer(), forward);

StateVec svc = sv.transported(forward).get(0);
path += svc.deltaPath;
path += (forward ? 1 : -1) * svc.deltaPath;
svc.setPathLength(path);

double V0 = mv.measurements.get(0).surface.unc[0];
Expand Down Expand Up @@ -473,7 +473,7 @@ private void calcFinalChisq(int sector, boolean nofilter) {

double h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[0]);
svc = sv.transported(forward).get(k1+1);
path += svc.deltaPath;
path += (forward ? 1 : -1) * svc.deltaPath;
svc.setPathLength(path);
svc.setProjector(mv.measurements.get(k1 + 1).surface.wireLine[0].origin().x());
svc.setProjectorDoca(h);
Expand Down Expand Up @@ -504,6 +504,10 @@ public Matrix propagateToVtx(int sector, double Zf) {
return sv.transport(sector, finalStateVec.k, Zf, finalStateVec, mv, this.getSwimmer());
}

public double getDeltaPathToVtx(int sector, double Zf) {
return sv.getDeltaPath(sector, finalStateVec.k, Zf, finalStateVec, mv, this.getSwimmer());
}

@Override
public void runFitter(AStateVecs sv, AMeasVecs mv) {
throw new UnsupportedOperationException("Not supported yet.");
Expand Down
100 changes: 99 additions & 1 deletion ...clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/StateVecs.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ public void initFromHB(StateVec initSV, double beta) {
this.trackTrajS.clear();
this.trackTrajT.put(0, new StateVec(initSV));
}

/**
*
* @param sector
Expand Down Expand Up @@ -150,6 +150,104 @@ public Matrix transport(int sector, int i, double Zf, StateVec iVec, AMeasVecs m

return fVec.CM;

}

/**
*
* @param sector
* @param i initial state vector index
* @param Zf
* @param iVec state vector at the initial index
* @param mv measurements
*/
public double getDeltaPath(int sector, int i, double Zf, StateVec iVec, AMeasVecs mv, Swim swimmer) { // s = signed step-size

double stepSize = 1.0;
StateVec fVec = new StateVec(0);
fVec.x = iVec.x;
fVec.y = iVec.y;
fVec.z = iVec.z;
fVec.tx = iVec.tx;
fVec.ty = iVec.ty;
fVec.Q = iVec.Q;
fVec.B = iVec.B;
Matrix5x5.copy(iVec.CM, fVec.CM);

double s = 0;
double zInit = mv.measurements.get(i).surface.measPoint.z();
double BatMeas = iVec.B;

double z = zInit;

while (Math.signum(Zf - zInit) * z < Math.signum(Zf - zInit) * Zf) {
z = fVec.z;
if (z == Zf) {
break;
}

double x = fVec.x;
double y = fVec.y;
double tx = fVec.tx;
double ty = fVec.ty;
double Q = fVec.Q;
double dPath = fVec.deltaPath;
Matrix cMat = new Matrix();
Matrix5x5.copy(fVec.CM, cMat);
s = Math.signum(Zf - zInit) * stepSize;

// LOGGER.log(Level.FINE, " from "+(float)Z[i]+" to "+(float)Z[f]+" at "+(float)z+" By is "+bf[1]+" B is "+Math.sqrt(bf[0]*bf[0]+bf[1]*bf[1]+bf[2]*bf[2])/Bmax+" stepSize is "+s);
if (Math.signum(Zf - zInit) * (z + s) > Math.signum(Zf - zInit) * Zf) {
s = Math.signum(Zf - zInit) * Math.abs(Zf - z);
}

//rk.RK4transport(sector, Q, x, y, z, tx, ty, s, swimmer, cMat, fVec, dPath);
rk.RK4transport(sector, s, swimmer, cMat, fVec);

// Q process noise matrix estimate
double p = Math.abs(1. / iVec.Q);

double X0 = this.getX0(mv.measurements.get(i).surface, z, Z);
double t_ov_X0 = Math.abs(s) / X0;//path length in radiation length units = t/X0 [true path length/ X0] ; Ar radiation length = 14 cm

double beta = this.beta;
if (beta > 1.0 || beta <= 0) {
beta = 1.0;
}

double sctRMS = 0;

////// Todo: Modify multi-scattering or remove it; After update, some parameters, like iteration termintion chonditions, may need to be updated.
// Speed of light should be 1
// From one measurement site to another, F and Q should be calculated separaetely with multiple steps; and then C' = FTCF + Q
if (Math.abs(s) > 0) {
sctRMS = ((0.0136) / (beta * PhysicsConstants.speedOfLight() * p)) * Math.sqrt(t_ov_X0)
* (1 + 0.038 * Math.log(t_ov_X0));
}

double cov_txtx = (1 + tx * tx) * (1 + tx * tx + ty * ty) * sctRMS * sctRMS;
double cov_tyty = (1 + ty * ty) * (1 + tx * tx + ty * ty) * sctRMS * sctRMS;
double cov_txty = tx * ty * (1 + tx * tx + ty * ty) * sctRMS * sctRMS;

fMS.set(
0, 0, 0, 0, 0,
0, 0, 0, 0, 0,
0, 0, cov_txtx, cov_txty, 0,
0, 0, cov_txty, cov_tyty, 0,
0, 0, 0, 0, 0
);

Matrix5x5.copy(fVec.CM, copyMatrix);
Matrix5x5.add(copyMatrix, fMS, fVec.CM);

if (Math.abs(fVec.B - BatMeas) < 0.0001) {
stepSize *= 2;
}

BatMeas = fVec.B;
}

return fVec.deltaPath;

}

/**
Expand Down
28 changes: 18 additions & 10 deletions reconstruction/dc/src/main/java/org/jlab/rec/dc/track/TrackCandListFinder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -907,8 +907,6 @@ private List<Track> findStraightTracks(CrossList crossList, DCGeant4Factory DcDe
continue;
}

List<org.jlab.rec.dc.trajectory.StateVec> kfStateVecsAlongTrajectory = setKFStateVecsAlongTrajectory(kFZRef);

StateVec fn = new StateVec();
if (!kFZRef.setFitFailed && kFZRef.finalStateVec != null) {
fn.set(kFZRef.finalStateVec.x, kFZRef.finalStateVec.y, kFZRef.finalStateVec.tx, kFZRef.finalStateVec.ty);
Expand All @@ -927,6 +925,11 @@ private List<Track> findStraightTracks(CrossList crossList, DCGeant4Factory DcDe
cand.set_FitConvergenceStatus(kFZRef.ConvStatus);
cand.set_Id(cands.size() + 1);
cand.set_CovMat(kFZRef.finalStateVec.CM);

Point3D VTCS = cand.get(cand.size()-1).getCoordsInTiltedSector(cand.get_Vtx0().x(), cand.get_Vtx0().y(), cand.get_Vtx0().z());
double deltaPathToVtx = kFZRef.getDeltaPathToVtx(cand.get(cand.size()-1).get_Sector(), VTCS.z());

List<org.jlab.rec.dc.trajectory.StateVec> kfStateVecsAlongTrajectory = setKFStateVecsAlongTrajectory(kFZRef, deltaPathToVtx);
cand.setStateVecs(kfStateVecsAlongTrajectory);

// add candidate to list of tracks
Expand Down Expand Up @@ -1071,7 +1074,6 @@ private List<Track> findCurvedTracks(CrossList crossList, DCGeant4Factory DcDete
kFZRef.init(measSurfaces, initSV);

kFZRef.runFitter();
List<org.jlab.rec.dc.trajectory.StateVec> kfStateVecsAlongTrajectory = setKFStateVecsAlongTrajectory(kFZRef);

if (kFZRef.finalStateVec == null) {
continue;
Expand All @@ -1093,15 +1095,21 @@ private List<Track> findCurvedTracks(CrossList crossList, DCGeant4Factory DcDete
cand.set_FitNDF(kFZRef.NDF);
cand.set_FitConvergenceStatus(kFZRef.ConvStatus);

cand.set_CovMat(kFZRef.finalStateVec.CM);
cand.setStateVecs(kfStateVecsAlongTrajectory);

cand.set_CovMat(kFZRef.finalStateVec.CM);

cand.setFinalStateVec(fitStateVec);
cand.set_Id(cands.size() + 1);
this.setTrackPars(cand, traj,
trjFind, fitStateVec,
fitStateVec.getZ(),
DcDetector, dcSwim);

Point3D VTCS = cand.get(cand.size()-1).getCoordsInTiltedSector(cand.get_Vtx0().x(), cand.get_Vtx0().y(), cand.get_Vtx0().z());
double deltaPathToVtx = kFZRef.getDeltaPathToVtx(cand.get(cand.size()-1).get_Sector(), VTCS.z());

List<org.jlab.rec.dc.trajectory.StateVec> kfStateVecsAlongTrajectory = setKFStateVecsAlongTrajectory(kFZRef, deltaPathToVtx);
cand.setStateVecs(kfStateVecsAlongTrajectory);

// add candidate to list of tracks
if (cand.fit_Successful = true) {
cands.add(cand);
Expand All @@ -1117,15 +1125,15 @@ private List<Track> findCurvedTracks(CrossList crossList, DCGeant4Factory DcDete
return cands;
}

public List<org.jlab.rec.dc.trajectory.StateVec> setKFStateVecsAlongTrajectory(KFitter kFZRef) {
public List<org.jlab.rec.dc.trajectory.StateVec> setKFStateVecsAlongTrajectory(KFitter kFZRef, double deltaPathToVtx) {
List<org.jlab.rec.dc.trajectory.StateVec> kfStateVecsAlongTrajectory = new ArrayList<>();

for(int i = 0; i < kFZRef.kfStateVecsAlongTrajectory.size(); i++) {
org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec svc = kFZRef.kfStateVecsAlongTrajectory.get(i);
org.jlab.rec.dc.trajectory.StateVec sv = new org.jlab.rec.dc.trajectory.StateVec(svc.x, svc.y, svc.tx, svc.ty);
sv.setZ(svc.z);
sv.setB(svc.B);
sv.setPathLength(svc.getPathLength());
sv.setPathLength(svc.getPathLength() + deltaPathToVtx);
sv.setProjector(svc.getProjector());
sv.setProjectorDoca(svc.getProjectorDoca());
kfStateVecsAlongTrajectory.add(sv);
Expand All @@ -1134,15 +1142,15 @@ public List<org.jlab.rec.dc.trajectory.StateVec> setKFStateVecsAlongTrajectory(K
return kfStateVecsAlongTrajectory;
}

public List<org.jlab.rec.dc.trajectory.StateVec> setKFStateVecsAlongTrajectory(KFitterStraight kFZRef) {
public List<org.jlab.rec.dc.trajectory.StateVec> setKFStateVecsAlongTrajectory(KFitterStraight kFZRef, double deltaPathToVtx) {
List<org.jlab.rec.dc.trajectory.StateVec> kfStateVecsAlongTrajectory = new ArrayList<>();

for(int i = 0; i < kFZRef.kfStateVecsAlongTrajectory.size(); i++) {
org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec svc = kFZRef.kfStateVecsAlongTrajectory.get(i);
org.jlab.rec.dc.trajectory.StateVec sv = new org.jlab.rec.dc.trajectory.StateVec(svc.x, svc.y, svc.tx, svc.ty);
sv.setZ(svc.z);
sv.setB(svc.B);
sv.setPathLength(svc.getPathLength());
sv.setPathLength(svc.getPathLength() + deltaPathToVtx);
sv.setProjector(svc.getProjector());
sv.setProjectorDoca(svc.getProjectorDoca());
kfStateVecsAlongTrajectory.add(sv);
Expand Down
Loading

0 comments on commit 25557f6

Please sign in to comment.