diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AMeasVecs.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AMeasVecs.java index c20bcdfd80..50c25ae412 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AMeasVecs.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AMeasVecs.java @@ -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; /** * @@ -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; @@ -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); @@ -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) { diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/KFitter.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/KFitter.java index 8b2f038c10..2ecff1db9e 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/KFitter.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/KFitter.java @@ -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.*; /** @@ -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); @@ -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; @@ -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); @@ -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); @@ -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; @@ -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); } @@ -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) { @@ -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; @@ -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++) { @@ -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); @@ -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); @@ -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; @@ -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); @@ -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; @@ -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; @@ -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; @@ -1020,12 +1038,12 @@ 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]); @@ -1033,7 +1051,7 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) { 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]); @@ -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; @@ -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; @@ -1092,12 +1112,12 @@ 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]); @@ -1105,7 +1125,7 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) { 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]); diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/KFitterStraight.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/KFitterStraight.java index 8218df35d9..60943ac4c8 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/KFitterStraight.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/KFitterStraight.java @@ -13,7 +13,9 @@ import org.jlab.clas.tracking.kalmanfilter.zReference.StateVecs; import org.jlab.clas.tracking.utilities.RungeKuttaDoca; import org.jlab.clas.tracking.utilities.MatrixOps.Libr; +import org.jlab.geom.prim.Line3D; import org.jlab.geom.prim.Point3D; +import org.jlab.geom.prim.Vector3D; import org.jlab.jnp.matrix.*; /** @@ -272,7 +274,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) { @@ -288,7 +290,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; @@ -321,7 +325,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++) { @@ -337,8 +341,10 @@ private boolean filter(int k, boolean forward) { } Point3D point2 = new Point3D(x_filt, y_filt, mVec.surface.measPoint.z()); + Vector3D dir2 = new Vector3D(tx_filt, ty_filt, 1); + Line3D line2 = new Line3D(point2, dir2); - h = mv.hDoca(point2, mVec.surface.wireLine[1]); + h = mv.hDoca(line2, mVec.surface.wireLine[1]); signMeas = Math.signum(mVec.surface.doca[1]); sign = Math.signum(h); @@ -436,8 +442,10 @@ private 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]); + Point3D point = new Point3D(svc.x, svc.y, mv.measurements.get(0).surface.measPoint.z()); + Vector3D dir = new Vector3D(sVec.tx, sVec.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); @@ -448,7 +456,7 @@ private 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; @@ -470,9 +478,11 @@ private 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()); + 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); @@ -485,7 +495,7 @@ private 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; diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/MeasVecs.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/MeasVecs.java index 8814ca17ea..60ce46189e 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/MeasVecs.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/MeasVecs.java @@ -10,6 +10,7 @@ import org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec; import org.jlab.geom.prim.Line3D; import org.jlab.geom.prim.Point3D; +import org.jlab.geom.prim.Vector3D; /** * @@ -17,23 +18,40 @@ */ public class MeasVecs extends AMeasVecs { - public double[] H(double x, double y, double z, Line3D wireLine) { + public double[] H(double x, double y, double z, double tx, double ty, Line3D wireLine) { double[] hMatrix = new double[5]; double Err = 0.025; - double[][] Result = new double[2][2]; + double Err_dir = 0.00025; + double[][] Result = new double[2][4]; for (int i = 0; i < 2; i++) { Point3D point = new Point3D(x + (double) Math.pow(-1, i) * Err, y, z); - Result[i][0] = hDoca(point, wireLine); + Vector3D dir = new Vector3D(tx, ty, 1); + Line3D line = new Line3D(point, dir); + Result[i][0] = hDoca(line, wireLine); } for (int i = 0; i < 2; i++) { Point3D point = new Point3D(x, y + (double) Math.pow(-1, i) * Err, z); - Result[i][1] = hDoca(point, wireLine); + Vector3D dir = new Vector3D(tx, ty, 1); + Line3D line = new Line3D(point, dir); + Result[i][1] = hDoca(line, wireLine); } - + for (int i = 0; i < 2; i++) { + Point3D point = new Point3D(x, y, z); + Vector3D dir = new Vector3D(tx + (double) Math.pow(-1, i) * Err_dir, ty, 1); + Line3D line = new Line3D(point, dir); + Result[i][2] = hDoca(line, wireLine); + } + for (int i = 0; i < 2; i++) { + Point3D point = new Point3D(x, y, z); + Vector3D dir = new Vector3D(tx, ty + (double) Math.pow(-1, i) * Err_dir, 1); + Line3D line = new Line3D(point, dir); + Result[i][3] = hDoca(line, wireLine); + } + hMatrix[0] = (Result[0][0] - Result[1][0]) / (2. * Err); hMatrix[1] = (Result[0][1] - Result[1][1]) / (2. * Err); - hMatrix[2] = 0; - hMatrix[3] = 0; + hMatrix[2] = (Result[0][2] - Result[1][2]) / (2. * Err_dir); + hMatrix[3] = (Result[0][3] - Result[1][3]) / (2. * Err_dir); hMatrix[4] = 0; return hMatrix;