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 903fb2415d..c20bcdfd80 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 @@ -14,6 +14,7 @@ /** * * @author ziegler + * @author Tongtong Cao */ public abstract class AMeasVecs { @@ -22,7 +23,8 @@ public abstract class AMeasVecs { public void setMeasVecs(List measSurfaces) { measurements = new ArrayList<>(); - Collections.sort(measSurfaces); + if(measSurfaces.get(0).type != Type.LINEDOCA) // Measurements from DC has been sorted + Collections.sort(measSurfaces); for(int i = 0; i < measSurfaces.size(); i++) { MeasVec mvec = new MeasVec(); mvec.k = i ; @@ -33,9 +35,57 @@ public void setMeasVecs(List measSurfaces) { mvec.skip = mvec.surface.passive; mvec.hemisphere = measSurfaces.get(i).hemisphere; measurements.add(mvec); + if(measSurfaces.get(i).type == Type.LINEDOCA) { //Todo: assign a value to the Surface index and use that as the identifier + mvec.region = measSurfaces.get(i).region; + mvec.sector = measSurfaces.get(i).getSector(); + mvec.superlayer = measSurfaces.get(i).getSuperLayer(); + mvec.layer = measSurfaces.get(i).getLayer(); + } } } + // For DC, there could be two hits at a measurement + public double[] dhDoca(int k, StateVec stateVec) { + double value[] = {Double.NaN, Double.NaN}; + + 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]); + + double signMeas = 1; + double sign = 1; + if(surf.doca[1]!=-99 || !(Math.abs(surf.doca[0])<0.5 && surf.doca[1]==-99 ) ) { //use LR only for double hits or large enough doca for one hit + signMeas = Math.signum(surf.doca[0]); + sign = Math.signum(h); + } else { + signMeas = Math.signum(h); + sign = Math.signum(h); + } + + value[0] = signMeas*Math.abs(surf.doca[0]) - sign*Math.abs(h); + + //USE THE DOUBLE HIT + if(surf.doca[1]!=-99) { + h = hDoca(point, surf.wireLine[1]); + + signMeas = Math.signum(surf.doca[1]); + sign = Math.signum(h); + + value[1]= signMeas*Math.abs(surf.doca[1]) - sign*Math.abs(h); + } + + return value; + } + + // Return a signed doca for DC + public double hDoca(Point3D point, Line3D wireLine) { + + Line3D WL = new Line3D(); + WL.copy(wireLine); + WL.copy(WL.distance(point)); + + return WL.length()*Math.signum(WL.direction().x()); + } public double dh(int k, StateVec stateVec) { @@ -53,7 +103,7 @@ public double dh(int k, StateVec stateVec) { } if( this.measurements.get(stateVec.k).surface.type == Type.PLANEWITHPOINT || this.measurements.get(stateVec.k).surface.type == Type.CYLINDERWITHPOINT) { - Point3D p = new Point3D(this.measurements.get(stateVec.k).surface.refPoint); + Point3D p = new Point3D(this.measurements.get(stateVec.k).surface.measPoint); value = p.distance(stateVec.x, stateVec.y, stateVec.z); } if( this.measurements.get(stateVec.k).surface.type == Type.CYLINDERWITHARC) { @@ -133,7 +183,7 @@ public double h(int k, StateVec stateVec) { } if( this.measurements.get(stateVec.k).surface.type == Type.PLANEWITHPOINT || this.measurements.get(stateVec.k).surface.type == Type.CYLINDERWITHPOINT) { - Point3D p = new Point3D(this.measurements.get(stateVec.k).surface.refPoint); + Point3D p = new Point3D(this.measurements.get(stateVec.k).surface.measPoint); p.setZ(0); value = p.distance(stateVec.x, stateVec.y, 0); } @@ -209,6 +259,10 @@ public class MeasVec implements Comparable { public boolean skip = false; public double hemisphere = 1; + //For DC + public int region = -1; + public int sector = -1; + public int superlayer = -1; @Override public int compareTo(MeasVec arg) { diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AStateVecs.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AStateVecs.java index 5e61bfab0f..cb6d038400 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AStateVecs.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AStateVecs.java @@ -10,6 +10,14 @@ import org.jlab.clas.tracking.kalmanfilter.helical.KFitter; import org.jlab.clas.tracking.trackrep.Helix; import org.jlab.geom.prim.Point3D; +import org.jlab.jnp.matrix.Matrix; +import org.jlab.jnp.matrix.Matrix5x5; + +/** + * + * @author ziegler + * @author Tongtong Cao + */ public abstract class AStateVecs { @@ -245,6 +253,81 @@ public class StateVec { public double energyLoss; public double dx; public double path; + + /////////////////////// extra variables for forward tracking /////////////////////// + // tx = px/pz + public double ty; //=py/pz + public double Q; //q/p + public double B; + public double deltaPath; + public Matrix CM = new Matrix(); // Todo: unify covMat and CM + private double _PathLength; + + /////////////////////// For DAF /////////////////////// + private double weightDAF_single = 1; + private double[] weightDAF_double = {0.5, 0.5}; + + public double getWeightDAF_singleHit(){ + return weightDAF_single; + } + public void setWeightDAF_singleHit(double weight){ + this.weightDAF_single = weight; + } + + public double[] getWeightDAF_doubleHits(){ + return weightDAF_double; + } + public void setWeightDAF_doubleHits(double[] weight){ + this.weightDAF_double = weight; + } + + // Todo: move variables finalDAFWeight, _PathLength, hw, and h from AStateVecs to AMeasVecs + private double finalDAFWeight = -999; + public double getFinalDAFWeight(){ + return finalDAFWeight; + } + public void setFinalDAFWeight(double weight){ + this.finalDAFWeight = weight; + } + + boolean isDoubleHit = false; + + public boolean getIsDoubleHit(){ + return isDoubleHit; + } + public void setIsDoubleHit(boolean isDoubleHit){ + this.isDoubleHit = isDoubleHit; + } + + public double getPathLength() { + return _PathLength; + } + + public void setPathLength(double _PathLength) { + this._PathLength = _PathLength; + } + + // KF projector --> get Wire midPoint match + private double hw; + + public double getProjector() { + return hw; + } + + public void setProjector(double h) { + this.hw = h; + } + // KF projector --> get fit doca + private double h; + + public double getProjectorDoca() { + return h; + } + + public void setProjectorDoca(double h) { + this.h = h; + } + /////////////////////// extra variables for forward tracking /////////////////////// public StateVec(int k) { this.k = k; @@ -294,8 +377,18 @@ public final void copy(StateVec s) { this.energyLoss = s.energyLoss; this.dx = s.dx; this.path = s.path; - this.covMat = this.copyMatrix(s.covMat); - this.F = this.copyMatrix(s.F); + if(s.covMat != null) + this.covMat = this.copyMatrix(s.covMat); + if(s.F != null) + this.F = this.copyMatrix(s.F); + this.ty = s.ty; + this.Q = s.Q; + this.B = s.B; + this.deltaPath = s.deltaPath; + if(s.CM != null) + Matrix5x5.copy(s.CM, this.CM); + this.weightDAF_single = s.weightDAF_single; + this.weightDAF_double = s.weightDAF_double; } public void copyCovMat(double[][] c) { diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/Surface.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/Surface.java index 7b5577c81c..59348bd03d 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/Surface.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/Surface.java @@ -14,12 +14,15 @@ /** * * @author ziegler + * @author Tongtong Cao */ public class Surface implements Comparable { public Type type; public Plane3D plane; - public Point3D refPoint; + public Point3D measPoint; + public Point3D measPoint_err; + // Todo: make a line to replace two end points public Point3D lineEndPoint1; public Point3D lineEndPoint2; public Point3D finitePlaneCorner1; @@ -40,10 +43,56 @@ public class Surface implements Comparable { public boolean passive = false; public double hemisphere = 1; + public double[] unc = new double[2]; + public double[] doca = new double[2]; + public Line3D[] wireLine = new Line3D[2]; + public int region; + public int superlayer; + public int nMeas = 1; + + + public void setNMeas(int n) { + nMeas = n; + } + + public int getNMeas() { + return nMeas; + } + + // For URWell + public Surface(int sector, double x, double y, double z, double x_err, double y_err) { + type = Type.PLANEWITHPOINT; + this.sector = sector; + Point3D point = new Point3D(x, y, z); + measPoint = point; + Point3D point_err = new Point3D(x_err, y_err, 0); + measPoint_err = point_err; + Material material_air = new Material("air", 0, 0, 0, 30400, 0, Units.CM); + Material material_argon = new Material("argon", 0, 0, 0, 14, 0, Units.CM); + materials.add(material_air); + materials.add(material_argon); + } + + // For DC + public Surface(int region, double[] doca, double[] unc, double error, Line3D[] wireLine) { + type = Type.LINEDOCA; + this.region = region; + this.doca = doca; + this.unc = unc; + this.error = error; + this.wireLine = wireLine; + Point3D point = new Point3D(0, 0, wireLine[0].origin().z()); + this.measPoint = point; + Material material_air = new Material("air", 0, 0, 0, 30400, 0, Units.CM); + Material material_argon = new Material("argon", 0, 0, 0, 14, 0, Units.CM); + materials.add(material_air); + materials.add(material_argon); + } + public Surface(Plane3D plane3d, Point3D refrPoint, Point3D c1, Point3D c2, double accuracy) { type = Type.PLANEWITHPOINT; plane = plane3d; - refPoint = refrPoint; + measPoint = refrPoint; finitePlaneCorner1 = c1; finitePlaneCorner2 = c2; swimAccuracy = accuracy; @@ -82,7 +131,7 @@ public Surface(Cylindrical3D cylinder3d, Strip strp, double accuracy) { public Surface(Cylindrical3D cylinder3d, Point3D refrPoint, double accuracy) { type = Type.CYLINDERWITHPOINT; cylinder = cylinder3d; - refPoint = refrPoint; + measPoint = refrPoint; swimAccuracy = accuracy; } public Surface(Cylindrical3D cylinder3d, Point3D endPoint1, Point3D endPoint2, double accuracy) { @@ -184,6 +233,20 @@ public int getLayer() { public void setLayer(int layer) { this.layer = layer; } + + /** + * @return the superlayer + */ + public int getSuperLayer() { + return superlayer; + } + + /** + * @param superlayer the superlayer to set + */ + public void setSuperLayer(int superlayer) { + this.superlayer = superlayer; + } /** * @return the sector diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/Type.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/Type.java index 1814266679..3065ec0924 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/Type.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/Type.java @@ -3,11 +3,12 @@ /** * * @author ziegler + * @author Tongtong Cao */ public enum Type { UDF(-1), PLANEWITHPOINT(0), PLANEWITHLINE(1), PLANEWITHSTRIP(2), CYLINDERWITHPOINT(3), CYLINDERWITHLINE(4), CYLINDERWITHARC(5), - CYLINDERWITHSTRIP(6), LINE(7); + CYLINDERWITHSTRIP(6), LINE(7), LINEDOCA(8); private final int value; Type(int value) { diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/Constants.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/Constants.java new file mode 100644 index 0000000000..7eb9d2cfb7 --- /dev/null +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/Constants.java @@ -0,0 +1,32 @@ +package org.jlab.clas.tracking.kalmanfilter.zReference; + +import java.util.logging.Logger; + +/** + * Constants used in forward tracking + * + * author: Tongtong + */ +public class Constants { + + // private constructor for a singleton + private Constants() { + } + + + public static Logger LOGGER = Logger.getLogger(Constants.class.getName()); + + // CONSTATNS for TRANSFORMATION + public static final double ITERSTOPXHB = 1.2e-2; + public static final double ITERSTOPYHB = 1.4e-1; + public static final double ITERSTOPTXHB = 2.5e-4; + public static final double ITERSTOPTYHB = 1.0e-3; + public static final double ITERSTOPQHB = 1.6e-3; + + public static final double ITERSTOPXTB = 5.5e-5; + public static final double ITERSTOPYTB = 8.0e-4; + public static final double ITERSTOPTXTB = 2.1e-6; + public static final double ITERSTOPTYTB = 3.5e-6; + public static final double ITERSTOPQTB = 1.1e-5; + +} \ No newline at end of file diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/DAFilter.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/DAFilter.java new file mode 100644 index 0000000000..2f5fae412a --- /dev/null +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/DAFilter.java @@ -0,0 +1,170 @@ +package org.jlab.clas.tracking.kalmanfilter.zReference; +import org.jlab.clas.clas.math.FastMath; +import org.jlab.geom.prim.Point3D; +import org.jlab.geom.prim.Line3D; + +/** + * Calculate effective Doca and variance by DAF + * @author Tongtong + */ +public class DAFilter { + + private static double dafChi2Cut = 8; + + // For double hits + private double[] docas_double; + private double[] vars_double; + private double[] weights_double; + private Line3D[] wireLines_double; + private Line3D verticalLine; + private double halfWireDistance; + + // For single hit + private double doca_single; + private double var_single; + private double weight_single; + + // Effective doca + private double effectiveDoca; + private double effectiveVar; + private int indexReferenceWire = 0; + + public DAFilter(double[] docas, double[] vars, double[] weights, Line3D[] wireLines) { + this.docas_double = docas; + this.vars_double = vars; + this.weights_double = weights; + this.wireLines_double = wireLines; + this.verticalLine = wireLines[0].distance(wireLines[1]); + this.halfWireDistance = this.verticalLine.length()/2.; + } + + public DAFilter(double doca, double var, double weight) { + this.doca_single = doca; + this.var_single = var; + this.weight_single = weight; + } + + public static void setDafChi2Cut(double chi2Cut){ + dafChi2Cut = chi2Cut; + } + + public void calc_effectiveDoca_doubleHits(){ + if((wireLines_double[0] == wireLines_double[1]) || ((docas_double[0] == docas_double[1]) && !(docas_double[0] == 0 && docas_double[1] == 0))) { + effectiveVar = vars_double[0]/weights_double[0]; + effectiveDoca = docas_double[0]; + indexReferenceWire = 0; + return; + } + + // Calculate distance between doca point to middle line with sign + double[] toMids = {0, 0}; + + if(docas_double[0] < 0){ + toMids[0] = Math.abs(docas_double[0]) - halfWireDistance; + } + else if(docas_double[0] > 0){ + toMids[0] = halfWireDistance - docas_double[0]; + } + else{ + if(verticalLine.direction().x() > 0) toMids[0] = -halfWireDistance; + else toMids[0] = halfWireDistance; + } + + if(docas_double[1] < 0){ + toMids[1] = Math.abs(docas_double[1]) - halfWireDistance; + } + else if(docas_double[1] > 0){ + toMids[1] = halfWireDistance - docas_double[1]; + } + else{ + if(verticalLine.direction().x() > 0) toMids[1] = halfWireDistance; + else toMids[1] = -halfWireDistance; + } + + // Calculate weighted averge distance to middle line + double sumWeightedVarRec = 0; + double SumWeightedVarRecToMid = 0; + for(int i = 0; i < 2; i++){ + sumWeightedVarRec += weights_double[i] / vars_double[i]; + SumWeightedVarRecToMid += weights_double[i] / vars_double[i] * toMids[i]; + } + effectiveVar = 1/sumWeightedVarRec; + double effectiveToMid = effectiveVar * SumWeightedVarRecToMid; + + // Calculate effective doca with reference line, which correponding doca has higher weight + double docaLargerWeight = 0; + if(weights_double[0] >= weights_double[1]){ + indexReferenceWire = 0; + docaLargerWeight = docas_double[0]; + } + else{ + indexReferenceWire = 1; + docaLargerWeight = docas_double[1]; + } + + if (docaLargerWeight > 0) { + effectiveDoca = halfWireDistance - effectiveToMid; + } else { + if (effectiveToMid < 0) { + effectiveDoca = Math.abs(effectiveToMid) - halfWireDistance; + } else { + effectiveDoca = -effectiveToMid - halfWireDistance; + } + } + } + + public void calc_effectiveDoca_singleHit(){ + effectiveVar = var_single/weight_single; + effectiveDoca = doca_single; + } + + public double get_EffectiveDoca(){ + return effectiveDoca; + } + + public double get_EffectiveVar(){ + return effectiveVar; + } + + public int get_IndexReferenceWire(){ + return indexReferenceWire; + } + + public double calc_updatedWeight_singleHit(double residual, double annealingFactor){ + double factor = 1/Math.sqrt(2 * Math.PI * annealingFactor * var_single); + + double Chi2 = residual * residual/var_single; + double Phi = factor * Math.exp(-0.5 / annealingFactor * Chi2); + double Lambda = factor * Math.exp(-0.5 / annealingFactor * dafChi2Cut); + double sum = Phi + Lambda; + double updatedWeight = Phi/sum; + + if(updatedWeight < 1.e-100) updatedWeight = 1.e-100; + return updatedWeight; + } + + public double[] calc_updatedWeights_doubleHits(double[] residuals, double annealingFactor){ + double factor = 1/Math.sqrt(2 * Math.PI * annealingFactor * vars_double[0] * vars_double[1]); + + double[] Chi2 = new double[2]; + double[] Phi = new double[2]; + double[] Lambda = new double[2]; + double sum = 0; + for(int i = 0; i < 2; i++){ + Chi2[i] = residuals[i] * residuals[i]/vars_double[i]; + Phi[i] = factor * Math.exp(-0.5 / annealingFactor * Chi2[i]); + Lambda[i] = factor * Math.exp(-0.5 / annealingFactor * dafChi2Cut); + sum += (Phi[i] + Lambda[i]); + } + + double[] updatedWeights = {0.5, 0.5}; + for(int i = 0; i < 2; i++){ + updatedWeights[i] = Phi[i]/sum; + if(updatedWeights[i] < 1.e-100) updatedWeights[i] = 1.e-100; + } + + return updatedWeights; + } + +} + 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 new file mode 100644 index 0000000000..c723fb2a3f --- /dev/null +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/KFitter.java @@ -0,0 +1,1179 @@ +package org.jlab.clas.tracking.kalmanfilter.zReference; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +import org.jlab.clas.clas.math.FastMath; +import org.jlab.clas.swimtools.Swim; +import org.jlab.clas.tracking.kalmanfilter.AKFitter; +import org.jlab.clas.tracking.kalmanfilter.AMeasVecs; +import org.jlab.clas.tracking.kalmanfilter.AStateVecs; +import org.jlab.clas.tracking.kalmanfilter.Surface; +import org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec; +import org.jlab.clas.tracking.kalmanfilter.zReference.MeasVecs; +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.Point3D; +import org.jlab.jnp.matrix.*; + +/** + * + * @author Tongtong Cao + */ +public class KFitter extends AKFitter { + + private static List dafAnnealingFactorsTB = new ArrayList<>(Arrays.asList(64., 16., 4., 1.)); + private int dafAnnealingFactorsIndex = 0; + private double dafAnnealingFactor = 1; + + private double ndfDAF = -5; + + private static final double initialCMBlowupFactor = 70; + + private StateVecs sv = new StateVecs(); + private MeasVecs mv = new MeasVecs(); + private StateVec finalSmoothedStateVec = null; + private StateVec finalTransportedStateVec = null; + + public StateVec finalStateVec = null; + public StateVec initialStateVec = null; + public List kfStateVecsAlongTrajectory; + + private int iterNum; + + private double chi2kf = 0; + private double KFScale = 4; + + private int svzLength; + + public int ConvStatus = 1; + + private double Z[]; + + private boolean stopIteration = false; + + private boolean TBT = false; + + Matrix first_inverse = new Matrix(); + Matrix addition = new Matrix(); + Matrix result = new Matrix(); + Matrix result_inv = new Matrix(); + Matrix adj = new Matrix(); + + public KFitter(boolean filter, int iterations, int dir, Swim swim, double Z[], Libr mo) { + super(filter, iterations, dir, swim, mo); + this.Z = Z; + } + + public static void setDafAnnealingFactorsTB(String strDAFAnnealingFactorsTB){ + dafAnnealingFactorsTB.clear(); + String strs[] = strDAFAnnealingFactorsTB.split(","); + for(int i = 0; i < strs.length; i++) + dafAnnealingFactorsTB.add(Double.valueOf(strs[i])); + } + + public final void init(List measSurfaces, StateVec initSV) { + finalSmoothedStateVec = null; + finalTransportedStateVec = null; + this.NDF = -5; + this.chi2 = Double.POSITIVE_INFINITY; + this.numIter = 0; + this.setFitFailed = false; + mv.setMeasVecs(measSurfaces); + for (int i = 0; i < mv.measurements.size(); i++) { + if (mv.measurements.get(i).skip == false) { + this.NDF += mv.measurements.get(i).surface.getNMeas(); + } + } + + sv.init(initSV); + sv.Z = Z; + } + + public final void initFromHB(List measSurfaces, StateVec initSV, double beta, boolean useDAF) { + if(useDAF) initFromHB(measSurfaces, initSV, beta); + else initFromHBNoDAF(measSurfaces, initSV, beta); + } + + public final void initFromHB(List measSurfaces, StateVec initSV, double beta) { + finalSmoothedStateVec = null; + finalTransportedStateVec = null; + this.NDF = -5; + this.chi2 = Double.POSITIVE_INFINITY; + this.numIter = 0; + this.setFitFailed = false; + mv.setMeasVecs(measSurfaces); + for (int i = 0; i < mv.measurements.size(); i++) { + if (mv.measurements.get(i).skip == false) { + this.NDF++; + } + } + sv.initFromHB(initSV, beta); + sv.Z = Z; + TBT = true; + } + + public final void initFromHBNoDAF(List measSurfaces, StateVec initSV, double beta) { + finalSmoothedStateVec = null; + finalTransportedStateVec = null; + this.NDF0 = -5; + this.NDF = -5; + this.chi2 = Double.POSITIVE_INFINITY; + this.numIter = 0; + this.setFitFailed = false; + mv.setMeasVecs(measSurfaces); + for (int i = 0; i < mv.measurements.size(); i++) { + if (mv.measurements.get(i).skip == false) { + this.NDF += mv.measurements.get(i).surface.getNMeas(); + } + } + + sv.initFromHB(initSV, beta); + sv.Z = Z; + TBT = true; + } + + public void runFitter(boolean useDAF) { + if(useDAF) runFitter(); + else runFitterNoDAF(); + } + + public void runFitter() { + this.chi2 = Double.POSITIVE_INFINITY; + double initChi2 = Double.POSITIVE_INFINITY; + this.svzLength = this.mv.measurements.size(); + + int sector = this.mv.measurements.get(0).sector; + + if (TBT == true) { + this.chi2kf = 0; + // Get the input parameters + for (int k = 0; k < svzLength - 1; k++) { + sv.transport(sector, k, k + 1, this.sv.trackTrajT.get(k), mv, this.getSwimmer(), true); + } + this.calcFinalChisqDAF(sector, true); + this.initialStateVec = sv.trackTrajT.get(svzLength - 1); + this.finalStateVec = sv.trackTrajT.get(svzLength - 1); + initChi2 = this.chi2; + if (Double.isNaN(chi2)) { + this.setFitFailed = true; + return; + } + } + + for (int i = 1; i <= totNumIter; i++) { + iterNum = i; + this.chi2kf = 0; + + if (i > 1) { + if (dafAnnealingFactorsIndex < dafAnnealingFactorsTB.size()) { + dafAnnealingFactor = dafAnnealingFactorsTB.get(dafAnnealingFactorsIndex); + dafAnnealingFactorsIndex++; + } else { + dafAnnealingFactor = 1; + dafAnnealingFactorsIndex++; + } + + for (int k = svzLength - 1; k > 0; k--) { + boolean forward = false; + if (k >= 2) { + + // Not backward transport and filter states for the last measurement layer + if (k == svzLength - 1) { + this.sv.transported(forward).put(k, this.sv.trackTrajF.get(k)); + this.sv.filtered(forward).put(k, this.sv.trackTrajF.get(k)); + if (!sv.transport(sector, k, k - 2, this.sv.trackTrajF.get(k), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + } else { + if (!sv.transport(sector, k, k - 2, this.sv.trackTrajB.get(k), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + } + if(TBT){ + if (!this.filter(k - 2, forward, dafAnnealingFactor)) { + this.stopIteration = true; + break; + } + } + else{ + if (!this.filter(k - 2, forward)) { + this.stopIteration = true; + break; + } + } + + if (!sv.transport(sector, k - 2, k - 1, this.sv.trackTrajB.get(k - 2), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + + if(TBT){ + if (!this.filter(k - 1, forward, dafAnnealingFactor)) { + this.stopIteration = true; + break; + } + } + else{ + if (!this.filter(k - 1, forward)) { + this.stopIteration = true; + break; + } + } + } else { + if (!sv.transport(sector, 1, 0, this.sv.trackTrajB.get(1), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + + if(TBT){ + if (!this.filter(0, forward, dafAnnealingFactor)) { + this.stopIteration = true; + break; + } + } + else{ + if (!this.filter(0, forward)) { + this.stopIteration = true; + break; + } + } + } + } + } + + if (this.stopIteration) { + break; + } + + if (dafAnnealingFactorsIndex < dafAnnealingFactorsTB.size()) { + dafAnnealingFactor = dafAnnealingFactorsTB.get(dafAnnealingFactorsIndex); + dafAnnealingFactorsIndex++; + } else { + dafAnnealingFactor = 1; + dafAnnealingFactorsIndex++; + } + + for (int k = 0; k < svzLength - 1; k++) { + boolean forward = true; + + if (iterNum == 1 && (k == 0)) { + if (TBT == true) { + this.sv.transported(true).put(0, this.sv.transported(false).get(0)); // For TBT, calcFinalChisq() is called previously. + } + } + + if (k == 0) { + if (i == 1) { + sv.filtered(forward).put(0, this.sv.trackTrajT.get(0)); + if (!this.sv.transport(sector, 0, 1, this.sv.trackTrajT.get(0), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + } else { + double c00 = this.sv.trackTrajB.get(0).CM.get(0, 0); + double c11 = this.sv.trackTrajB.get(0).CM.get(1, 1); + double c22 = this.sv.trackTrajB.get(0).CM.get(2, 2); + double c33 = this.sv.trackTrajB.get(0).CM.get(3, 3); + double c44 = this.sv.trackTrajB.get(0).CM.get(4, 4); + Matrix newCM = new Matrix(); + newCM.set(c00*initialCMBlowupFactor, 0, 0, 0, 0, + 0, c11*initialCMBlowupFactor, 0, 0, 0, + 0, 0, c22*initialCMBlowupFactor, 0, 0, + 0, 0, 0, c33*initialCMBlowupFactor, 0, + 0, 0, 0, 0, c44*initialCMBlowupFactor); + this.sv.trackTrajB.get(0).CM = newCM; + + this.sv.transported(forward).put(0, this.sv.trackTrajB.get(0)); + this.sv.filtered(forward).put(0, this.sv.trackTrajB.get(0)); + + if (!this.sv.transport(sector, 0, 1, this.sv.trackTrajB.get(0), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + } + } else { + if (!this.sv.transport(sector, k, k + 1, this.sv.trackTrajF.get(k), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + + } + if(TBT){ + if (!this.filter(k + 1, forward, dafAnnealingFactor)) { + this.stopIteration = true; + break; + } + } + else{ + if (!this.filter(k + 1, forward)) { + this.stopIteration = true; + break; + } + } + } + + if (this.stopIteration) { + break; + } + + if (i > 1) { + if (this.setFitFailed == true) { + i = totNumIter; + } + if (this.setFitFailed == false) { + if (this.finalStateVec != null) { + if (!TBT) { + if (Math.abs(sv.trackTrajF.get(svzLength - 1).Q - this.finalStateVec.Q) < Constants.ITERSTOPQHB + && Math.abs(sv.trackTrajF.get(svzLength - 1).x - this.finalStateVec.x) < Constants.ITERSTOPXHB + && Math.abs(sv.trackTrajF.get(svzLength - 1).y - this.finalStateVec.y) < Constants.ITERSTOPYHB + && Math.abs(sv.trackTrajF.get(svzLength - 1).tx - this.finalStateVec.tx) < Constants.ITERSTOPTXHB + && Math.abs(sv.trackTrajF.get(svzLength - 1).ty - this.finalStateVec.ty) < Constants.ITERSTOPYHB) { + i = totNumIter; + } + } else { + if (Math.abs(sv.trackTrajF.get(svzLength - 1).Q - this.finalStateVec.Q) < Constants.ITERSTOPQTB + && Math.abs(sv.trackTrajF.get(svzLength - 1).x - this.finalStateVec.x) < Constants.ITERSTOPXTB + && Math.abs(sv.trackTrajF.get(svzLength - 1).y - this.finalStateVec.y) < Constants.ITERSTOPYTB + && Math.abs(sv.trackTrajF.get(svzLength - 1).tx - this.finalStateVec.tx) < Constants.ITERSTOPTXTB + && Math.abs(sv.trackTrajF.get(svzLength - 1).ty - this.finalStateVec.ty) < Constants.ITERSTOPTYTB) { + i = totNumIter; + } + } + } + this.finalStateVec = sv.trackTrajF.get(svzLength - 1); + + } else { + this.ConvStatus = 1; // Should be 0??? + } + } + + } + + if (totNumIter == 1) { + if (this.setFitFailed == false && this.stopIteration == false) { + this.finalStateVec = sv.trackTrajF.get(svzLength - 1); + } + } + + if(TBT) this.calcFinalChisqDAF(sector); + else this.calcFinalChisq(sector); + + if (Double.isNaN(chi2)) { + this.setFitFailed = true; + } + } + + public void runFitterNoDAF() { + this.chi2 = Double.POSITIVE_INFINITY; + double initChi2 = Double.POSITIVE_INFINITY; + // this.NDF = mv.ndf; + this.svzLength = this.mv.measurements.size(); + + int sector = this.mv.measurements.get(0).sector; + + if (TBT == true) { + this.chi2kf = 0; + // Get the input parameters + for (int k = 0; k < svzLength - 1; k++) { + sv.transport(sector, k, k + 1, this.sv.trackTrajT.get(k), mv, this.getSwimmer(), true); + } + this.calcFinalChisq(sector, true); + this.initialStateVec = sv.trackTrajT.get(svzLength - 1); + this.finalStateVec = sv.trackTrajT.get(svzLength - 1); + initChi2 = this.chi2; + if (Double.isNaN(chi2)) { + this.setFitFailed = true; + return; + } + } + + for (int i = 1; i <= totNumIter; i++) { + iterNum = i; + this.chi2kf = 0; + + if (i > 1) { + + for (int k = svzLength - 1; k > 0; k--) { + boolean forward = false; + if (k >= 2) { + + // Not backward transport and filter states for the last measurement layer + if (k == svzLength - 1) { + if (!sv.transport(sector, k, k - 2, this.sv.trackTrajF.get(k), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + } else { + if (!sv.transport(sector, k, k - 2, this.sv.trackTrajB.get(k), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + } + + if (!this.filter(k - 2, forward)) { + this.stopIteration = true; + break; + } + + if (!sv.transport(sector, k - 2, k - 1, this.sv.trackTrajB.get(k - 2), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + + if (!this.filter(k - 1, forward)) { + this.stopIteration = true; + break; + } + } else { + if (!sv.transport(sector, 1, 0, this.sv.trackTrajB.get(1), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + + if (!this.filter(0, forward)) { + this.stopIteration = true; + break; + } + } + } + } + + if (this.stopIteration) { + break; + } + + for (int k = 0; k < svzLength - 1; k++) { + boolean forward = true; + + if (iterNum == 1 && (k == 0)) { + if (TBT == true) { + this.sv.transported(true).put(0, this.sv.transported(false).get(0)); // For TBT, calcFinalChisq() is called previously. + } + } + + if (k == 0) { + if (i == 1) { + if (!this.sv.transport(sector, 0, 1, this.sv.trackTrajT.get(0), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + } else { + double c00 = this.sv.trackTrajB.get(0).CM.get(0, 0); + double c11 = this.sv.trackTrajB.get(0).CM.get(1, 1); + double c22 = this.sv.trackTrajB.get(0).CM.get(2, 2); + double c33 = this.sv.trackTrajB.get(0).CM.get(3, 3); + double c44 = this.sv.trackTrajB.get(0).CM.get(4, 4); + Matrix newCM = new Matrix(); + newCM.set(c00*initialCMBlowupFactor, 0, 0, 0, 0, + 0, c11*initialCMBlowupFactor, 0, 0, 0, + 0, 0, c22*initialCMBlowupFactor, 0, 0, + 0, 0, 0, c33*initialCMBlowupFactor, 0, + 0, 0, 0, 0, c44*initialCMBlowupFactor); + this.sv.trackTrajB.get(0).CM = newCM; + + if (!this.sv.transport(sector, 0, 1, this.sv.trackTrajB.get(0), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + } + } else { + if (!this.sv.transport(sector, k, k + 1, this.sv.trackTrajF.get(k), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + + } + + if (!this.filter(k + 1, forward)) { + this.stopIteration = true; + break; + } + } + + if (this.stopIteration) { + break; + } + + if (i > 1) { + if (this.setFitFailed == true) { + i = totNumIter; + } + if (this.setFitFailed == false) { + if (this.finalStateVec != null) { + if (!TBT) { + if (Math.abs(sv.trackTrajF.get(svzLength - 1).Q - this.finalStateVec.Q) < 1.6e-3 + && Math.abs(sv.trackTrajF.get(svzLength - 1).x - this.finalStateVec.x) < 1.2e-2 + && Math.abs(sv.trackTrajF.get(svzLength - 1).y - this.finalStateVec.y) < 1.4e-1 + && Math.abs(sv.trackTrajF.get(svzLength - 1).tx - this.finalStateVec.tx) < 2.5e-4 + && Math.abs(sv.trackTrajF.get(svzLength - 1).ty - this.finalStateVec.ty) < 1.0e-3) { + i = totNumIter; + } + } else { + if (Math.abs(sv.trackTrajF.get(svzLength - 1).Q - this.finalStateVec.Q) < 1.1e-5 + && Math.abs(sv.trackTrajF.get(svzLength - 1).x - this.finalStateVec.x) < 5.5e-5 + && Math.abs(sv.trackTrajF.get(svzLength - 1).y - this.finalStateVec.y) < 8.0e-4 + && Math.abs(sv.trackTrajF.get(svzLength - 1).tx - this.finalStateVec.tx) < 2.1e-6 + && Math.abs(sv.trackTrajF.get(svzLength - 1).ty - this.finalStateVec.ty) < 3.5e-6) { + i = totNumIter; + } + } + } + this.finalStateVec = sv.trackTrajF.get(svzLength - 1); + + } else { + this.ConvStatus = 1; // Should be 0??? + } + } + + } + + if (totNumIter == 1) { + if (this.setFitFailed == false && this.stopIteration == false) { + this.finalStateVec = sv.trackTrajF.get(svzLength - 1); + } + } + + this.calcFinalChisq(sector); + + if (Double.isNaN(chi2)) { + this.setFitFailed = true; + } + + if (TBT == true) { + if (chi2 > initChi2) { // fit failed + this.finalStateVec = this.initialStateVec; + sv.trackTrajT.put(svzLength - 1, this.initialStateVec); + this.calcFinalChisq(sector, true); + } + } + + } + + private boolean filter(int k, boolean forward, double annealingFactor) { + StateVec sVec = sv.transported(forward).get(k); + org.jlab.clas.tracking.kalmanfilter.AMeasVecs.MeasVec mVec = mv.measurements.get(k); + + if (Double.isNaN(sVec.x) || Double.isNaN(sVec.y) + || Double.isNaN(sVec.tx) || Double.isNaN(sVec.ty) + || Double.isNaN(sVec.Q)) { + this.setFitFailed = true; + return false; + } + if (sVec != null && sVec.CM != null + && k < mv.measurements.size() && mVec.skip == false) { + double c2 = 0; + double x_filt = 0; + double y_filt = 0; + double tx_filt = 0; + double ty_filt = 0; + double Q_filt = 0; + Matrix cMat = new Matrix(); + + double updatedWeights_singleHit = 1; + double[] updatedWeights_doubleHits = {0.5, 0.5}; + + if (mVec.surface.doca[1] == -99) { + StateVec sVecPreviousFiltered = sv.filtered(!forward).get(k); + double daf_weight = 1; + if (sVecPreviousFiltered != null) { + daf_weight = sVecPreviousFiltered.getWeightDAF_singleHit(); + } + double var = mVec.surface.unc[0] * KFScale; + DAFilter daf = new DAFilter(mVec.surface.doca[0], var, daf_weight); + daf.calc_effectiveDoca_singleHit(); + + double effectiveDoca = daf.get_EffectiveDoca(); + double effectiveVar = daf.get_EffectiveVar(); + + + double[] K = new double[5]; + double V = effectiveVar; + double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), mVec.surface.wireLine[0]); + Matrix CaInv = this.filterCovMat(H, sVec.CM, V); + if (CaInv != null) { + Matrix5x5.copy(CaInv, cMat); + } else { + return false; + } + + for (int j = 0; j < 5; j++) { + // the gain matrix + K[j] = (H[0] * cMat.get(j, 0) + + H[1] * cMat.get(j, 1)) / V; + } + + Point3D point = new Point3D(sVec.x, sVec.y, mVec.surface.measPoint.z()); + double h = mv.hDoca(point, mVec.surface.wireLine[0]); + + c2 = (effectiveDoca - h) * (effectiveDoca - h) / V; + + x_filt = sVec.x + + K[0] * (effectiveDoca - h); + y_filt = sVec.y + + K[1] * (effectiveDoca - h); + tx_filt = sVec.tx + + K[2] * (effectiveDoca - h); + ty_filt = sVec.ty + + K[3] * (effectiveDoca - h); + Q_filt = sVec.Q + + 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 residual = effectiveDoca - h0; + updatedWeights_singleHit = daf.calc_updatedWeight_singleHit(residual, annealingFactor); + } + else{ + StateVec sVecPreviousFiltered = sv.filtered(!forward).get(k); + double[] daf_weights = {0.5, 0.5}; + if (sVecPreviousFiltered != null) { + daf_weights = sVecPreviousFiltered.getWeightDAF_doubleHits(); + } + double[] vars = {mVec.surface.unc[0] * KFScale, mVec.surface.unc[1] * KFScale}; + DAFilter daf = new DAFilter(mVec.surface.doca, vars, daf_weights, mVec.surface.wireLine); + daf.calc_effectiveDoca_doubleHits(); + + double effectiveDoca = daf.get_EffectiveDoca(); + double effectiveVar = daf.get_EffectiveVar(); + int indexReferenceWire = daf.get_IndexReferenceWire(); + + double[] K = new double[5]; + double V = effectiveVar; + double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), mVec.surface.wireLine[indexReferenceWire]); + Matrix CaInv = this.filterCovMat(H, sVec.CM, V); + if (CaInv != null) { + Matrix5x5.copy(CaInv, cMat); + } else { + return false; + } + + for (int j = 0; j < 5; j++) { + // the gain matrix + K[j] = (H[0] * cMat.get(j, 0) + + H[1] * cMat.get(j, 1)) / V; + } + + Point3D point = new Point3D(sVec.x, sVec.y, mVec.surface.measPoint.z()); + double h = mv.hDoca(point, mVec.surface.wireLine[indexReferenceWire]); + + c2 = (effectiveDoca - h) * (effectiveDoca - h) / V; + + x_filt = sVec.x + + K[0] * (effectiveDoca - h); + y_filt = sVec.y + + K[1] * (effectiveDoca - h); + tx_filt = sVec.tx + + K[2] * (effectiveDoca - h); + ty_filt = sVec.ty + + K[3] * (effectiveDoca - h); + Q_filt = sVec.Q + + 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]); + double[] residuals = {mVec.surface.doca[0] - h0, mVec.surface.doca[1] - h1}; + updatedWeights_doubleHits = daf.calc_updatedWeights_doubleHits(residuals, annealingFactor); + } + + chi2kf += c2; + if (filterOn) { + StateVec filteredVec = sv.new StateVec(k); + filteredVec.x = x_filt; + filteredVec.y = y_filt; + filteredVec.tx = tx_filt; + filteredVec.ty = ty_filt; + filteredVec.Q = Q_filt; + filteredVec.z = sVec.z; + filteredVec.B = sVec.B; + filteredVec.deltaPath = sVec.deltaPath; + + filteredVec.CM = cMat; + filteredVec.setWeightDAF_singleHit(updatedWeights_singleHit); + filteredVec.setWeightDAF_doubleHits(updatedWeights_doubleHits); + + sv.filtered(forward).put(k, filteredVec); + } else { + return false; + } + + return true; + } else { + return false; + } + } + + private boolean filter(int k, boolean forward) { + StateVec sVec = sv.transported(forward).get(k); + org.jlab.clas.tracking.kalmanfilter.AMeasVecs.MeasVec mVec = mv.measurements.get(k); + + if (Double.isNaN(sVec.x) || Double.isNaN(sVec.y) + || Double.isNaN(sVec.tx) || Double.isNaN(sVec.ty) + || Double.isNaN(sVec.Q)) { + this.setFitFailed = true; + return false; + } + if (sVec != null && sVec.CM != null + && k < mv.measurements.size() && mVec.skip == false) { + + 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]); + Matrix CaInv = this.filterCovMat(H, sVec.CM, V); + Matrix cMat = new Matrix(); + if (CaInv != null) { + Matrix5x5.copy(CaInv, cMat); + } else { + return false; + } + + for (int j = 0; j < 5; j++) { + // the gain matrix + K[j] = (H[0] * cMat.get(j, 0) + + H[1] * cMat.get(j, 1)) / V; + } + + Point3D point = new Point3D(sVec.x, sVec.y, mVec.surface.measPoint.z()); + double h = mv.hDoca(point, mVec.surface.wireLine[0]); + + double signMeas = 1; + double sign = 1; + if (mVec.surface.doca[1] != -99 + || !(Math.abs(mVec.surface.doca[0]) < 0.5 + && mVec.surface.doca[1] == -99)) { // use LR only for double hits && large + // enough docas + signMeas = Math.signum(mVec.surface.doca[0]); + sign = Math.signum(h); + } else { + signMeas = Math.signum(h); + sign = Math.signum(h); + } + + double c2 = ((signMeas * Math.abs(mVec.surface.doca[0]) - sign * Math.abs(h)) + * (signMeas * Math.abs(mVec.surface.doca[0]) - sign * Math.abs(h)) / V); + + double x_filt = sVec.x + + K[0] * (signMeas * Math.abs(mVec.surface.doca[0]) - sign * Math.abs(h)); + double y_filt = sVec.y + + K[1] * (signMeas * Math.abs(mVec.surface.doca[0]) - sign * Math.abs(h)); + double tx_filt = sVec.tx + + K[2] * (signMeas * Math.abs(mVec.surface.doca[0]) - sign * Math.abs(h)); + double ty_filt = sVec.ty + + K[3] * (signMeas * Math.abs(mVec.surface.doca[0]) - sign * Math.abs(h)); + double Q_filt = sVec.Q + + K[4] * (signMeas * Math.abs(mVec.surface.doca[0]) - sign * Math.abs(h)); + + // USE THE DOUBLE HIT + 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]); + CaInv = this.filterCovMat(H, cMat, V); + if (CaInv != null) { + for (int i = 0; i < 5; i++) { + Matrix5x5.copy(CaInv, cMat); + } + } else { + return false; + } + for (int j = 0; j < 5; j++) { + // the gain matrix + K[j] = (H[0] * cMat.get(j, 0) + + H[1] * cMat.get(j, 1)) / V; + } + + Point3D point2 = new Point3D(x_filt, y_filt, mVec.surface.measPoint.z()); + + h = mv.hDoca(point2, mVec.surface.wireLine[1]); + + signMeas = Math.signum(mVec.surface.doca[1]); + sign = Math.signum(h); + + x_filt += K[0] * (signMeas * Math.abs(mVec.surface.doca[1]) - sign * Math.abs(h)); + y_filt += K[1] * (signMeas * Math.abs(mVec.surface.doca[1]) - sign * Math.abs(h)); + tx_filt += K[2] * (signMeas * Math.abs(mVec.surface.doca[1]) - sign * Math.abs(h)); + ty_filt += K[3] * (signMeas * Math.abs(mVec.surface.doca[1]) - sign * Math.abs(h)); + Q_filt += K[4] * (signMeas * Math.abs(mVec.surface.doca[1]) - sign * Math.abs(h)); + + c2 += ((signMeas * Math.abs(mVec.surface.doca[1]) - sign * Math.abs(h)) + * (signMeas * Math.abs(mVec.surface.doca[1]) - sign * Math.abs(h)) / V); + } + + chi2kf += c2; + if (filterOn) { + StateVec filteredVec = sv.new StateVec(k); + filteredVec.x = x_filt; + filteredVec.y = y_filt; + filteredVec.tx = tx_filt; + filteredVec.ty = ty_filt; + filteredVec.Q = Q_filt; + filteredVec.z = sVec.z; + filteredVec.B = sVec.B; + filteredVec.deltaPath = sVec.deltaPath; + + filteredVec.CM = cMat; + + sv.filtered(forward).put(k, filteredVec); + } else { + return false; + } + + return true; + } else { + return false; + } + } + + public Matrix filterCovMat(double[] H, Matrix Ci, double V) { + + double det = Matrix5x5.inverse(Ci, first_inverse, adj); + if (Math.abs(det) < 1.e-60) { + return null; + } + + addition.set( + H[0] * H[0] / V, H[0] * H[1] / V, 0, 0, 0, + H[0] * H[1] / V, H[1] * H[1] / V, 0, 0, 0, + 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0); + + Matrix5x5.add(first_inverse, addition, result); + double det2 = Matrix5x5.inverse(result, result_inv, adj); + if (Math.abs(det2) < 1.e-60) { + return null; + } + + return result_inv; + } + + private void calcFinalChisq(int sector) { + calcFinalChisq(sector, false); + } + + private void calcFinalChisq(int sector, boolean nofilter) { + int k = svzLength - 1; + this.chi2 = 0; + double path = 0; + double[] nRj = new double[3]; + + StateVec sVec; + + // To be changed: to match wit the old package, we make the following codes. Could be changed when other codes for application of calcFinalChisq are changed. + if (nofilter || (sv.trackTrajF.get(k) == null)) { + sVec = sv.trackTrajT.get(k); + } else { + sVec = sv.trackTrajF.get(k); + } + + kfStateVecsAlongTrajectory = new ArrayList<>(); + if (sVec != null && sVec.CM != null) { + + boolean forward = false; + sv.transport(sector, k, 0, sVec, mv, this.getSwimmer(), forward); + + StateVec svc = sv.transported(forward).get(0); + path += 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()); + double h0 = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[0]); + + svc.setProjector(mv.measurements.get(0).surface.wireLine[0].origin().x()); + svc.setProjectorDoca(h0); + kfStateVecsAlongTrajectory.add(svc); + double res = (mv.measurements.get(0).surface.doca[0] - h0); + chi2 += (mv.measurements.get(0).surface.doca[0] - h0) * (mv.measurements.get(0).surface.doca[0] - h0) / V0; + nRj[mv.measurements.get(0).region - 1] += res * res / mv.measurements.get(0).error; + //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]); + 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; + + StateVec svc2 = sv.new StateVec(svc); + svc2.setProjector(mv.measurements.get(0).surface.wireLine[1].origin().x()); + svc2.setProjectorDoca(h0); + kfStateVecsAlongTrajectory.add(svc2); + } + + forward = true; + for (int k1 = 0; k1 < k; k1++) { + if (k1 == 0) { + sv.transport(sector, k1, k1 + 1, svc, mv, this.getSwimmer(), forward); + } else { + sv.transport(sector, k1, k1 + 1, sv.transported(forward).get(k1), mv, this.getSwimmer(), forward); + } + + 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()); + + double h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[0]); + svc = sv.transported(forward).get(k1 + 1); + path += svc.deltaPath; + svc.setPathLength(path); + svc.setProjector(mv.measurements.get(k1 + 1).surface.wireLine[0].origin().x()); + svc.setProjectorDoca(h); + kfStateVecsAlongTrajectory.add(svc); + res = (mv.measurements.get(k1 + 1).surface.doca[0] - h); + chi2 += (mv.measurements.get(k1 + 1).surface.doca[0] - h) * (mv.measurements.get(k1 + 1).surface.doca[0] - h) / V; + nRj[mv.measurements.get(k1 + 1).region - 1] += res * res / V; + //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]); + 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; + + StateVec svc2 = sv.new StateVec(svc); + svc2.setProjector(mv.measurements.get(k1 + 1).surface.wireLine[1].origin().x()); + svc2.setProjectorDoca(h); + kfStateVecsAlongTrajectory.add(svc2); + } + } + } + + } + + private void calcFinalChisqDAF(int sector) { + calcFinalChisqDAF(sector, false); + } + + private void calcFinalChisqDAF(int sector, boolean nofilter) { + ndfDAF = -5; + + int k = svzLength - 1; + this.chi2 = 0; + double path = 0; + + StateVec sVec; + + // To be changed: to match wit the old package, we make the following codes. Could be changed when other codes for application of calcFinalChisq are changed. + if (nofilter || (sv.trackTrajF.get(k) == null)) { + sVec = sv.trackTrajT.get(k); + } else { + sVec = sv.trackTrajF.get(k); + } + + kfStateVecsAlongTrajectory = new ArrayList<>(); + if (sVec != null && sVec.CM != null) { + + boolean forward = false; + sv.transport(sector, k, 0, sVec, mv, this.getSwimmer(), forward); + + StateVec svc = sv.transported(forward).get(0); + path += svc.deltaPath; + svc.setPathLength(path); + + Point3D point = new Point3D(svc.x, svc.y, mv.measurements.get(0).surface.measPoint.z()); + if(mv.measurements.get(0).surface.doca[1] == -99) { + StateVec sVecPreviousFiltered = sv.filtered(true).get(0); + double daf_weight = 1; + if (sVecPreviousFiltered != null) { + daf_weight = sVecPreviousFiltered.getWeightDAF_singleHit(); + } + double V0 = mv.measurements.get(0).surface.unc[0]; + DAFilter daf = new DAFilter(mv.measurements.get(0).surface.doca[0], V0, daf_weight); + daf.calc_effectiveDoca_singleHit(); + + double effectiveDoca = daf.get_EffectiveDoca(); + double effectiveVar = daf.get_EffectiveVar(); + + double h = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[0]); + double res = (effectiveDoca - h); + chi2 += res*res / effectiveVar; + ndfDAF += daf_weight; + + svc.setProjectorDoca(h); + svc.setProjector(mv.measurements.get(0).surface.wireLine[0].origin().x()); + svc.setFinalDAFWeight(daf_weight); + svc.setIsDoubleHit(false); + kfStateVecsAlongTrajectory.add(svc); + } + else{ + StateVec sVecPreviousFiltered = sv.filtered(true).get(0); + double[] daf_weights = {0.5, 0.5}; + if (sVecPreviousFiltered != null) { + daf_weights = sVecPreviousFiltered.getWeightDAF_doubleHits(); + } + double[] vars = {mv.measurements.get(0).surface.unc[0], mv.measurements.get(0).surface.unc[1]}; + DAFilter daf = new DAFilter(mv.measurements.get(0).surface.doca, vars, daf_weights, mv.measurements.get(0).surface.wireLine); + daf.calc_effectiveDoca_doubleHits(); + + double effectiveDoca = daf.get_EffectiveDoca(); + double effectiveVar = daf.get_EffectiveVar(); + int indexReferenceWire = daf.get_IndexReferenceWire(); + + double h = mv.hDoca(point, 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]); + 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]); + svc2.setProjectorDoca(h); + svc2.setProjector(mv.measurements.get(0).surface.wireLine[1].origin().x()); + svc2.setFinalDAFWeight(daf_weights[1]); + svc2.setIsDoubleHit(true); + kfStateVecsAlongTrajectory.add(svc2); + } + + forward = true; + for (int k1 = 0; k1 < k; k1++) { + if (k1 == 0) { + sv.transport(sector, k1, k1 + 1, svc, mv, this.getSwimmer(), forward); + } else { + sv.transport(sector, k1, k1 + 1, sv.transported(forward).get(k1), mv, this.getSwimmer(), forward); + } + + svc = sv.transported(forward).get(k1 + 1); + path += 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()); + if(mv.measurements.get(k1 + 1).surface.doca[1] == -99) { + StateVec sVecPreviousFiltered = sv.filtered(true).get(k1 + 1); + double daf_weight = 1; + if (sVecPreviousFiltered != null) { + daf_weight = sVecPreviousFiltered.getWeightDAF_singleHit(); + } + double V0 = mv.measurements.get(k1 + 1).surface.unc[0]; + DAFilter daf = new DAFilter(mv.measurements.get(k1 + 1).surface.doca[0], V0, daf_weight); + daf.calc_effectiveDoca_singleHit(); + + double effectiveDoca = daf.get_EffectiveDoca(); + double effectiveVar = daf.get_EffectiveVar(); + + double h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[0]); + double res = (effectiveDoca - h); + chi2 += res*res / effectiveVar; + ndfDAF += daf_weight; + + svc.setProjectorDoca(h); + svc.setProjector(mv.measurements.get(k1 + 1).surface.wireLine[0].origin().x()); + svc.setFinalDAFWeight(daf_weight); + svc.setIsDoubleHit(false); + kfStateVecsAlongTrajectory.add(svc); + } + else{ + StateVec sVecPreviousFiltered = sv.filtered(true).get(k1 + 1); + double[] daf_weights = {0.5, 0.5}; + if (sVecPreviousFiltered != null) { + daf_weights = sVecPreviousFiltered.getWeightDAF_doubleHits(); + } + double[] vars = {mv.measurements.get(k1 + 1).surface.unc[0], mv.measurements.get(k1 + 1).surface.unc[1]}; + DAFilter daf = new DAFilter(mv.measurements.get(k1 + 1).surface.doca, vars, daf_weights, mv.measurements.get(k1 + 1).surface.wireLine); + daf.calc_effectiveDoca_doubleHits(); + + double effectiveDoca = daf.get_EffectiveDoca(); + double effectiveVar = daf.get_EffectiveVar(); + int indexReferenceWire = daf.get_IndexReferenceWire(); + + double h = mv.hDoca(point, 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]); + 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]); + svc2.setProjectorDoca(h); + svc2.setProjector(mv.measurements.get(k1 + 1).surface.wireLine[1].origin().x()); + svc2.setFinalDAFWeight(daf_weights[1]); + svc2.setIsDoubleHit(true); + kfStateVecsAlongTrajectory.add(svc2); + } + } + } + } + + public Matrix propagateToVtx(int sector, double Zf) { + return sv.transport(sector, finalStateVec.k, Zf, finalStateVec, mv, this.getSwimmer()); + } + + //Todo: apply the common funciton to replace current function above + @Override + public void runFitter(AStateVecs sv, AMeasVecs mv) { + throw new UnsupportedOperationException("Not supported yet."); + } + + //Todo: apply the common funciton to replace current function above + @Override + public StateVec filter(int k, StateVec vec, AMeasVecs mv) { + throw new UnsupportedOperationException("Not supported yet."); + } + + @Override + public StateVec smooth(int k, AStateVecs sv, AMeasVecs mv) { + throw new UnsupportedOperationException("Not supported yet."); + } + + @Override + public StateVec smooth(StateVec v1, StateVec v2) { + throw new UnsupportedOperationException("Not supported yet."); + } + + public MeasVecs getMeasVecs() { + return mv; + } + + public StateVecs getStateVecs() { + return sv; + } + + public double getNDFDAF(){ + return ndfDAF; + } + + public void printlnMeasVecs() { + for (int i = 0; i < mv.measurements.size(); i++) { + org.jlab.clas.tracking.kalmanfilter.AMeasVecs.MeasVec measvec = mv.measurements.get(i); + String s = String.format("k=%d region=%d superlayer=%d layer=%d error=%.4f", measvec.k, measvec.region, measvec.superlayer, + measvec.layer, measvec.error); + s += String.format(" Surface: index=%d ", measvec.surface.getIndex()); + s += String.format( + " Surface line 0: doca=%.4f unc=%.4f origin_x =%.4f, origin_y =%.4f, origin_z =%.4f, end_x=%.4f, end_y=%.4f, end_z=%.4f", + measvec.surface.doca[0], measvec.surface.unc[0], measvec.surface.wireLine[0].origin().x(), + measvec.surface.wireLine[0].origin().y(), measvec.surface.measPoint.z(), + measvec.surface.wireLine[0].end().x(), measvec.surface.wireLine[0].end().y(), + measvec.surface.wireLine[0].end().z()); + if (measvec.surface.wireLine[1] != null) { + s += String.format( + " Surface line 1: doca=%.4f unc=%.4f origin_x =%.4f, origin_y =%.4f, origin_z =%.4f, end_x=%.4f, end_y=%.4f, end_z=%.4f", + measvec.surface.doca[1], measvec.surface.unc[1], measvec.surface.wireLine[1].origin().x(), + measvec.surface.wireLine[1].origin().y(), measvec.surface.wireLine[1].origin().z(), + measvec.surface.wireLine[1].end().x(), measvec.surface.wireLine[1].end().y(), + measvec.surface.wireLine[1].end().z()); + } + + System.out.println(s); + } + } + +} 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 new file mode 100644 index 0000000000..232bcaa7d0 --- /dev/null +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/KFitterStraight.java @@ -0,0 +1,559 @@ +package org.jlab.clas.tracking.kalmanfilter.zReference; + +import java.util.ArrayList; +import java.util.List; + +import org.jlab.clas.clas.math.FastMath; +import org.jlab.clas.swimtools.Swim; +import org.jlab.clas.tracking.kalmanfilter.AKFitter; +import org.jlab.clas.tracking.kalmanfilter.AMeasVecs; +import org.jlab.clas.tracking.kalmanfilter.AStateVecs; +import org.jlab.clas.tracking.kalmanfilter.Surface; +import org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec; +import org.jlab.clas.tracking.kalmanfilter.zReference.MeasVecs; +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.Point3D; +import org.jlab.jnp.matrix.*; + +/** + * + * @author Tongtong Cao + */ +public class KFitterStraight extends AKFitter { + + private StateVecs sv = new StateVecs(); + private MeasVecs mv = new MeasVecs(); + private StateVec finalSmoothedStateVec = null; + private StateVec finalTransportedStateVec = null; + + public StateVec finalStateVec = null; + public StateVec initialStateVec = null; + public List kfStateVecsAlongTrajectory; + + private int interNum; + + private double chi2kf = 0; + private double KFScale = 4; + + private int svzLength; + + public int ConvStatus = 1; + + private double Z[]; + + private boolean stopIteration = false; + + private boolean TBT = false; + + Matrix first_inverse = new Matrix(); + Matrix addition = new Matrix(); + Matrix result = new Matrix(); + Matrix result_inv = new Matrix(); + Matrix adj = new Matrix(); + + public KFitterStraight(boolean filter, int iterations, int dir, Swim swim, double Z[], Libr mo) { + super(filter, iterations, dir, swim, mo); + this.Z = Z; + } + + public final void init(List measSurfaces, StateVec initSV) { + finalSmoothedStateVec = null; + finalTransportedStateVec = null; + this.NDF = -5; + this.chi2 = Double.POSITIVE_INFINITY; + this.numIter = 0; + this.setFitFailed = false; + mv.setMeasVecs(measSurfaces); + for (int i = 0; i < mv.measurements.size(); i++) { + if (mv.measurements.get(i).skip == false) { + this.NDF += mv.measurements.get(i).surface.getNMeas(); + } + } + + sv.init(initSV); + sv.Z = Z; + } + + public final void initFromHB(List measSurfaces, StateVec initSV, double beta) { + finalSmoothedStateVec = null; + finalTransportedStateVec = null; + this.NDF = -5; + this.chi2 = Double.POSITIVE_INFINITY; + this.numIter = 0; + this.setFitFailed = false; + mv.setMeasVecs(measSurfaces); + for (int i = 0; i < mv.measurements.size(); i++) { + if (mv.measurements.get(i).skip == false) { + this.NDF += mv.measurements.get(i).surface.getNMeas(); + } + } + + sv.initFromHB(initSV, beta); + sv.Z = Z; + TBT = true; + } + + public void runFitter() { + this.chi2 = Double.POSITIVE_INFINITY; + double initChi2 = Double.POSITIVE_INFINITY; + // this.NDF = mv.ndf; + this.svzLength = this.mv.measurements.size(); + + int sector = this.mv.measurements.get(0).sector; + + if (TBT == true) { + this.chi2kf = 0; + // Get the input parameters + for (int k = 0; k < svzLength - 1; k++) { + sv.transport(sector, k, k + 1, this.sv.trackTrajT.get(k), mv, this.getSwimmer(), true); + } + this.calcFinalChisq(sector, true); + this.initialStateVec = sv.trackTrajT.get(svzLength - 1); + this.finalStateVec = sv.trackTrajT.get(svzLength - 1); + initChi2 = this.chi2; + if (Double.isNaN(chi2)) { + this.setFitFailed = true; + return; + } + } + + for (int i = 1; i <= totNumIter; i++) { + interNum = i; + this.chi2kf = 0; + + if (i > 1) { + + for (int k = svzLength - 1; k > 0; k--) { + boolean forward = false; + if (k >= 2) { + + // Not backward transport and filter states for the last measurement layer + if (k == svzLength - 1) { + if (!sv.transport(sector, k, k - 2, this.sv.trackTrajF.get(k), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + } else { + if (!sv.transport(sector, k, k - 2, this.sv.trackTrajB.get(k), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + } + + if (!this.filter(k - 2, forward)) { + this.stopIteration = true; + break; + } + + if (!sv.transport(sector, k - 2, k - 1, this.sv.trackTrajB.get(k - 2), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + + if (!this.filter(k - 1, forward)) { + this.stopIteration = true; + break; + } + } else { + if (!sv.transport(sector, 1, 0, this.sv.trackTrajB.get(1), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + + if (!this.filter(0, forward)) { + this.stopIteration = true; + break; + } + } + } + } + + if (this.stopIteration) { + break; + } + + for (int k = 0; k < svzLength - 1; k++) { + boolean forward = true; + + if (interNum == 1 && (k == 0)) { + if (TBT == true) { + this.sv.transported(true).put(0, this.sv.transported(false).get(0)); // For TBT, calcFinalChisq() is called previously. + } + } + + if (k == 0) { + if (i == 1) { + if (!this.sv.transport(sector, 0, 1, this.sv.trackTrajT.get(0), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + } else { + if (!this.sv.transport(sector, 0, 1, this.sv.trackTrajB.get(0), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + } + } else { + if (!this.sv.transport(sector, k, k + 1, this.sv.trackTrajF.get(k), mv, this.getSwimmer(), forward)) { + this.stopIteration = true; + break; + } + + } + + if (!this.filter(k + 1, forward)) { + this.stopIteration = true; + break; + } + } + + if (this.stopIteration) { + break; + } + + if (i > 1) { + if (this.setFitFailed == true) { + i = totNumIter; + } + if (this.setFitFailed == false) { + if (this.finalStateVec != null) { + if (Math.abs(sv.trackTrajF.get(svzLength - 1).Q - this.finalStateVec.Q) < Constants.ITERSTOPQTB + && Math.abs(sv.trackTrajF.get(svzLength - 1).x - this.finalStateVec.x) < Constants.ITERSTOPXTB + && Math.abs(sv.trackTrajF.get(svzLength - 1).y - this.finalStateVec.y) < Constants.ITERSTOPYTB + && Math.abs(sv.trackTrajF.get(svzLength - 1).tx - this.finalStateVec.tx) < Constants.ITERSTOPTXTB + && Math.abs(sv.trackTrajF.get(svzLength - 1).ty - this.finalStateVec.ty) < Constants.ITERSTOPTYTB) { + i = totNumIter; + } + } + this.finalStateVec = sv.trackTrajF.get(svzLength - 1); + + } else { + this.ConvStatus = 1; // Should be 0??? + } + } + } + + if (totNumIter == 1) { + if (this.setFitFailed == false && this.stopIteration == false) { + this.finalStateVec = sv.trackTrajF.get(svzLength - 1); + } + } + + this.calcFinalChisq(sector); + + if (Double.isNaN(chi2)) { + this.setFitFailed = true; + } + + if (TBT == true) { + if (chi2 > initChi2) { // fit failed + this.finalStateVec = this.initialStateVec; + sv.trackTrajT.put(svzLength - 1, this.initialStateVec); + this.calcFinalChisq(sector, true); + } + } + + } + + private boolean filter(int k, boolean forward) { + StateVec sVec = sv.transported(forward).get(k); + org.jlab.clas.tracking.kalmanfilter.AMeasVecs.MeasVec mVec = mv.measurements.get(k); + + if (Double.isNaN(sVec.x) || Double.isNaN(sVec.y) + || Double.isNaN(sVec.tx) || Double.isNaN(sVec.ty) + || Double.isNaN(sVec.Q)) { + this.setFitFailed = true; + return false; + } + if (sVec != null && sVec.CM != null + && k < mv.measurements.size() && mVec.skip == false) { + + + 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]); + Matrix CaInv = this.filterCovMat(H, sVec.CM, V); + Matrix cMat = new Matrix(); + if (CaInv != null) { + Matrix5x5.copy(CaInv, cMat); + } else { + return false; + } + + for (int j = 0; j < 5; j++) { + // the gain matrix + K[j] = (H[0] * cMat.get(j, 0) + + H[1] * cMat.get(j, 1)) / V; + } + + Point3D point = new Point3D(sVec.x, sVec.y, mVec.surface.measPoint.z()); + double h = mv.hDoca(point, mVec.surface.wireLine[0]); + + double signMeas = 1; + double sign = 1; + if (mVec.surface.doca[1] != -99 + || !(Math.abs(mVec.surface.doca[0]) < 0.5 + && mVec.surface.doca[1] == -99)) { // use LR only for double hits && large + // enough docas + signMeas = Math.signum(mVec.surface.doca[0]); + sign = Math.signum(h); + } else { + signMeas = Math.signum(h); + sign = Math.signum(h); + } + + double c2 = ((signMeas * Math.abs(mVec.surface.doca[0]) - sign * Math.abs(h)) + * (signMeas * Math.abs(mVec.surface.doca[0]) - sign * Math.abs(h)) / V); + + double x_filt = sVec.x + + K[0] * (signMeas * Math.abs(mVec.surface.doca[0]) - sign * Math.abs(h)); + double y_filt = sVec.y + + K[1] * (signMeas * Math.abs(mVec.surface.doca[0]) - sign * Math.abs(h)); + double tx_filt = sVec.tx + + K[2] * (signMeas * Math.abs(mVec.surface.doca[0]) - sign * Math.abs(h)); + double ty_filt = sVec.ty + + K[3] * (signMeas * Math.abs(mVec.surface.doca[0]) - sign * Math.abs(h)); + double Q_filt = sVec.Q + + K[4] * (signMeas * Math.abs(mVec.surface.doca[0]) - sign * Math.abs(h)); + + // USE THE DOUBLE HIT + 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]); + CaInv = this.filterCovMat(H, cMat, V); + if (CaInv != null) { + for (int i = 0; i < 5; i++) { + Matrix5x5.copy(CaInv, cMat); + } + } else { + return false; + } + for (int j = 0; j < 5; j++) { + // the gain matrix + K[j] = (H[0] * cMat.get(j, 0) + + H[1] * cMat.get(j, 1)) / V; + } + + Point3D point2 = new Point3D(x_filt, y_filt, mVec.surface.measPoint.z()); + + h = mv.hDoca(point2, mVec.surface.wireLine[1]); + + signMeas = Math.signum(mVec.surface.doca[1]); + sign = Math.signum(h); + + x_filt += K[0] * (signMeas * Math.abs(mVec.surface.doca[1]) - sign * Math.abs(h)); + y_filt += K[1] * (signMeas * Math.abs(mVec.surface.doca[1]) - sign * Math.abs(h)); + tx_filt += K[2] * (signMeas * Math.abs(mVec.surface.doca[1]) - sign * Math.abs(h)); + ty_filt += K[3] * (signMeas * Math.abs(mVec.surface.doca[1]) - sign * Math.abs(h)); + Q_filt += K[4] * (signMeas * Math.abs(mVec.surface.doca[1]) - sign * Math.abs(h)); + + c2 += ((signMeas * Math.abs(mVec.surface.doca[1]) - sign * Math.abs(h)) + * (signMeas * Math.abs(mVec.surface.doca[1]) - sign * Math.abs(h)) / V); + } + + chi2kf += c2; + if (filterOn) { + StateVec filteredVec = sv.new StateVec(k); + filteredVec.x = x_filt; + filteredVec.y = y_filt; + filteredVec.tx = tx_filt; + filteredVec.ty = ty_filt; + filteredVec.Q = Q_filt; + filteredVec.z = sVec.z; + filteredVec.B = sVec.B; + filteredVec.deltaPath = sVec.deltaPath; + + filteredVec.CM = cMat; + + sv.filtered(forward).put(k, filteredVec); + } + else { + return false; + } + + return true; + } + else { + return false; + } + } + + public Matrix filterCovMat(double[] H, Matrix Ci, double V) { + + double det = Matrix5x5.inverse(Ci, first_inverse, adj); + if(Math.abs(det)<1.e-60) + return null; + + addition.set( + H[0] * H[0] / V, H[0] * H[1] / V, 0, 0, 0, + H[0] * H[1] / V, H[1] * H[1] / V, 0, 0, 0, + 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0); + + Matrix5x5.add(first_inverse, addition, result); + double det2 = Matrix5x5.inverse(result, result_inv, adj); + if(Math.abs(det2)<1.e-60) + return null; + + return result_inv; + } + + private void calcFinalChisq(int sector) { + calcFinalChisq(sector, false); + } + + private void calcFinalChisq(int sector, boolean nofilter) { + int k = svzLength - 1; + this.chi2 = 0; + double path = 0; + double[] nRj = new double[3]; + + StateVec sVec; + + // To be changed: to match wit the old package, we make the following codes. Could be changed when other codes for application of calcFinalChisq are changed. + if(nofilter || (sv.trackTrajF.get(k) == null)) { + sVec = sv.trackTrajT.get(k); + } + else { + sVec = sv.trackTrajF.get(k); + } + + + kfStateVecsAlongTrajectory = new ArrayList<>(); + if (sVec != null && sVec.CM != null) { + + boolean forward = false; + sv.transport(sector, k, 0, sVec, mv, this.getSwimmer(), forward); + + StateVec svc = sv.transported(forward).get(0); + path += 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()); + double h0 = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[0]); + + svc.setProjector(mv.measurements.get(0).surface.wireLine[0].origin().x()); + svc.setProjectorDoca(h0); + kfStateVecsAlongTrajectory.add(svc); + double res = (mv.measurements.get(0).surface.doca[0] - h0); + chi2 += (mv.measurements.get(0).surface.doca[0] - h0) * (mv.measurements.get(0).surface.doca[0] - h0) / V0; + nRj[mv.measurements.get(0).region-1]+=res*res/mv.measurements.get(0).error; + //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]); + 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; + + StateVec svc2 = sv.new StateVec(svc); + svc2.setProjector(mv.measurements.get(0).surface.wireLine[1].origin().x()); + svc2.setProjectorDoca(h0); + kfStateVecsAlongTrajectory.add(svc2); + } + + forward = true; + for (int k1 = 0; k1 < k; k1++) { + if(k1 == 0) { + sv.transport(sector, k1, k1 + 1, svc, mv, this.getSwimmer(), forward); + } + else { + sv.transport(sector, k1, k1 + 1, sv.transported(forward).get(k1), mv, this.getSwimmer(), forward); + } + + 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()); + + double h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[0]); + svc = sv.transported(forward).get(k1+1); + path += svc.deltaPath; + svc.setPathLength(path); + svc.setProjector(mv.measurements.get(k1 + 1).surface.wireLine[0].origin().x()); + svc.setProjectorDoca(h); + kfStateVecsAlongTrajectory.add(svc); + res = (mv.measurements.get(k1 + 1).surface.doca[0] - h); + chi2 += (mv.measurements.get(k1 + 1).surface.doca[0] - h) * (mv.measurements.get(k1 + 1).surface.doca[0] - h) / V; + nRj[mv.measurements.get(k1 + 1).region-1]+=res*res/V; + //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]); + 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; + + StateVec svc2 = sv.new StateVec(svc); + svc2.setProjector(mv.measurements.get(k1 + 1).surface.wireLine[1].origin().x()); + svc2.setProjectorDoca(h); + kfStateVecsAlongTrajectory.add(svc2); + } + } + } + + } + + + public Matrix propagateToVtx(int sector, double Zf) { + return sv.transport(sector, finalStateVec.k, Zf, finalStateVec, mv, this.getSwimmer()); + } + + @Override + public void runFitter(AStateVecs sv, AMeasVecs mv) { + throw new UnsupportedOperationException("Not supported yet."); + } + + @Override + public StateVec filter(int k, StateVec vec, AMeasVecs mv) { + throw new UnsupportedOperationException("Not supported yet."); + } + + @Override + public StateVec smooth(int k, AStateVecs sv, AMeasVecs mv) { + throw new UnsupportedOperationException("Not supported yet."); + } + + @Override + public StateVec smooth(StateVec v1, StateVec v2) { + throw new UnsupportedOperationException("Not supported yet."); + } + + public MeasVecs getMeasVecs() { + return mv; + } + + public StateVecs getStateVecs() { + return sv; + } + + public void printlnMeasVecs() { + for (int i = 0; i < mv.measurements.size(); i++) { + org.jlab.clas.tracking.kalmanfilter.AMeasVecs.MeasVec measvec = mv.measurements.get(i); + String s = String.format("k=%d region=%d superlayer=%d layer=%d error=%.4f", measvec.k, measvec.region, measvec.superlayer, + measvec.layer, measvec.error); + s += String.format(" Surface: index=%d", measvec.surface.getIndex()); + s += String.format( + " Surface line 0: doca=%.4f unc=%.4f origin_x =%.4f, origin_y =%.4f, origin_z =%.4f, end_x=%.4f, end_y=%.4f, end_z=%.4f", + measvec.surface.doca[0], measvec.surface.unc[0], measvec.surface.wireLine[0].origin().x(), + measvec.surface.wireLine[0].origin().y(), measvec.surface.measPoint.z(), + measvec.surface.wireLine[0].end().x(), measvec.surface.wireLine[0].end().y(), + measvec.surface.wireLine[0].end().z()); + if (measvec.surface.wireLine[1] != null) + s += String.format( + " Surface line 1: doca=%.4f unc=%.4f origin_x =%.4f, origin_y =%.4f, origin_z =%.4f, end_x=%.4f, end_y=%.4f, end_z=%.4f", + measvec.surface.doca[1], measvec.surface.unc[1], measvec.surface.wireLine[1].origin().x(), + measvec.surface.wireLine[1].origin().y(), measvec.surface.wireLine[1].origin().z(), + measvec.surface.wireLine[1].end().x(), measvec.surface.wireLine[1].end().y(), + measvec.surface.wireLine[1].end().z()); + + System.out.println(s); + } + } + +} 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 new file mode 100644 index 0000000000..8814ca17ea --- /dev/null +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/MeasVecs.java @@ -0,0 +1,46 @@ +package org.jlab.clas.tracking.kalmanfilter.zReference; + +import java.util.ArrayList; +import java.util.List; + +import org.jlab.clas.swimtools.Swim; +import org.jlab.clas.tracking.kalmanfilter.AMeasVecs; +import org.jlab.clas.tracking.kalmanfilter.AStateVecs; +import org.jlab.clas.tracking.kalmanfilter.Material; +import org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec; +import org.jlab.geom.prim.Line3D; +import org.jlab.geom.prim.Point3D; + +/** + * + * @author tongtong cao + */ +public class MeasVecs extends AMeasVecs { + + public double[] H(double x, double y, double z, Line3D wireLine) { + double[] hMatrix = new double[5]; + double Err = 0.025; + double[][] Result = new double[2][2]; + 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); + } + 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); + } + + 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[4] = 0; + + return hMatrix; + } + + @Override + public double[] H(AStateVecs.StateVec stateVec, AStateVecs sv, MeasVec mv, Swim swimmer) { + throw new UnsupportedOperationException("Not supported yet."); + } +} diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/StateVecs.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/StateVecs.java new file mode 100644 index 0000000000..f69dd0bfc9 --- /dev/null +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/StateVecs.java @@ -0,0 +1,322 @@ +package org.jlab.clas.tracking.kalmanfilter.zReference; + +import org.jlab.clas.pdg.PhysicsConstants; +import org.jlab.clas.swimtools.Swim; +import org.jlab.clas.tracking.kalmanfilter.AMeasVecs; +import org.jlab.clas.tracking.kalmanfilter.AMeasVecs.MeasVec; +import org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec; +import org.jlab.clas.tracking.kalmanfilter.AStateVecs; +import org.jlab.clas.tracking.kalmanfilter.Surface; +import org.jlab.clas.tracking.kalmanfilter.Units; +import org.jlab.clas.tracking.trackrep.Helix; +import org.jlab.clas.tracking.utilities.MatrixOps; +import org.jlab.geom.prim.Line3D; +import org.jlab.geom.prim.Point3D; +import org.jlab.geom.prim.Vector3D; +import org.jlab.jnp.matrix.Matrix5x5; +import org.jlab.clas.tracking.utilities.RungeKuttaDoca; +import org.jlab.jnp.matrix.Matrix; + +/** + * + * @author Tongtong Cao + */ +public class StateVecs extends AStateVecs { + + private RungeKuttaDoca rk = new RungeKuttaDoca(); + private final Matrix fMS = new Matrix(); + private final Matrix copyMatrix = new Matrix(); + + public double Z[]; + + private double beta = 1.0; // beta depends on mass hypothesis + + // For hit-based tracking + public void init(StateVec initSV) { + this.initSV = initSV; + this.trackTrajT.clear(); + this.trackTrajF.clear(); + this.trackTrajP.clear(); + this.trackTrajB.clear(); + this.trackTrajS.clear(); + this.trackTrajT.put(0, new StateVec(initSV)); + } + + // For time-based tracking + public void initFromHB(StateVec initSV, double beta) { + this.beta = beta; + this.initSV = initSV; + this.trackTrajT.clear(); + this.trackTrajF.clear(); + this.trackTrajP.clear(); + this.trackTrajB.clear(); + this.trackTrajS.clear(); + this.trackTrajT.put(0, new StateVec(initSV)); + } + + /** + * + * @param sector + * @param i initial state vector index + * @param Zf + * @param iVec state vector at the initial index + * @param mv measurements + */ + public Matrix transport(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.CM; + + } + + /** + * + * @param sector + * @param i initial state vector index + * @param f final state vector index + * @param iVec state vector at the initial index + * @param mv measurements + */ + public boolean transport(int sector, int i, int f, StateVec iVec, AMeasVecs mv, Swim swimmer, boolean forward) { // s = signed step-size + if (iVec == null) { + return false; + } + double stepSize = 1.0; + StateVec fVec = new StateVec(f); + 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; + double zFinal = mv.measurements.get(f).surface.measPoint.z(); + + while (Math.signum(zFinal - zInit) * z < Math.signum(zFinal - zInit) * zFinal) { + z = fVec.z; + if (z == zFinal) { + 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(zFinal - 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(zFinal - zInit) * (z + s) > Math.signum(zFinal - zInit) * zFinal) { + s = Math.signum(zFinal - zInit) * Math.abs(zFinal - 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; + } + + if (forward) { + this.trackTrajT.put(f, fVec); + } else { + this.trackTrajP.put(f, fVec); + } + + if (Double.isNaN(fVec.x) || Double.isNaN(fVec.y) || Double.isNaN(fVec.tx) || Double.isNaN(fVec.ty) || Double.isNaN(fVec.Q)) { + return false; + } else { + return true; + } + + } + + //Todo: update it when updating multi-scattering + public double getX0(Surface surface, double z, double Z[]) { + double X0 = surface.getMaterials().get(0).getX0(); + double tolerance = 0.01; + + for (int i = 1; i < Z.length; i++) { + if (i % 2 == 0) { + continue; + } + if (z >= Z[i] - tolerance && z <= Z[i + 1] + tolerance) { + return surface.getMaterials().get(1).getX0(); + } + } + return X0; + } + + @Override + public double[][] Q(StateVec vec, AMeasVecs mv) { + throw new UnsupportedOperationException("Not supported yet."); + } + + @Override + public void corrForEloss(int dir, StateVec vec, AMeasVecs mv) { + throw new UnsupportedOperationException("Not supported yet."); + } + + @Override + public boolean getStateVecPosAtMeasSite(StateVec sv, AMeasVecs.MeasVec mv, Swim swim) { + throw new UnsupportedOperationException("Not supported yet."); + } + + @Override + public boolean setStateVecPosAtMeasSite(StateVec sv, MeasVec mv, Swim swimmer) { + throw new UnsupportedOperationException("Not supported yet."); + } + + @Override + public void printlnStateVec(StateVec S) { + throw new UnsupportedOperationException("Not supported yet."); + } + + @Override + public double[][] F(StateVec iVec, StateVec fVec) { + throw new UnsupportedOperationException("Not supported yet."); + } + + @Override + public void init(Helix helix, double[][] cov, double xref, double yref, double zref, double mass, Swim swimmer) { + throw new UnsupportedOperationException("Not for forward tracking."); + + } + + @Override + public void init(double x0, double z0, double tx, double tz, Units units, double[][] cov) { + throw new UnsupportedOperationException("Not for forward tracking."); + } +} diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/utilities/RungeKuttaDoca.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/utilities/RungeKuttaDoca.java new file mode 100644 index 0000000000..47129534df --- /dev/null +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/utilities/RungeKuttaDoca.java @@ -0,0 +1,365 @@ +package org.jlab.clas.tracking.utilities; + +import org.jlab.clas.pdg.PhysicsConstants; +import org.jlab.jnp.matrix.*; +import org.jlab.clas.swimtools.Swim; +import org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec;; + +/** + * Swims a given state vector to a given Z position using Runge Kutta 4 transport. + * @author ziegler + * @author benkel + * @author Tongtong Cao + */ +public class RungeKuttaDoca { + private final float[] _b = new float[3]; + final double v = PhysicsConstants.speedOfLight() * 0.0001; + + public RungeKuttaDoca() {} + + /** Swim to Z position without updating the covariance matrix. */ + public void SwimToZ(int sector, StateVec vec, Swim swim, double z0, float[] bf) { + double stepSize = 1.0; + swim.Bfield(sector, vec.x, vec.y, vec.z, bf); + + vec.B = Math.sqrt(bf[0]*bf[0] + bf[1]*bf[1] + bf[2]*bf[2]); + double s = vec.B; + final double travelSign = Math.signum(z0 - vec.z); + double BatMeas = 0; + + while(travelSign * vec.z < travelSign * z0) { + s = travelSign * stepSize; + if (travelSign*(vec.z+s) > travelSign*z0) s = travelSign*Math.abs(z0-vec.z); + + this.RK4transport(sector, s, swim, vec); + + if (Math.abs(vec.B - BatMeas) < 0.0001) stepSize *= 2; + BatMeas = vec.B; + } + } + + /** + * Transport using Runge Kutta 4 without updating the covariance matrix. Lab system = 1, TSC = 0. + * Internal state array (sN) is defined as {x, y, tx, ty}, since q doesn't change. + */ + private void RK4transport(int sector, double h, Swim swim, StateVec vec) { + // Set initial state. + double qv = vec.Q*v; + double[] s0 = {vec.x, vec.y, vec.tx, vec.ty}; + double[] sNull = {0, 0, 0, 0}; + + // Transport. + double[] s1 = RK4step(sector, vec.z, 0, swim, s0, sNull, qv); + double[] s2 = RK4step(sector, vec.z, 0.5*h, swim, s0, s1, qv); + double[] s3 = RK4step(sector, vec.z, 0.5*h, swim, s0, s2, qv); + double[] s4 = RK4step(sector, vec.z, h, swim, s0, s3, qv); + + // Set final state. + vec.z += h; + vec.x += this.RK4(s1[0], s2[0], s3[0], s4[0], h); + vec.y += this.RK4(s1[1], s2[1], s3[1], s4[1], h); + vec.tx += this.RK4(s1[2], s2[2], s3[2], s4[2], h); + vec.ty += this.RK4(s1[3], s2[3], s3[3], s4[3], h); + + vec.B = Math.sqrt(_b[0]*_b[0] + _b[1]*_b[1] + _b[2]*_b[2]); + vec.deltaPath += Math.sqrt((s0[0]-vec.x)*(s0[0]-vec.x)+(s0[1]-vec.y)*(s0[1]-vec.y)+h*h); + } + + /** Perform a single RK4 step without updating the covariance matrix. */ + private double[] RK4step(int sector, double z0, double h, Swim swim, double[] sInit, + double[] sPrev, double qv) { + swim.Bfield(sector, sInit[0]+h*sPrev[0], sInit[1]+h*sPrev[1], z0+h, _b); + double[] sNext = {0,0,0,0}; + sNext[0] = sInit[2] + h*sPrev[2]; + sNext[1] = sInit[3] + h*sPrev[3]; + double C = C(sNext[0], sNext[1]); + sNext[2] = qv * Ax(C, sNext[0], sNext[1]); + sNext[3] = qv * Ay(C, sNext[0], sNext[1]); + return sNext; + } + + /** Transport using Runge Kutta 4, updating the covariance matrix. */ + public void RK4transport(int sector, double h, Swim swim, Matrix cMat, StateVec vec) { + // Set initial state and Jacobian. + double qv = vec.Q*v; + RK4Vec s0 = new RK4Vec(vec); + RK4Vec sNull = new RK4Vec(); + + // Perform steps. + RK4Vec s1 = RK4step(sector, vec.z, 0, swim, s0, sNull, qv); + RK4Vec s2 = RK4step(sector, vec.z, 0.5*h, swim, s0, s1, qv); + RK4Vec s3 = RK4step(sector, vec.z, 0.5*h, swim, s0, s2, qv); + RK4Vec s4 = RK4step(sector, vec.z, h, swim, s0, s3, qv); + + // Compute and set final state and covariance matrix. + RK4Vec sF = computeFinalState(h, s0, s1, s2, s3, s4); + + vec.x = sF.x; + vec.y = sF.y; + vec.tx = sF.tx; + vec.ty = sF.ty; + vec.z += h; + vec.B = Math.sqrt(_b[0]*_b[0]+_b[1]*_b[1]+_b[2]*_b[2]); + vec.deltaPath += Math.sqrt((s0.x-vec.x)*(s0.x-vec.x) + (s0.y-vec.y)*(s0.y-vec.y) + h*h); + vec.CM.set(computeCovMat(cMat, sF)); + } + + /** Perform one RK4 step, updating the covariance matrix. */ + private RK4Vec RK4step(int sector, double z0, double h, Swim swim, + RK4Vec sInit, RK4Vec sPrev, double qv) { + RK4Vec sNext = new RK4Vec(); + swim.Bfield(sector, sInit.x + h*sPrev.x, sInit.y + h*sPrev.y, z0 + h, _b); + + // State. + sNext.x = sInit.tx + h*sPrev.tx; + sNext.y = sInit.ty + h*sPrev.ty; + + double Csq = Csq(sNext.x, sNext.y); + double C = C(Csq); + double Ax = Ax(C, sNext.x, sNext.y); + double Ay = Ay(C, sNext.x, sNext.y); + + sNext.tx = qv * Ax; + sNext.ty = qv * Ay; + + // Jacobian. + sNext.dxdtx0 = sInit.dtxdtx0 + h*sPrev.dtxdtx0; + sNext.dxdty0 = sInit.dtxdty0 + h*sPrev.dtxdty0; + sNext.dxdq0 = sInit.dtxdq0 + h*sPrev.dtxdq0; + sNext.dydtx0 = sInit.dtydtx0 + h*sPrev.dtydtx0; + sNext.dydty0 = sInit.dtydty0 + h*sPrev.dtydty0; + sNext.dydq0 = sInit.dtydq0 + h*sPrev.dtydq0; + + double dAx_dtx = dAx_dtx(C, Csq, Ax, sNext.x, sNext.y); + double dAx_dty = dAx_dty(C, Csq, Ax, sNext.x, sNext.y); + double dAy_dtx = dAy_dtx(C, Csq, Ay, sNext.x, sNext.y); + double dAy_dty = dAy_dty(C, Csq, Ay, sNext.x, sNext.y); + + sNext.dtxdtx0 = this.dtx_dtx0(qv, dAx_dtx, dAx_dty, sNext.dxdtx0, sNext.dydtx0); + sNext.dtxdty0 = this.dtx_dty0(qv, dAx_dty, sNext.dxdty0, sNext.dydty0); + sNext.dtxdq0 = this.dtx_dq0( qv, Ax, dAx_dtx, dAx_dty, sNext.dxdq0, sNext.dydq0); + sNext.dtydtx0 = this.dty_dtx0(qv, dAy_dtx, dAy_dty, sNext.dxdtx0, sNext.dydtx0); + sNext.dtydty0 = this.dty_dty0(qv, dAy_dty, sNext.dxdty0, sNext.dydty0); + sNext.dtydq0 = this.dty_dq0( qv, Ay, dAy_dtx, dAy_dty, sNext.dxdq0, sNext.dydq0); + + return sNext; + } + + /** Compute the final state for each entry in the internal state matrix. */ + private RK4Vec computeFinalState(double h, RK4Vec s0, RK4Vec s1, RK4Vec s2, RK4Vec s3, + RK4Vec s4) { + RK4Vec sF = new RK4Vec(); + + sF.x = s0.x + this.RK4(s1.x, s2.x, s3.x, s4.x, h); + sF.dxdtx0 = this.RK4(s1.dxdtx0, s2.dxdtx0, s3.dxdtx0, s4.dxdtx0, h); + sF.dxdty0 = this.RK4(s1.dxdty0, s2.dxdty0, s3.dxdty0, s4.dxdty0, h); + sF.dxdq0 = this.RK4(s1.dxdq0, s2.dxdq0, s3.dxdq0, s4.dxdq0, h); + + sF.y = s0.y + this.RK4(s1.y, s2.y, s3.y, s4.y, h); + sF.dydtx0 = this.RK4(s1.dydtx0, s2.dydtx0, s3.dydtx0, s4.dydtx0, h); + sF.dydty0 = this.RK4(s1.dydty0, s2.dydty0, s3.dydty0, s4.dydty0, h); + sF.dydq0 = this.RK4(s1.dydq0, s2.dydq0, s3.dydq0, s4.dydq0, h); + + sF.tx = s0.tx + this.RK4(s1.tx, s2.tx, s3.tx, s4.tx, h); + sF.dtxdtx0 = 1 + this.RK4(s1.dtxdtx0, s2.dtxdtx0, s3.dtxdtx0, s4.dtxdtx0, h); + sF.dtxdty0 = this.RK4(s1.dtxdty0, s2.dtxdty0, s3.dtxdty0, s4.dtxdty0, h); + sF.dtxdq0 = this.RK4(s1.dtxdq0, s2.dtxdq0, s3.dtxdq0, s4.dtxdq0, h); + + sF.ty = s0.ty + this.RK4(s1.ty, s2.ty, s3.ty, s4.ty, h); + sF.dtydtx0 = this.RK4(s1.dtydtx0, s2.dtydtx0, s3.dtydtx0, s4.dtydtx0, h); + sF.dtydty0 = 1 + this.RK4(s1.dtydty0, s2.dtydty0, s3.dtydty0, s4.dtydty0, h); + sF.dtydq0 = this.RK4(s1.dtydq0, s2.dtydq0, s3.dtydq0, s4.dtydq0, h); + + return sF; + } + + /** Compute the final covariance matrix. covMat = FCF^T. */ + private double[][] computeCovMat(Matrix C, RK4Vec sF) { + double[][] cNext = new double[5][5]; + cNext[0][0] = C.get(0,0)+C.get(2,0)*sF.dxdtx0+C.get(3,0)*sF.dxdty0+C.get(4,0)*sF.dxdq0 + + sF.dxdq0 *(C.get(0,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0) + + sF.dxdtx0*(C.get(0,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0) + + sF.dxdty0*(C.get(0,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0); + + cNext[0][1] = C.get(0,1)+C.get(2,1)*sF.dxdtx0+C.get(3,1)*sF.dxdty0+C.get(4,1)*sF.dxdq0 + + sF.dydq0 *(C.get(0,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0) + + sF.dydtx0*(C.get(0,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0) + + sF.dydty0*(C.get(0,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0); + + cNext[0][2] = sF.dtxdq0 *(C.get(0,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0) + + sF.dtxdtx0*(C.get(0,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0) + + sF.dtxdty0*(C.get(0,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0); + + cNext[0][3] = sF.dtydq0 *(C.get(0,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0) + + sF.dtydtx0*(C.get(0,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0) + + sF.dtydty0*(C.get(0,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0); + + cNext[0][4] = C.get(0,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0; + + cNext[1][0] = C.get(1,0)+C.get(2,0)*sF.dydtx0+C.get(3,0)*sF.dydty0+C.get(4,0)*sF.dydq0 + + sF.dxdq0 *(C.get(1,4)+C.get(2,4)*sF.dydtx0+C.get(3,4)*sF.dydty0+C.get(4,4)*sF.dydq0) + + sF.dxdtx0*(C.get(1,2)+C.get(2,2)*sF.dydtx0+C.get(3,2)*sF.dydty0+C.get(4,2)*sF.dydq0) + + sF.dxdty0*(C.get(1,3)+C.get(2,3)*sF.dydtx0+C.get(3,3)*sF.dydty0+C.get(4,3)*sF.dydq0); + + cNext[1][1] = C.get(1,1)+C.get(2,1)*sF.dydtx0+C.get(3,1)*sF.dydty0+C.get(4,1)*sF.dydq0 + + sF.dydq0 *(C.get(1,4)+C.get(2,4)*sF.dydtx0+C.get(3,4)*sF.dydty0+C.get(4,4)*sF.dydq0) + + sF.dydtx0*(C.get(1,2)+C.get(2,2)*sF.dydtx0+C.get(3,2)*sF.dydty0+C.get(4,2)*sF.dydq0) + + sF.dydty0*(C.get(1,3)+C.get(2,3)*sF.dydtx0+C.get(3,3)*sF.dydty0+C.get(4,3)*sF.dydq0); + + cNext[1][2] = sF.dtxdq0 *(C.get(1,4)+C.get(2,4)*sF.dydtx0+C.get(3,4)*sF.dydty0+C.get(4,4)*sF.dydq0) + + sF.dtxdtx0*(C.get(1,2)+C.get(2,2)*sF.dydtx0+C.get(3,2)*sF.dydty0+C.get(4,2)*sF.dydq0) + + sF.dtxdty0*(C.get(1,3)+C.get(2,3)*sF.dydtx0+C.get(3,3)*sF.dydty0+C.get(4,3)*sF.dydq0); + + cNext[1][3] = sF.dtydq0 *(C.get(1,4)+C.get(2,4)*sF.dydtx0+C.get(3,4)*sF.dydty0+C.get(4,4)*sF.dydq0) + + sF.dtydtx0*(C.get(1,2)+C.get(2,2)*sF.dydtx0+C.get(3,2)*sF.dydty0+C.get(4,2)*sF.dydq0) + + sF.dtydty0*(C.get(1,3)+C.get(2,3)*sF.dydtx0+C.get(3,3)*sF.dydty0+C.get(4,3)*sF.dydq0); + + cNext[1][4] = C.get(1,4)+C.get(2,4)*sF.dydtx0+C.get(3,4)*sF.dydty0+C.get(4,4)*sF.dydq0; + + cNext[2][0] = C.get(2,0)*sF.dtxdtx0+C.get(3,0)*sF.dtxdty0+C.get(4,0)*sF.dtxdq0 + + sF.dxdq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0) + + sF.dxdtx0*(C.get(2,2)*sF.dtxdtx0+C.get(3,2)*sF.dtxdty0+C.get(4,2)*sF.dtxdq0) + + sF.dxdty0*(C.get(2,3)*sF.dtxdtx0+C.get(3,3)*sF.dtxdty0+C.get(4,3)*sF.dtxdq0); + + cNext[2][1] = C.get(2,1)*sF.dtxdtx0+C.get(3,1)*sF.dtxdty0+C.get(4,1)*sF.dtxdq0 + + sF.dydq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0) + + sF.dydtx0*(C.get(2,2)*sF.dtxdtx0+C.get(3,2)*sF.dtxdty0+C.get(4,2)*sF.dtxdq0) + + sF.dydty0*(C.get(2,3)*sF.dtxdtx0+C.get(3,3)*sF.dtxdty0+C.get(4,3)*sF.dtxdq0); + + cNext[2][2] = sF.dtxdq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0) + + sF.dtxdtx0*(C.get(2,2)*sF.dtxdtx0+C.get(3,2)*sF.dtxdty0+C.get(4,2)*sF.dtxdq0) + + sF.dtxdty0*(C.get(2,3)*sF.dtxdtx0+C.get(3,3)*sF.dtxdty0+C.get(4,3)*sF.dtxdq0); + + cNext[2][3] = sF.dtydq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0) + + sF.dtydtx0*(C.get(2,2)*sF.dtxdtx0+C.get(3,2)*sF.dtxdty0+C.get(4,2)*sF.dtxdq0) + + sF.dtydty0*(C.get(2,3)*sF.dtxdtx0+C.get(3,3)*sF.dtxdty0+C.get(4,3)*sF.dtxdq0); + + cNext[2][4] = C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0; + + cNext[3][0] = C.get(2,0)*sF.dtydtx0+C.get(3,0)*sF.dtydty0+C.get(4,0)*sF.dtydq0 + + sF.dxdq0 *(C.get(2,4)*sF.dtydtx0+C.get(3,4)*sF.dtydty0+C.get(4,4)*sF.dtydq0) + + sF.dxdtx0*(C.get(2,2)*sF.dtydtx0+C.get(3,2)*sF.dtydty0+C.get(4,2)*sF.dtydq0) + + sF.dxdty0*(C.get(2,3)*sF.dtydtx0+C.get(3,3)*sF.dtydty0+C.get(4,3)*sF.dtydq0); + + cNext[3][1] = C.get(2,1)*sF.dtydtx0+C.get(3,1)*sF.dtydty0+C.get(4,1)*sF.dtydq0 + + sF.dydq0 *(C.get(2,4)*sF.dtydtx0+C.get(3,4)*sF.dtydty0+C.get(4,4)*sF.dtydq0) + + sF.dydtx0*(C.get(2,2)*sF.dtydtx0+C.get(3,2)*sF.dtydty0+C.get(4,2)*sF.dtydq0) + + sF.dydty0*(C.get(2,3)*sF.dtydtx0+C.get(3,3)*sF.dtydty0+C.get(4,3)*sF.dtydq0); + + cNext[3][2] = sF.dtxdq0 *(C.get(2,4)*sF.dtydtx0+C.get(3,4)*sF.dtydty0+C.get(4,4)*sF.dtydq0) + + sF.dtxdtx0*(C.get(2,2)*sF.dtydtx0+C.get(3,2)*sF.dtydty0+C.get(4,2)*sF.dtydq0) + + sF.dtxdty0*(C.get(2,3)*sF.dtydtx0+C.get(3,3)*sF.dtydty0+C.get(4,3)*sF.dtydq0); + + cNext[3][3] = sF.dtydq0 *(C.get(2,4)*sF.dtydtx0+C.get(3,4)*sF.dtydty0+C.get(4,4)*sF.dtydq0) + + sF.dtydtx0*(C.get(2,2)*sF.dtydtx0+C.get(3,2)*sF.dtydty0+C.get(4,2)*sF.dtydq0) + + sF.dtydty0*(C.get(2,3)*sF.dtydtx0+C.get(3,3)*sF.dtydty0+C.get(4,3)*sF.dtydq0); + + cNext[3][4] = C.get(2,4)*sF.dtydtx0+C.get(3,4)*sF.dtydty0+C.get(4,4)*sF.dtydq0; + + cNext[4][0] = C.get(4,0)+C.get(4,2)*sF.dxdtx0+C.get(4,3)*sF.dxdty0+C.get(4,4)*sF.dxdq0; + + cNext[4][1] = C.get(4,1)+C.get(4,2)*sF.dydtx0+C.get(4,3)*sF.dydty0+C.get(4,4)*sF.dydq0; + + cNext[4][2] = C.get(4,2)*sF.dtxdtx0+C.get(4,3)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0; + + cNext[4][3] = C.get(4,2)*sF.dtydtx0+C.get(4,3)*sF.dtydty0+C.get(4,4)*sF.dtydq0; + + cNext[4][4] = C.get(4,4); + + return cNext; + } + + /** Get the final RK4 estimate. */ + private double RK4(double k1, double k2, double k3, double k4, double h) { + return (h/6) * (k1 + 2*k2 + 2*k3 + k4); + } + + // Auxiliary calculations. + private double C(double tx, double ty) { + return Math.sqrt(1 + tx*tx + ty*ty); + } + private double C(double Csq) { + return Math.sqrt(Csq); + } + private double Csq(double tx, double ty) { + return 1 + tx*tx + ty*ty; + } + + private double Ax(double C, double tx, double ty) { + return C * (ty * (tx * _b[0] + _b[2]) - (1 + tx * tx) * _b[1]); + } + private double Ay(double C, double tx, double ty) { + return C * (-tx * (ty * _b[1] + _b[2]) + (1 + ty * ty) * _b[0]); + } + + private double dAx_dtx(double C, double C2, double Ax, double tx, double ty) { + return tx * Ax/C2 + C * (ty*_b[0] - 2*tx*_b[1]); + } + private double dAx_dty(double C, double C2, double Ax, double tx, double ty) { + return ty * Ax/C2 + C * (tx*_b[0] + _b[2]); + } + private double dAy_dtx(double C, double C2, double Ay, double tx, double ty) { + return tx * Ay/C2 + C * (-ty*_b[1] - _b[2]); + } + private double dAy_dty(double C, double C2, double Ay, double tx, double ty) { + return ty * Ay/C2 + C * (-tx*_b[1] + 2*ty*_b[0]); + } + + // Total derivatives. + private double dtx_dtx0(double qv, double dAx_dtx, double dAx_dty, + double dtx_dtx0, double dty_dtx0) { + return qv * (dAx_dtx*dtx_dtx0 + dAx_dty*dty_dtx0); + } + private double dtx_dty0(double qv, double dAx_dty, double dtx_dty0, double dty_dty0) { + return qv * (dAx_dty*dtx_dty0 + dAx_dty*dty_dty0); + } + private double dtx_dq0(double qv, double Ax, double dAx_dtx, double dAx_dty, + double dtx_dq0, double dty_dq0) { + return v*Ax + qv * (dAx_dtx*dtx_dq0 + dAx_dty*dty_dq0); + } + private double dty_dtx0(double qv, double dAy_dtx, double dAy_dty, + double dtx_dtx0, double dty_dtx0) { + return qv * (dAy_dtx*dtx_dtx0 + dAy_dty*dty_dtx0); + } + private double dty_dty0(double qv, double dAy_dty, double dtx_dty0, double dty_dty0) { + return qv * (dAy_dty*dtx_dty0 + dAy_dty*dty_dty0); + } + private double dty_dq0(double qv, double Ay, double dAy_dtx, double dAy_dty, + double dtx_dq0, double dty_dq0) { + return v*Ay + qv * (dAy_dtx*dtx_dq0 + dAy_dty*dty_dq0); + } + + /** State vector and its derivatives used internally. */ + private class RK4Vec { + public double x = 0; + public double dxdtx0 = 0; + public double dxdty0 = 0; + public double dxdq0 = 0; + + public double y = 0; + public double dydtx0 = 0; + public double dydty0 = 0; + public double dydq0 = 0; + + public double tx = 0; + public double dtxdtx0 = 0; + public double dtxdty0 = 0; + public double dtxdq0 = 0; + + public double ty = 0; + public double dtydtx0 = 0; + public double dtydty0 = 0; + public double dtydq0 = 0; + + RK4Vec() {} + RK4Vec(StateVec vec) { + this.x = vec.x; + this.y = vec.y; + this.tx = vec.tx; + this.ty = vec.ty; + + this.dtxdtx0 = 1; + this.dtydty0 = 1; + } + } +} diff --git a/common-tools/cnuphys/swimmer/src/main/java/cnuphys/swimZ/SwimZResult.java b/common-tools/cnuphys/swimmer/src/main/java/cnuphys/swimZ/SwimZResult.java index cb77549280..38fcff5151 100644 --- a/common-tools/cnuphys/swimmer/src/main/java/cnuphys/swimZ/SwimZResult.java +++ b/common-tools/cnuphys/swimmer/src/main/java/cnuphys/swimZ/SwimZResult.java @@ -209,6 +209,58 @@ public double sectorGetBDL(int sector, FieldProbe probe) { return _bdl; } + + /** + * Get the approximate integral |B x dL| in the middle plane + * + * @param sector sector 1..6 + * @param probe the probe use to compute this result trajectory + * @return the approximate integral |B x dL| in kG*cm + */ + public double sectorGetBDLXZPlane(int sector, FieldProbe probe) { + + // only compute if necessary + if (Double.isNaN(_bdl)) { + _bdl = 0; + _pathLength = 0; + + int size = size(); + + SwimZStateVector prev = null; + if (size > 1) { + + double dr[] = new double[3]; + + float b[] = new float[3]; + double bxdl[] = new double[3]; + double bxdlxz[] = new double[3]; + + for (SwimZStateVector next : _trajectory) { + if (prev != null) { + prev.dR(next, dr); + _pathLength += vecmag(dr); + + //get the field at the midpoint + float xmid = (float) ((prev.x + next.x) / 2); + float ymid = (float) ((prev.y + next.y) / 2); + float zmid = (float) ((prev.z + next.z) / 2); + probe.field(sector, xmid, ymid, zmid, b); + + cross(b, dr, bxdl); + bxdlxz[0] = bxdl[0]; + bxdlxz[1] = 0; + bxdlxz[2] = bxdl[2]; + + _bdl += vecmag(bxdlxz); + + } + prev = next; + } + } + } + + return _bdl; + } // usual cross product c = a x b private static void cross(float a[], double b[], double c[]) { diff --git a/common-tools/swim-tools/src/main/java/org/jlab/clas/swimtools/Swim.java b/common-tools/swim-tools/src/main/java/org/jlab/clas/swimtools/Swim.java index 5fb3fee461..5cec9b2b52 100644 --- a/common-tools/swim-tools/src/main/java/org/jlab/clas/swimtools/Swim.java +++ b/common-tools/swim-tools/src/main/java/org/jlab/clas/swimtools/Swim.java @@ -279,6 +279,88 @@ public double[] SwimToPlaneTiltSecSys(int sector, double z_cm) { return value; } + + public double[] SwimToPlaneTiltSecSysBdlXZPlane(int sector, double z_cm) { + double z = z_cm / 100; // the magfield method uses meters + double[] value = new double[8]; + + if (_pTot < MINTRKMOM || this.SwimUnPhys==true) // fiducial cut + { + return null; + } + + // use a SwimZResult instead of a trajectory (dph) + SwimZResult szr = null; + + SwimTrajectory traj = null; + double hdata[] = new double[3]; + + try { + + if (_pTot > SWIMZMINMOM) { + + // use the new z swimmer (dph) + // NOTE THE DISTANCE, UNITS FOR swimZ are cm, NOT m like the old + // swimmer (dph) + + double stepSizeCM = stepSize * 100; // convert to cm + + // create the starting SwimZ state vector + SwimZStateVector start = new SwimZStateVector(_x0 * 100, _y0 * 100, _z0 * 100, _pTot, _theta, _phi); + + try { + szr = PC.RCF_z.sectorAdaptiveRK(sector, _charge, _pTot, start, z_cm, stepSizeCM, hdata); + } catch (SwimZException e) { + szr = null; + //System.err.println("[WARNING] Tilted SwimZ Failed for p = " + _pTot); + } + } + + if (szr != null) { + double bdl = szr.sectorGetBDLXZPlane(sector, PC.RCF_z.getProbe()); + double pathLength = szr.getPathLength(); // already in cm + + SwimZStateVector last = szr.last(); + double p3[] = szr.getThreeMomentum(last); + + value[0] = last.x; // xf in cm + value[1] = last.y; // yz in cm + value[2] = last.z; // zf in cm + value[3] = p3[0]; + value[4] = p3[1]; + value[5] = p3[2]; + value[6] = pathLength; + value[7] = bdl / 10; // convert from kg*cm to T*cm + } else { // use old swimmer. Either low momentum or SwimZ failed. + // (dph) + + traj = PC.RCF.sectorSwim(sector, _charge, _x0, _y0, _z0, _pTot, _theta, _phi, z, accuracy, _rMax, + _maxPathLength, stepSize, cnuphys.swim.Swimmer.CLAS_Tolerance, hdata); + + // traj.computeBDL(sector, rprob); + if(traj==null) + return null; + + traj.sectorComputeBDL(sector, PC.RCP); + // traj.computeBDL(rcompositeField); + + double lastY[] = traj.lastElement(); + value[0] = lastY[0] * 100; // convert back to cm + value[1] = lastY[1] * 100; // convert back to cm + value[2] = lastY[2] * 100; // convert back to cm + value[3] = lastY[3] * _pTot; + value[4] = lastY[4] * _pTot; + value[5] = lastY[5] * _pTot; + value[6] = lastY[6] * 100; + value[7] = lastY[7] * 10; + } // use old swimmer + } catch (Exception e) { + e.printStackTrace(); + } + return value; + + } + /** * * @param z_cm diff --git a/etc/bankdefs/hipo4/dc.json b/etc/bankdefs/hipo4/dc.json index 2bba41bd85..e8dd22f348 100644 --- a/etc/bankdefs/hipo4/dc.json +++ b/etc/bankdefs/hipo4/dc.json @@ -266,7 +266,7 @@ {"name":"t1_z", "type":"F", "info":"Upstream Region 1 track z-position in the lab (in cm)"}, {"name":"t1_px", "type":"F", "info":"Upstream Region 1 track unit x-momentum vector in the lab"}, {"name":"t1_py", "type":"F", "info":"Upstream Region 1 track unit y-momentum vector in the lab"}, - {"name":"t1_pz", "type":"F", "info":"Upstream Region 1 track unit z-momentum vector in the lab"}, + {"name":"t1_pz", "type":"F", "info":"Upstream Region 1 track unit z-momentum vector in the lab"}, {"name":"Vtx0_x", "type":"F", "info":"Vertex x-position of the swam track to the DOCA to the beamline (in cm)"}, {"name":"Vtx0_y", "type":"F", "info":"Vertex y-position of the swam track to the DOCA to the beamline (in cm)"}, {"name":"Vtx0_z", "type":"F", "info":"Vertex z-position of the swam track to the DOCA to the beamline (in cm)"}, @@ -347,6 +347,7 @@ {"name":"trkDoca", "type":"F", "info":"track doca of the hit (in cm)"}, {"name":"timeResidual", "type":"F", "info":"time residual of the hit (in cm)"}, {"name":"fitResidual", "type":"F", "info":"fit residual of the hit (in cm, from KF)"}, + {"name":"DAFWeight", "type":"F", "info":"Weight by DAF; the 1st bit of status indicates that a hit is belong to single or double"}, {"name":"LR", "type":"B", "info":"Left/Right ambiguity of the hit"}, {"name":"X", "type":"F", "info":"wire x-coordinate in tilted-sector"}, {"name":"Z", "type":"F", "info":"wire z-coordinate in tilted-sector"}, @@ -499,7 +500,7 @@ {"name":"t1_z", "type":"F", "info":"Upstream Region 1 track z-position in the lab (in cm)"}, {"name":"t1_px", "type":"F", "info":"Upstream Region 1 track unit x-momentum vector in the lab"}, {"name":"t1_py", "type":"F", "info":"Upstream Region 1 track unit y-momentum vector in the lab"}, - {"name":"t1_pz", "type":"F", "info":"Upstream Region 1 track unit z-momentum vector in the lab"}, + {"name":"t1_pz", "type":"F", "info":"Upstream Region 1 track unit z-momentum vector in the lab"}, {"name":"Vtx0_x", "type":"F", "info":"Vertex x-position of the swam track to the DOCA to the beamline (in cm)"}, {"name":"Vtx0_y", "type":"F", "info":"Vertex y-position of the swam track to the DOCA to the beamline (in cm)"}, {"name":"Vtx0_z", "type":"F", "info":"Vertex z-position of the swam track to the DOCA to the beamline (in cm)"}, @@ -518,7 +519,8 @@ {"name":"q", "type":"B", "info":"charge of the track"}, {"name":"pathlength", "type":"F", "info":"pathlength of the track"}, {"name":"chi2", "type":"F", "info":"fit chi2 of the track"}, - {"name":"ndf", "type":"S", "info":"fit ndf of the track"} + {"name":"ndf", "type":"S", "info":"fit ndf weightd by DAF for the track"}, + {"name":"ndf0", "type":"S", "info":"traditional ndf for the track"} ] }, { diff --git a/etc/bankdefs/hipo4/dcnn.json b/etc/bankdefs/hipo4/dcnn.json index a007771fee..7a2aa7be58 100644 --- a/etc/bankdefs/hipo4/dcnn.json +++ b/etc/bankdefs/hipo4/dcnn.json @@ -224,6 +224,7 @@ {"name":"timeResidual", "type":"F", "info":"time residual of the hit (in cm)"}, {"name":"fitResidual", "type":"F", "info":"fit residual of the hit (in cm, from KF)"}, {"name":"dDoca", "type":"F", "info":"delta Doca correction (cm)"}, + {"name":"DAFWeight", "type":"F", "info":"Weight by DAF; the 1st bit of status indicates that a hit is belong to single or double"}, {"name":"LR", "type":"B", "info":"Left/Right ambiguity of the hit"}, {"name":"X", "type":"F", "info":"wire x-coordinate in tilted-sector"}, {"name":"Z", "type":"F", "info":"wire z-coordinate in tilted-sector"}, @@ -380,7 +381,8 @@ {"name":"q", "type":"B", "info":"charge of the track"}, {"name":"pathlength", "type":"F", "info":"pathlength of the track"}, {"name":"chi2", "type":"F", "info":"fit chi2 of the track"}, - {"name":"ndf", "type":"S", "info":"fit ndf of the track"} + {"name":"ndf", "type":"S", "info":"fit ndf weightd by DAF for the track"}, + {"name":"ndf0", "type":"S", "info":"traditional ndf for the track"} ] }, { diff --git a/etc/bankdefs/hipo4/urwell.json b/etc/bankdefs/hipo4/urwell.json index 0981d24a0c..913309dd6f 100644 --- a/etc/bankdefs/hipo4/urwell.json +++ b/etc/bankdefs/hipo4/urwell.json @@ -55,5 +55,5 @@ {"name":"cluster2", "type":"S", "info":"id of the cluster in the W layer"}, {"name":"status", "type":"S", "info":"status of the cluster"} ] - } + } ] \ No newline at end of file diff --git a/reconstruction/dc/pom.xml b/reconstruction/dc/pom.xml index b332bd7cc4..5a9f38b25b 100644 --- a/reconstruction/dc/pom.xml +++ b/reconstruction/dc/pom.xml @@ -18,6 +18,18 @@ + + + org.jlab.clas12.detector + clas12detector-urwell + 1.0-SNAPSHOT + + + + org.jlab.clas + clas-tracking + 10.0.11-SNAPSHOT + org.jlab.clas diff --git a/reconstruction/dc/src/main/java/org/jlab/rec/dc/Constants.java b/reconstruction/dc/src/main/java/org/jlab/rec/dc/Constants.java index d29dfc291d..11f0267afe 100644 --- a/reconstruction/dc/src/main/java/org/jlab/rec/dc/Constants.java +++ b/reconstruction/dc/src/main/java/org/jlab/rec/dc/Constants.java @@ -17,7 +17,6 @@ import org.jlab.geom.base.Detector; import org.jlab.rec.dc.trajectory.TrajectorySurfaces; import org.jlab.utils.groups.IndexedTable; -import org.jlab.clas.clas.math.FastMath; import org.jlab.detector.banks.RawBank.OrderType; /** @@ -52,20 +51,36 @@ public static Constants getInstance() { public static boolean DEBUG = false; // CONSTATNS for TRANSFORMATION - public static final double SIN25 = FastMath.sin(Math.toRadians(25.)); - public static final double COS25 = FastMath.cos(Math.toRadians(25.)); - public static final double COS30 = FastMath.cos(Math.toRadians(30.)); - public static final double SIN6 = FastMath.sin(Math.toRadians(6.)); - public static final double COS6 = FastMath.cos(Math.toRadians(6.)); + public static final double SIN25 = Math.sin(Math.toRadians(25.)); + public static final double COS25 = Math.cos(Math.toRadians(25.)); + public static final double COS30 = Math.cos(Math.toRadians(30.)); + public static final double SIN6 = Math.sin(Math.toRadians(6.)); + public static final double COS6 = Math.cos(Math.toRadians(6.)); public static final double TAN6 = Math.tan(Math.toRadians(6.)); - public static final double CTAN6 = 1 / TAN6; - public static final double[] SINSECTOR60 = {0, FastMath.sin(Math.toRadians(60.)), FastMath.sin(Math.toRadians(120.)), 0, - FastMath.sin(Math.toRadians(240.)), FastMath.sin(Math.toRadians(300.))}; + public static final double CTAN6 = 1/TAN6; + public static final double[] SINSECTOR60 = {0, Math.sin(Math.toRadians(60.)), Math.sin(Math.toRadians(120.)), 0, + Math.sin(Math.toRadians(240.)), Math.sin(Math.toRadians(300.))}; public static final double[] COSSECTOR60 = {1, 0.5, -0.5, -1, -0.5, 0.5}; - public static final double[] SINSECTORNEG60 = {0, FastMath.sin(Math.toRadians(-60.)), FastMath.sin(Math.toRadians(-120.)), 0, - FastMath.sin(Math.toRadians(-240.)), FastMath.sin(Math.toRadians(-300.))}; + public static final double[] SINSECTORNEG60 = {0, Math.sin(Math.toRadians(-60.)), Math.sin(Math.toRadians(-120.)), 0, + Math.sin(Math.toRadians(-240.)), Math.sin(Math.toRadians(-300.))}; public static final double[] COSSECTORNEG60 = {1, 0.5, -0.5, -1, -0.5, 0.5}; - + + ////////////// Uncertainties for initial state + ////// DC only + public static final double HBINITIALSTATEUNCSCALE = 1.5; + public static final double HBINITIALSTATEXUNC = 7.8; + public static final double HBINITIALSTATEYUNC = 5.7; + public static final double HBINITIALSTATETXUNC = 0.063; + public static final double HBINITIALSTATETYUNC = 0.036; + public static final double HBINITIALSTATEQUNC = 0.13; + + public static final double TBINITIALSTATEUNCSCALE = 1.5; + public static final double TBINITIALSTATEXUNC = 0.14; + public static final double TBINITIALSTATEYUNC = 1.03; + public static final double TBINITIALSTATETXUNC = 0.0025; + public static final double TBINITIALSTATETYUNC = 0.0091; + public static final double TBINITIALSTATEQUNC = 0.0084; + // PHYSICS CONSTANTS public static final double SPEEDLIGHT = 29.97924580; public static final double LIGHTVEL = 0.00299792458; // velocity of light (cm/ns) - conversion factor from radius in cm to momentum in GeV/c diff --git a/reconstruction/dc/src/main/java/org/jlab/rec/dc/banks/Banks.java b/reconstruction/dc/src/main/java/org/jlab/rec/dc/banks/Banks.java index 9f45773497..2243808840 100644 --- a/reconstruction/dc/src/main/java/org/jlab/rec/dc/banks/Banks.java +++ b/reconstruction/dc/src/main/java/org/jlab/rec/dc/banks/Banks.java @@ -120,7 +120,7 @@ public String getSegmentsBank() { public String getCrossesBank() { return this.getOutputBank("Crosses"); - } + } public String getTracksBank() { return this.getOutputBank("Tracks"); diff --git a/reconstruction/dc/src/main/java/org/jlab/rec/dc/banks/RecoBankWriter.java b/reconstruction/dc/src/main/java/org/jlab/rec/dc/banks/RecoBankWriter.java index 8f9735b5a6..3be9e11d45 100644 --- a/reconstruction/dc/src/main/java/org/jlab/rec/dc/banks/RecoBankWriter.java +++ b/reconstruction/dc/src/main/java/org/jlab/rec/dc/banks/RecoBankWriter.java @@ -322,7 +322,7 @@ public DataBank fillHBCrossesBank(DataEvent event, List crosslist) { } return bank; } - + public DataBank fillHBTracksBank(DataEvent event, List candlist) { String name = bankNames.getTracksBank(); DataBank bank = event.createBank(name, candlist.size()); @@ -398,7 +398,7 @@ public DataBank fillHBTracksBank(DataEvent event, List candlist) { //bank.show(); return bank; } - + public DataBank fillHBTrajectoryBank(DataEvent event, List candlist) { return this.fillTrajectoryBank(event, candlist); } @@ -495,6 +495,7 @@ private DataBank fillTBHitsBank(DataEvent event, List hitlist) { bank.setShort("clusterID", i, (short) hitlist.get(i).get_AssociatedClusterID()); bank.setByte("trkID", i, (byte) hitlist.get(i).get_AssociatedTBTrackID()); bank.setFloat("timeResidual", i, (float) hitlist.get(i).get_TimeResidual()); + bank.setFloat("DAFWeight", i, (float) hitlist.get(i).getDAFWeight()); bank.setInt("TDC",i,hitlist.get(i).get_TDC()); bank.setByte("jitter",i, (byte) hitlist.get(i).getJitter()); @@ -702,6 +703,7 @@ private DataBank fillTBTracksBank(DataEvent event, List candlist) { DataBank bank = event.createBank(name, candlist.size()); for (int i = 0; i < candlist.size(); i++) { bank.setShort("id", i, (short) candlist.get(i).get_Id()); + //bank.setShort("status", i, (short) (100+candlist.get(i).get_Status()*10+candlist.get(i).get_MissingSuperlayer())); bank.setShort("status", i, (short) candlist.get(i).getBitStatus()); bank.setByte("sector", i, (byte) candlist.get(i).getSector()); @@ -761,7 +763,17 @@ private DataBank fillTBTracksBank(DataEvent event, List candlist) { i, (short) candlist.get(i).getSingleSuperlayer().get_fittedCluster().get_Id()); } bank.setFloat("chi2", i, (float) candlist.get(i).get_FitChi2()); - bank.setShort("ndf", i, (short) candlist.get(i).get_FitNDF()); + // To not interrupt current type of ndf, ndf weighted by DAF is converted from float to interger + int ndfDAF = 999; + if(candlist.get(i).get_NDFDAF() > 0){ + ndfDAF = (int) Math.ceil(candlist.get(i).get_NDFDAF()); + } + else if (candlist.get(i).get_NDFDAF() < 0){ + ndfDAF = (int) Math.floor(candlist.get(i).get_NDFDAF()); + } + bank.setShort("ndf", i, (short) ndfDAF); + // ndf0 is for traditional ndf for the track; # of hits can be obtained through it + bank.setShort("ndf0", i, (short) candlist.get(i).get_FitNDF()); } return bank; @@ -866,9 +878,9 @@ else if (fhits != null && clusters == null) { ); } } - + public void fillAllTBBanks(DataEvent event, List fhits, List clusters, - List segments, List crosses, + List segments, List crosses, List trkcands) { if (event == null) { @@ -905,5 +917,5 @@ public void fillAllTBBanks(DataEvent event, List fhits, List { private double _tProp; private double _tStart; // The event start time private double _Time; //Time = TDC - tFlight - tProp - T0 - TStart + + private double _DAFWeight = -999; // Weight by DAF + /** * identifying outoftimehits; */ @@ -1037,13 +1040,30 @@ public double get_Beta0to1() { return beta; } + /** + * @return _DAFWeight + */ + public double getDAFWeight() { + return _DAFWeight; + } + + /** + * @param weight the _DAFWeight to set + */ + public void setDAFWeight(double weight) { + this._DAFWeight = weight; + } + public void updateHitfromSV(StateVec st, DCGeant4Factory DcDetector) { // this.set_Id(this.get_Id()); // this.set_TDC(this.get_TDC()); // this.set_AssociatedHBTrackID(trk.get_Id()); // this.set_AssociatedClusterID(this.get_AssociatedClusterID()); this.setAssociatedStateVec(st); - this.set_TrkResid(this.get_Doca() * Math.signum(st.getProjectorDoca()) - st.getProjectorDoca()); + this.set_TrkResid(this.get_Doca() * this.get_LeftRightAmb() - st.getProjectorDoca()); + this.setDAFWeight(st.getDAFWeight()); + //Set the first bit of TB hit status to indicate that a hit is belong to single or double + if(st.getIsDoubleHit() == 1) this.set_QualityFac(1); this.setB(st.getB()); this.setSignalPropagTimeAlongWire(st.x(), st.y(), DcDetector); this.setSignalTimeOfFlight(); diff --git a/reconstruction/dc/src/main/java/org/jlab/rec/dc/segment/Segment.java b/reconstruction/dc/src/main/java/org/jlab/rec/dc/segment/Segment.java index 3cb01b215b..b4617909f9 100644 --- a/reconstruction/dc/src/main/java/org/jlab/rec/dc/segment/Segment.java +++ b/reconstruction/dc/src/main/java/org/jlab/rec/dc/segment/Segment.java @@ -286,14 +286,14 @@ public void set_SegmentEndPointsSecCoordSys(DCGeant4Factory DcDetector) { //double Z_1 = GeometryLoader.dcDetector.getSector(0).getSuperlayer(this.get_Superlayer()-1).getLayer(0).getComponent(0).getMidpoint().z(); double Z_1 = DcDetector.getWireMidpoint(this.get_Sector() - 1, this.get_Superlayer() - 1, 0, 0).z; double X_1 = this.get_fittedCluster().get_clusterLineFitSlope() * Z_1 + this.get_fittedCluster().get_clusterLineFitIntercept(); - + double x1 = Constants.COS25 * X_1 + Constants.SIN25 * Z_1; double z1 = -Constants.SIN25 * X_1 + Constants.COS25 * Z_1; //double Z_2 = GeometryLoader.dcDetector.getSector(0).getSuperlayer(this.get_Superlayer()-1).getLayer(5).getComponent(0).getMidpoint().z(); double Z_2 = DcDetector.getWireMidpoint(this.get_Sector() - 1, this.get_Superlayer() - 1, 5, 0).z; double X_2 = this.get_fittedCluster().get_clusterLineFitSlope() * Z_2 + this.get_fittedCluster().get_clusterLineFitIntercept(); - + double x2 = Constants.COS25 * X_2 + Constants.SIN25 * Z_2; double z2 = -Constants.SIN25 * X_2 + Constants.COS25 * Z_2; @@ -349,7 +349,6 @@ public void set_SegmentEndPoints(double[] _SegmentEndPoints) { * by a point on the plane and a unit normal vector. */ private Plane3D calc_fitPlane(Point3D refPoint, Vector3D refDir) { - double X = Math.pow(-1, (this.get_Superlayer() - 1)) * Constants.SIN6; double Y = Constants.COS6; diff --git a/reconstruction/dc/src/main/java/org/jlab/rec/dc/track/Track.java b/reconstruction/dc/src/main/java/org/jlab/rec/dc/track/Track.java index a0f92fb32f..5105388bc9 100644 --- a/reconstruction/dc/src/main/java/org/jlab/rec/dc/track/Track.java +++ b/reconstruction/dc/src/main/java/org/jlab/rec/dc/track/Track.java @@ -59,16 +59,17 @@ public void setFinalStateVec(StateVec finalStateVec) { private Vector3D _pAtOrig_TiltedCS; private String _trking; private int _FitNDF; + private double _NDFDAF; private double _fitChisq; public boolean fit_Successful; private int _missingSuperlayer; private Segment _singleSuperlayer ; private int _fitConvergenceStatus; private StateVec finalStateVec ; - - + public Track() { } + /** * * @return missing superlayer of the track @@ -122,6 +123,24 @@ public int getBitStatus() { } + /** + * Status for different cross combos: + * 1: combos from R0R1R2R3 + * 2: combos from R1R2R3 + * 3: combos from R0R2R3 + * 4: combos from R0R1R3 + * 5: combos from R0R1R2 + */ + private int _Status_crossCombo=0; + + public int get_Status_crossCombo() { + return _Status_crossCombo; + } + + public void set_Status_crossCombo(int _Status) { + this._Status_crossCombo = _Status; + } + /** * * @return id of the track @@ -333,6 +352,22 @@ public int get_FitNDF() { public void set_FitNDF(int _FitNDF) { this._FitNDF = _FitNDF; } + + /** + * + * @return Kalman fit NDF weighted DAF + */ + public double get_NDFDAF() { + return _NDFDAF; + } + /** + * + * @param _NDFDAF Kalman fit NDF weighted by DAF + */ + public void set_NDFDAF(double _NDFDAF) { + this._NDFDAF = _NDFDAF; + } + /** * * @return Kalman fit covariance matrix @@ -420,7 +455,7 @@ public boolean overlaps(Track o) { } return value; } - + public boolean bestChi2(Track o) { return this.get_FitChi2() trkcands) { // LOGGER.log(Level.FINE, "Checking overlaps for tracks "); // t1.printInfo();t2.printInfo(); if(i!=j && t1.overlaps(t2)) { - if(t1.get_FitChi2()>t2.get_FitChi2()) + if(t1.get_FitChi2()/t1.get_FitNDF()>t2.get_FitChi2()/t2.get_FitNDF()) overlap=true; - else if(t1.get_FitChi2()==t2.get_FitChi2() && i>j) + else if(t1.get_FitChi2()/t1.get_FitNDF()==t2.get_FitChi2()/t2.get_FitNDF() && i>j) overlap=true; } // LOGGER.log(Level.FINE, overlap); @@ -690,24 +696,6 @@ else if(t1.get_FitChi2()==t2.get_FitChi2() && i>j) trkcands.addAll(selectedTracks); } -// public void removeOverlappingTracks(List trkcands) { -// List selectedTracks = new ArrayList(); -// List list = new ArrayList(); -// int size = trkcands.size(); -// for (int i = 0; i < size; i++) { -// list.clear(); -// this.getOverlapLists(trkcands.get(i), trkcands, list); -// trkcands.removeAll(list); -// size -= list.size(); -// Track selectedTrk = this.FindBestTrack(list); -// if (selectedTrk == null) -// continue; -// //if(this.ListContainsTrack(selectedTracks, selectedTrk)==false) -// selectedTracks.add(selectedTrk); -// } -// //trkcands.removeAll(trkcands); -// trkcands.addAll(selectedTracks); -// } /** * @param selectedTracks the list of selected tracks * @param selectedTrk the selected track @@ -787,7 +775,7 @@ public void matchHits(List stateVecAtPlanesList, Track trk, c.get_Segment2().isOnTrack=true; for (FittedHit h1 : c.get_Segment1()) { if (Math.abs(st.getZ() - h1.get_Z()) < 0.1 && c.get_Segment1().get_Id() > -1 - && (h1.get_XWire() - st.getProjector()) < 0.1) { + && Math.abs(h1.get_XWire() - st.getProjector()) < 0.1) { h1.set_AssociatedHBTrackID(trk.get_Id()); h1.updateHitfromSV(st, DcDetector); @@ -796,7 +784,7 @@ public void matchHits(List stateVecAtPlanesList, Track trk, } for (FittedHit h1 : c.get_Segment2()) { if (Math.abs(st.getZ() - h1.get_Z()) < 0.1 && c.get_Segment2().get_Id() > -1 - && (h1.get_XWire() - st.getProjector()) < 0.1) { + && Math.abs(h1.get_XWire() - st.getProjector()) < 0.1) { h1.set_AssociatedHBTrackID(trk.get_Id()); h1.updateHitfromSV(st, DcDetector); @@ -825,7 +813,7 @@ public void setHitDoubletsInfo(Segment seg) { h.setB(o.getB()); h.setTProp(o.getTProp()); h.setTFlight(o.getTFlight()); - h.set_QualityFac(1); + //h.set_QualityFac(1); } } } @@ -903,48 +891,48 @@ private List findStraightTracks(CrossList crossList, DCGeant4Factory DcDe cand.get(0).get_Dir().x() / cand.get(0).get_Dir().z(), cand.get(0).get_Dir().y() / cand.get(0).get_Dir().z()); cand.set_StateVecAtReg1MiddlePlane(VecAtReg1MiddlePlane); - // initialize the fitter with the candidate track - KFitterDoca kFit = new KFitterDoca(cand, DcDetector, false, dcSwim, 0); - kFit.totNumIter = 1; - if(LOGGER.getLevel()==Level.FINE) { - startTime = System.currentTimeMillis(); - } - kFit.runFitter(cand.get(0).get_Sector()); - LOGGER.log(Level.FINE, "Kalman fitter = " + (System.currentTimeMillis() - startTime)); - + LOGGER.log(Level.FINE, "Kalman fitter - 2 = " + (System.currentTimeMillis() - startTime)); + + KFitterStraight kFZRef = new KFitterStraight(true, 1, 1, dcSwim, Constants.getInstance().Z, Libr.JNP); + List measSurfaces = getMeasSurfaces(cand, DcDetector); + StateVecs svs = new StateVecs(); + org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec initSV = svs.new StateVec(0); + getInitState(cand, measSurfaces.get(0).measPoint.z(), 0, initSV, dcSwim, new float[3]); + kFZRef.init(measSurfaces, initSV); + + kFZRef.runFitter(); - if (kFit.finalStateVec == null) { + if (kFZRef.finalStateVec == null) { continue; } - // initialize the state vector corresponding to the last measurement site - StateVec fn = new StateVec(); + List kfStateVecsAlongTrajectory = setKFStateVecsAlongTrajectory(kFZRef); - //LOGGER.log(Level.FINE, " fit failed due to chi2 "+kFit.setFitFailed+" p "+1./Math.abs(kFit.finalStateVec.Q)); - if (!kFit.setFitFailed && kFit.finalStateVec != null) { - // set the state vector at the last measurement site - fn.set(kFit.finalStateVec.x, kFit.finalStateVec.y, kFit.finalStateVec.tx, kFit.finalStateVec.ty); - //set the track parameters if the filter does not fail - fn.setZ(kFit.finalStateVec.z); + StateVec fn = new StateVec(); + if (!kFZRef.setFitFailed && kFZRef.finalStateVec != null) { + fn.set(kFZRef.finalStateVec.x, kFZRef.finalStateVec.y, kFZRef.finalStateVec.tx, kFZRef.finalStateVec.ty); + fn.setZ(kFZRef.finalStateVec.z); cand.setFinalStateVec(fn); - cand.set_P(1. / Math.abs(kFit.finalStateVec.Q)); - cand.set_Q((int) Math.signum(kFit.finalStateVec.Q)); - this.setTrackPars(cand, traj, trjFind, fn, kFit.finalStateVec.z, DcDetector, dcSwim); - if(cand.fit_Successful==true) { - // candidate parameters are set from the state vector - cand.set_FitChi2(kFit.chi2); - cand.set_FitNDF(kFit.NDF); - cand.set_FitConvergenceStatus(kFit.ConvStatus); + + // set the track parameters + cand.set_P(1. / Math.abs(kFZRef.finalStateVec.Q)); + cand.set_Q((int) Math.signum(kFZRef.finalStateVec.Q)); + this.setTrackPars(cand, traj, trjFind, fn, kFZRef.finalStateVec.z, DcDetector, dcSwim); + + if (cand.fit_Successful == true) { + // candidate parameters + cand.set_FitChi2(kFZRef.chi2); + cand.set_FitNDF(kFZRef.NDF); + cand.set_FitConvergenceStatus(kFZRef.ConvStatus); cand.set_Id(cands.size() + 1); - cand.set_CovMat(kFit.finalCovMat.covMat); - cand.setStateVecs(kFit.kfStateVecsAlongTrajectory); - // add candidate to list of tracks + cand.set_CovMat(kFZRef.finalStateVec.CM); + cand.setStateVecs(kfStateVecsAlongTrajectory); + + // add candidate to list of tracks cands.add(cand); } } - //this.matchHits(traj.getStateVecs(), cand, DcDetector); -// cands.add(cand); } } } @@ -953,6 +941,8 @@ private List findStraightTracks(CrossList crossList, DCGeant4Factory DcDe private List findCurvedTracks(CrossList crossList, DCGeant4Factory DcDetector, double TORSCALE, Swim dcSwim, boolean donotapplyCuts) { + + if(LOGGER.getLevel()==Level.FINE) { startTime2 = System.currentTimeMillis(); } @@ -1018,95 +1008,16 @@ private List findCurvedTracks(CrossList crossList, DCGeant4Factory DcDete double theta3s1 = Math.atan(cand.get(2).get_Segment1().get_fittedCluster().get_clusterLineFitSlope()); double theta1s1 = Math.atan(cand.get(0).get_Segment1().get_fittedCluster().get_clusterLineFitSlope()); - if (cand.get(0).get_Segment2().get_Id() == -1) { - theta1s2 = theta1s1; //do not use - } //theta1s2=-999; //do not use - if (cand.get(0).get_Segment1().get_Id() == -1) { - theta1s1 = theta1s2; - } - //theta1s1=-999; - if (cand.get(2).get_Segment2().get_Id() == -1) { - theta3s2 = theta3s1; - } - //theta3s2=-999; - if (cand.get(2).get_Segment1().get_Id() == -1) { - theta3s1 = theta3s2; - } - //theta3s1=-999; - double theta3 = 0; - double theta1 = 0; - - double chisq = Double.POSITIVE_INFINITY; - double chi2; - double iBdl = traj.getIntegralBdl(); - double[] pars; - - if(LOGGER.getLevel()==Level.FINE) { - startTime = System.currentTimeMillis(); - } - pars = getTrackInitFit(cand.get(0).get_Sector(), x1, y1, z1, x2, y2, z2, x3, y3, z3, - ux, uy, uz, thX, thY, - theta1s1, theta3s1, - traj.getIntegralBdl(), TORSCALE, dcSwim); - chi2 = pars[0]; - if (chi2 < chisq) { - chisq = chi2; - theta1 = theta1s1; - theta3 = theta3s1; - iBdl = pars[1]; - } - - LOGGER.log(Level.FINE, "TrackInitFit-1 = " + (System.currentTimeMillis() - startTime)); + double theta1 = 0.5 * (theta1s1 + theta1s2); + double theta3 = 0.5 * (theta3s1 + theta3s2); - if(LOGGER.getLevel()==Level.FINE) { - startTime = System.currentTimeMillis(); - } - pars = getTrackInitFit(cand.get(0).get_Sector(), x1, y1, z1, x2, y2, z2, x3, y3, z3, + double[] pars = getTrackInitFit(cand.get(0).get_Sector(), x1, y1, z1, x2, y2, z2, x3, y3, z3, ux, uy, uz, thX, thY, - theta1s1, theta3s2, + theta1, theta3, traj.getA(), traj.getIntegralBdl(), TORSCALE, dcSwim); - chi2 = pars[0]; - if (chi2 < chisq) { - chisq = chi2; - theta1 = theta1s1; - theta3 = theta3s2; - iBdl = pars[1]; - } - LOGGER.log(Level.FINE, "TrackInitFit-2 = " + (System.currentTimeMillis() - startTime)); - - if(LOGGER.getLevel()==Level.FINE) { - startTime = System.currentTimeMillis(); - } - pars = getTrackInitFit(cand.get(0).get_Sector(), x1, y1, z1, x2, y2, z2, x3, y3, z3, - ux, uy, uz, thX, thY, - theta1s2, theta3s1, - traj.getIntegralBdl(), TORSCALE, dcSwim); - chi2 = pars[0]; - if (chi2 < chisq) { - chisq = chi2; - theta1 = theta1s2; - theta3 = theta3s1; - iBdl = pars[1]; - } - - LOGGER.log(Level.FINE, "TrackInitFit-3 = " + (System.currentTimeMillis() - startTime)); - - if(LOGGER.getLevel()==Level.FINE) { - startTime = System.currentTimeMillis(); - } - pars = getTrackInitFit(cand.get(0).get_Sector(), x1, y1, z1, x2, y2, z2, x3, y3, z3, - ux, uy, uz, thX, thY, - theta1s2, theta3s2, - traj.getIntegralBdl(), TORSCALE, dcSwim); - chi2 = pars[0]; - if (chi2 < chisq) { - theta1 = theta1s2; - theta3 = theta3s2; - iBdl = pars[1]; - } - - LOGGER.log(Level.FINE, "TrackInitFit-4 = " + (System.currentTimeMillis() - startTime)); + double chi2 = pars[0]; + double iBdl = pars[1]; if (chi2 > Constants.SEEDCUT && donotapplyCuts == false) { continue; @@ -1117,13 +1028,11 @@ private List findCurvedTracks(CrossList crossList, DCGeant4Factory DcDete //double iBdl = traj.getIntegralBdl(); if (iBdl != 0) { // momentum estimate if Bdl is non zero and the track has curvature - double p = calcInitTrkP(ux, uy, uz, thX, thY, - theta1, theta3, - iBdl, TORSCALE); + double p = calcInitTrkP(thX, thY, theta1, theta3, iBdl); if(LOGGER.getLevel()==Level.FINE) { startTime = System.currentTimeMillis(); } - int q = this.calcInitTrkQ(theta1, theta3, TORSCALE); + int q = this.calcInitTrkQ(traj.getA(), TORSCALE); LOGGER.log(Level.FINE, "calcInitTrkQ = " + (System.currentTimeMillis() - startTime)); @@ -1143,42 +1052,49 @@ private List findCurvedTracks(CrossList crossList, DCGeant4Factory DcDete cand.get(0).get_Dir().x() / cand.get(0).get_Dir().z(), cand.get(0).get_Dir().y() / cand.get(0).get_Dir().z()); cand.set_StateVecAtReg1MiddlePlane(VecAtReg1MiddlePlane); - // initialize the fitter with the candidate track - KFitterDoca kFit = null; - StateVec fitStateVec = null; - LOGGER.log(Level.FINE, "Kalman fitter - 2 = " + (System.currentTimeMillis() - startTime)); + StateVec fitStateVec = null; // prefer to initialize the seed with region 2 cross due to higher background in region 1 int crossIdxinList = 1; if (cand.get(1).isPseudoCross) { crossIdxinList = 0; } - kFit = new KFitterDoca(cand, DcDetector, false, dcSwim, crossIdxinList); - kFit.totNumIter = 10; - kFit.runFitter(cand.get(0).get_Sector()); - if (kFit.finalStateVec == null) { + + LOGGER.log(Level.FINE, "Kalman fitter - 2 = " + (System.currentTimeMillis() - startTime)); + + KFitter kFZRef = new KFitter(true, 10, 1, dcSwim, Constants.getInstance().Z, Libr.JNP); + List measSurfaces = getMeasSurfaces(cand, DcDetector); + StateVecs svs = new StateVecs(); + org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec initSV = svs.new StateVec(0); + getInitState(cand, measSurfaces.get(0).measPoint.z(), crossIdxinList, initSV, dcSwim, new float[3]); + kFZRef.init(measSurfaces, initSV); + + kFZRef.runFitter(); + List kfStateVecsAlongTrajectory = setKFStateVecsAlongTrajectory(kFZRef); + + if (kFZRef.finalStateVec == null) { continue; } else { - if (kFit.chi2 < Constants.MAXCHI2) { + if (kFZRef.chi2 < Constants.MAXCHI2) { - fitStateVec = new StateVec(kFit.finalStateVec.x, - kFit.finalStateVec.y, kFit.finalStateVec.tx, kFit.finalStateVec.ty); - q = (int) Math.signum(kFit.finalStateVec.Q); - p = 1. / Math.abs(kFit.finalStateVec.Q); - fitStateVec.setZ(kFit.finalStateVec.z); + fitStateVec = new StateVec(kFZRef.finalStateVec.x, + kFZRef.finalStateVec.y, kFZRef.finalStateVec.tx, kFZRef.finalStateVec.ty); + q = (int) Math.signum(kFZRef.finalStateVec.Q); + p = 1. / Math.abs(kFZRef.finalStateVec.Q); + fitStateVec.setZ(kFZRef.finalStateVec.z); //set the track parameters cand.set_P(p); cand.set_Q(q); // candidate parameters - cand.set_FitChi2(kFit.chi2); - cand.set_FitNDF(kFit.NDF); - cand.set_FitConvergenceStatus(kFit.ConvStatus); + cand.set_FitChi2(kFZRef.chi2); + cand.set_FitNDF(kFZRef.NDF); + cand.set_FitConvergenceStatus(kFZRef.ConvStatus); - cand.set_CovMat(kFit.finalCovMat.covMat); - cand.setStateVecs(kFit.kfStateVecsAlongTrajectory); + cand.set_CovMat(kFZRef.finalStateVec.CM); + cand.setStateVecs(kfStateVecsAlongTrajectory); cand.setFinalStateVec(fitStateVec); cand.set_Id(cands.size() + 1); @@ -1188,15 +1104,207 @@ private List findCurvedTracks(CrossList crossList, DCGeant4Factory DcDete DcDetector, dcSwim); // add candidate to list of tracks if (cand.fit_Successful = true) { - cands.add(cand); + cands.add(cand); } } } + + } } } } return cands; } + + public List setKFStateVecsAlongTrajectory(KFitter kFZRef) { + List 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.setProjector(svc.getProjector()); + sv.setProjectorDoca(svc.getProjectorDoca()); + kfStateVecsAlongTrajectory.add(sv); + } + + return kfStateVecsAlongTrajectory; + } + + public List setKFStateVecsAlongTrajectory(KFitterStraight kFZRef) { + List 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.setProjector(svc.getProjector()); + sv.setProjectorDoca(svc.getProjectorDoca()); + kfStateVecsAlongTrajectory.add(sv); + } + + return kfStateVecsAlongTrajectory; + } + + public void getInitState(Track trkcand, double z0, int c, org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec initStateVec, Swim dcSwim, float[] bf) { + + Point3D trkCrs = trkcand.get(c).get_Point(); + Point3D trkCrsD = trkcand.get(c).get_Dir(); + //org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec initSV = svs.new StateVec(0); + initStateVec.x = trkCrs.x(); + initStateVec.y = trkCrs.y(); + initStateVec.z = trkCrs.z(); + initStateVec.tx = trkCrsD.x()/trkCrsD.z(); + initStateVec.ty = trkCrsD.y()/trkCrsD.z(); + initStateVec.Q = trkcand.get_Q() / trkcand.get_P(); + + RungeKuttaDoca rk = new RungeKuttaDoca(); + rk.SwimToZ(trkcand.get(0).get_Sector(), initStateVec, dcSwim, z0, bf); + + double ex = Constants.HBINITIALSTATEUNCSCALE * Constants.HBINITIALSTATEXUNC; + double ey = Constants.HBINITIALSTATEUNCSCALE * Constants.HBINITIALSTATEYUNC; + double etx = Constants.HBINITIALSTATEUNCSCALE * Constants.HBINITIALSTATETXUNC; + double ety = Constants.HBINITIALSTATEUNCSCALE * Constants.HBINITIALSTATETYUNC; + double eQ = Constants.HBINITIALSTATEUNCSCALE * Constants.HBINITIALSTATEQUNC; + + Matrix initCMatrix = new Matrix(); + initCMatrix.set(ex * ex, 0, 0, 0, 0, + 0, ey * ey, 0, 0, 0, + 0, 0, etx * etx, 0, 0, + 0, 0, 0, ety * ety, 0, + 0, 0, 0, 0, eQ * eQ + ); + initStateVec.CM = initCMatrix; + } + + public List getMeasSurfaces(Track trkcand, DCGeant4Factory DcDetector) { + + List hOTS = new ArrayList<>(); // the list of hits on track + FittedHit hitOnTrk; + // loops over the regions (1 to 3) and the superlayers in a region (1 to 2) and obtains the hits on track + for (int c = 0; c < 3; c++) { + for (int s = 0; s < 2; s++) { + for (int h = 0; h < trkcand.get(c).get(s).size(); h++) { + // if (trkcand.get(c).get(s).get(h).get_Id() == -1 /*|| trkcand.get(c).get(s).get(h).get_Id() == 0*/) { //PASS1 + if (trkcand.get(c).get(s).get(h).get_Id() == -1 || trkcand.get(c).get(s).get(h).get_Id() == 0) { + continue; + } + trkcand.get(c).get(s).get(h).calc_CellSize(DcDetector); + hitOnTrk = trkcand.get(c).get(s).get(h); + int slayr = trkcand.get(c).get(s).get(h).get_Superlayer(); + + double sl1 = trkcand.get(c).get(s).get_fittedCluster().get_clusterLineFitSlope(); + double it1 = trkcand.get(c).get(s).get_fittedCluster().get_clusterLineFitIntercept(); + + double Z = hitOnTrk.get_Z(); + double X = sl1 * Z + it1; + + HitOnTrack hot = new HitOnTrack(slayr, X, Z, + trkcand.get(c).get(s).get(h).get_WireMaxSag(), + trkcand.get(c).get(s).get(h).get_WireLine() + ); + double err_sl1 = trkcand.get(c).get(s).get_fittedCluster().get_clusterLineFitSlopeErr(); + + double err_it1 = trkcand.get(c).get(s).get_fittedCluster().get_clusterLineFitInterceptErr(); + double err_cov1 = trkcand.get(c).get(s).get_fittedCluster().get_clusterLineFitSlIntCov(); + + hot._Unc[0] = Math.sqrt(err_sl1 * err_sl1 * Z * Z + err_it1 * err_it1); + hot._hitError = err_sl1 * err_sl1 * Z * Z + err_it1 * err_it1 + 2 * Z * err_cov1 + trkcand.get(c).get(s).get(h).get_DocaErr()*trkcand.get(c).get(s).get(h).get_DocaErr(); + //if(trkcand.get(c).get(s).get(h).get_Time()/CCDBConstants.getTMAXSUPERLAYER()[trkcand.get(c).get(s).get(h).get_Sector()-1][trkcand.get(c).get(s).get(h).get_Superlayer()-1]<1.1) + // hot._hitError = 100000; //exclude outoftimers from fit + hot.region = trkcand.get(c).get(s).get(h).get_Region(); + hot.sector = trkcand.get(c).get(s).get(h).get_Sector(); + hot.superlayer = trkcand.get(c).get(s).get(h).get_Superlayer(); + hot.layer = trkcand.get(c).get(s).get(h).get_Layer(); + + hot._doca[0] = trkcand.get(c).get(s).get(h).get_ClusFitDoca(); + + double LR = Math.signum(trkcand.get(c).get(s).get(h).get_XWire()-trkcand.get(c).get(s).get(h).get_X()); + + hot._doca[0]*=-LR; + hot._hitError = trkcand.get(c).get(s).get(h).get_DocaErr()*trkcand.get(c).get(s).get(h).get_DocaErr(); + //LOGGER.log(Level.FINE, " Z "+Z+" ferr "+(float)(hot._Unc /(hot._hitError/4.))); + hot._Unc[0] = hot._hitError; + hOTS.add(hot); + + } + } + } + Collections.sort(hOTS); // sort the collection in order of increasing Z value (i.e. going downstream from the target) + // identify double hits and take the average position + for (int i = 0; i < hOTS.size(); i++) { + if (i > 0) { + if (Math.abs(hOTS.get(i - 1)._Z - hOTS.get(i)._Z)<0.01) { + hOTS.get(i - 1)._doca[1] = hOTS.get(i)._doca[0]; + hOTS.get(i - 1)._Unc[1] = hOTS.get(i)._Unc[0]; + hOTS.get(i - 1)._wireLine[1] = hOTS.get(i)._wireLine[0]; + hOTS.remove(i); + hOTS.get(i - 1).nMeas = 2; + } + } + } + + List surfaces = new ArrayList<>(hOTS.size()); + for (int i = 0; i < hOTS.size(); i++) { + Surface surf = new Surface(hOTS.get(i).region, hOTS.get(i)._doca, hOTS.get(i)._Unc, hOTS.get(i)._hitError, hOTS.get(i)._wireLine); + surf.setSector(hOTS.get(i).sector); + surf.setSuperLayer(hOTS.get(i).superlayer); + surf.setLayer(hOTS.get(i).layer); + surf.setNMeas(hOTS.get(i).nMeas); + surfaces.add(i, surf); + } + + return surfaces; + } + + public class HitOnTrack implements Comparable { + + public double _hitError; + public double _X; + public double _Z; + public double[] _Unc = new double[2]; + public double _tilt; + public double[] _doca = new double[2]; + public double _wireMaxSag; + public Line3D[] _wireLine = new Line3D[2]; + public int region; + public int superlayer; + public int sector; + public int layer; + public int nMeas = 1; + + + public HitOnTrack(int superlayer, double X, double Z, double wiremaxsag, Line3D wireLine) { + _X = X; + _Z = Z; + _wireMaxSag = wiremaxsag; + _wireLine[0] = wireLine; + _doca[0] = -99; + _doca[1] = -99; + _Unc[0] = 1; + _Unc[1] = 1; + + //use stereo angle in fit based on wire direction + _tilt = 90-Math.toDegrees(wireLine.direction().asUnit().angle(new Vector3D(1,0,0))); + } + + @Override + public int compareTo(HitOnTrack o) { + if (this._Z == o._Z) { + return 0; + } + if (this._Z > o._Z) { + return 1; + } else { + return -1; + } + } + } + } diff --git a/reconstruction/dc/src/main/java/org/jlab/rec/dc/track/fit/MeasVecsDoca.java b/reconstruction/dc/src/main/java/org/jlab/rec/dc/track/fit/MeasVecsDoca.java index 1e36fdaa35..6dba453a00 100644 --- a/reconstruction/dc/src/main/java/org/jlab/rec/dc/track/fit/MeasVecsDoca.java +++ b/reconstruction/dc/src/main/java/org/jlab/rec/dc/track/fit/MeasVecsDoca.java @@ -48,7 +48,7 @@ public double h(double[] stateV, double Z, Line3D wireLine) { //LOGGER.log(Level.FINE, Math.signum(-WL.direction().x())+ // wireLine.origin().toString()+WL.toString()+" "+stateV[0]+" ,"+stateV[1]); - return WL.length()*Math.signum(-WL.direction().x()); + return WL.length()*Math.signum(WL.direction().x()); } public void setMeasVecs(Track trkcand, DCGeant4Factory DcDetector) { diff --git a/reconstruction/dc/src/main/java/org/jlab/rec/dc/trajectory/StateVec.java b/reconstruction/dc/src/main/java/org/jlab/rec/dc/trajectory/StateVec.java index 1bc2f7fa1c..6d8c5b296d 100644 --- a/reconstruction/dc/src/main/java/org/jlab/rec/dc/trajectory/StateVec.java +++ b/reconstruction/dc/src/main/java/org/jlab/rec/dc/trajectory/StateVec.java @@ -33,7 +33,27 @@ public double getPathLength() { public void setPathLength(double _PathLength) { this._PathLength = _PathLength; } + + private double _DAFWeight = -999; + + public double getDAFWeight() { + return _DAFWeight; + } + + public void setDAFWeight(double weight) { + this._DAFWeight = weight; + } + + private int _isDoubleHit = -1; + + public int getIsDoubleHit() { + return _isDoubleHit; + } + public void setIsDoubleHit(int _isDoubleHit) { + this._isDoubleHit = _isDoubleHit; + } + /** * Sets the. * diff --git a/reconstruction/dc/src/main/java/org/jlab/rec/dc/trajectory/TrackVec.java b/reconstruction/dc/src/main/java/org/jlab/rec/dc/trajectory/TrackVec.java index a1b3473d38..ac991d1401 100644 --- a/reconstruction/dc/src/main/java/org/jlab/rec/dc/trajectory/TrackVec.java +++ b/reconstruction/dc/src/main/java/org/jlab/rec/dc/trajectory/TrackVec.java @@ -228,7 +228,7 @@ public void TransformToTiltSectorFrame() { * @param t <0 going to lab, >0 going to tilted system * @return */ - public double[] tilt(double X, double Z, int t) { + public double[] tilt(double X, double Z, int t) { double rz = (double)t *X * Constants.SIN25 + Z * Constants.COS25; double rx = X * Constants.COS25 -(double)t* Z * Constants.SIN25; @@ -244,15 +244,16 @@ public double[] tilt(double X, double Z, int t) { */ public double[] rotateToSec(int sector, double x, double y, int t) { if(sector>0 && sector<7) { - if (t == 1) { + if(t == 1){ double rx = x * Constants.COSSECTOR60[sector - 1] - y * Constants.SINSECTOR60[sector - 1]; double ry = x * Constants.SINSECTOR60[sector - 1] + y * Constants.COSSECTOR60[sector - 1]; - return new double[]{rx, ry}; - } else { + return new double[] {rx, ry}; + } + else{ double rx = x * Constants.COSSECTORNEG60[sector - 1] - y * Constants.SINSECTORNEG60[sector - 1]; double ry = x * Constants.SINSECTORNEG60[sector - 1] + y * Constants.COSSECTORNEG60[sector - 1]; - return new double[]{rx, ry}; - } + return new double[] {rx, ry}; + } } else { return null; } diff --git a/reconstruction/dc/src/main/java/org/jlab/rec/dc/trajectory/Trajectory.java b/reconstruction/dc/src/main/java/org/jlab/rec/dc/trajectory/Trajectory.java index a9f0fc80bb..ddb69ebf94 100644 --- a/reconstruction/dc/src/main/java/org/jlab/rec/dc/trajectory/Trajectory.java +++ b/reconstruction/dc/src/main/java/org/jlab/rec/dc/trajectory/Trajectory.java @@ -31,9 +31,20 @@ public Trajectory() { private double pathLength; private List stateVecs; private List trajStateVecs = new ArrayList<>(); - + private final static double TOLERANCE = 0.1; // trajectory toleerance (cm) - + + private double a; // Parameter a for fitting function in xz plane: f(x) = a*z^2 + b*z + c + + public void setA(double a){ + this.a = a; + } + + // Return parameter a for fitting function f(x) = a*z^2 + b*z + c in xz plane + public double getA(){ + return a; + } + public List getStateVecs() { return stateVecs; } @@ -197,7 +208,7 @@ public void calcTrajectory(int trackId, Swim dcSwim, Point3D v, Vector3D p, int double[] htccPars = dcSwim.SwimToSphere(Constants.HTCCRADIUS); if(htccPars==null) return; this.addTrajectoryPoint(htccPars[0], htccPars[1], htccPars[2], htccPars[3], htccPars[4], htccPars[5], htccPars[6], htccPars[7], DetectorType.HTCC, 1); - + //Swim to planes for(int j = 0; j candCrossList, DCGeant4Factory DcDe traj.addAll(candCrossList); traj.setSector(candCrossList.get(0).get_Sector()); fitTrajectory(traj); - if (this.TrajChisqProbFitXZ getStateVecsAlongTrajectory(double x0, double y0, double z0, double tanTheta_x, double tanTheta_y, double p, int q, DCGeant4Factory DcDetector) { -// //initialize at target -// dcSwim.SetSwimParameters(x0, y0, z0, tanTheta_x, tanTheta_y, p, q); -// //position array -// double[] X = new double[36]; -// double[] Y = new double[36]; -// double[] Z = new double[36]; -// double[] thX = new double[36]; -// double[] thY = new double[36]; -// -// //Z[0] = GeometryLoader.dcDetector.getSector(0).getSuperlayer(0).getLayer(0).getPlane().point().z(); -// Z[0] = DcDetector.getLayerMidpoint(0, 0).z; -// double[] swamPars = dcSwim.SwimToPlane(Z[0]) ; -// X[0] = swamPars[0]; -// Y[0] = swamPars[1]; -// thX[0] = swamPars[3]/swamPars[5]; -// thY[0] = swamPars[4]/swamPars[5]; -// double pathLen = swamPars[6]; -// int planeIdx = 0; -// int lastSupLyrIdx = 0; -// int lastLyrIdx = 0; -// List stateVecAtPlanesList = new ArrayList(36); -// -// stateVecAtPlanesList.add(new StateVec(X[0],Y[0],thX[0], thY[0])); -// stateVecAtPlanesList.get(stateVecAtPlanesList.size()-1).setPathLength(pathLen); -// for(int superlayerIdx =0; superlayerIdx<6; superlayerIdx++) { -// for(int layerIdx =0; layerIdx<6; layerIdx++) { -// if(superlayerIdx ==0 && layerIdx==0) { -// continue; -// } else { -// // move to the next plane and determine the swam track parameters at that plane -// planeIdx++; -// dcSwim.SetSwimParameters(X[planeIdx-1], Y[planeIdx-1], Z[planeIdx-1], thX[planeIdx-1], thY[planeIdx-1], p, q); -// //Z[layerIdx] = GeometryLoader.dcDetector.getSector(0).getSuperlayer(superlayerIdx).getLayer(layerIdx).getPlane().point().z(); -// Z[planeIdx] = DcDetector.getLayerMidpoint(superlayerIdx, layerIdx).z; -// -// swamPars = dcSwim.SwimToPlane(Z[planeIdx]) ; -// X[planeIdx] = swamPars[0]; -// Y[planeIdx] = swamPars[1]; -// thX[planeIdx] = swamPars[3]/swamPars[5]; -// thY[planeIdx] = swamPars[4]/swamPars[5]; -// pathLen+=swamPars[6]; -// StateVec stVec = new StateVec(X[planeIdx],Y[planeIdx],thX[planeIdx], thY[planeIdx]); -// stVec.set_planeIdx(planeIdx); -// stVec.setPathLength(pathLen); -// stateVecAtPlanesList.add(stVec); -// } -// lastSupLyrIdx = superlayerIdx; -// lastLyrIdx = layerIdx; -// } -// } -// // return the list of state vectors at the list of measurement planes -// return stateVecAtPlanesList; -// } -// + /** * * @param DcDetector @@ -205,109 +143,100 @@ public List getStateVecsAlongTrajectory(DCGeant4Factory DcDetector) { * and constraining the quadratic parameters of the function describing the position values of the state vecs. * @param candCrossList list of crosses used in the fit */ - public void fitTrajectory(List candCrossList) { - tanTheta_x_fitCoeff = new double[2]; - tanTheta_y_fitCoeff = new double[2]; + public void fitTrajectory(List candCrossList) { x_fitCoeff = new double[3]; y_fitCoeff = new double[3]; - - + double[] theta_x = new double[3]; double[] theta_x_err = new double[3]; - double[] theta_y = new double[3]; - double[] theta_y_err = new double[3]; double[] x = new double[3]; - double[] x_err = new double[3]; double[] y = new double[3]; - double[] y_err = new double[3]; double[] z = new double[3]; - for (int i =0; i<3; i++) { + double[] x_err = new double[3]; + double[] y_err = new double[3]; + + for (int i = 0; i < 3; i++) { // make sure that the track direction makes sense - if(candCrossList.get(i).get_Dir().z()==0) { + if (candCrossList.get(i).get_Dir().z() == 0) { return; } x[i] = candCrossList.get(i).get_Point().x(); - x_err[i] = candCrossList.get(i).get_PointErr().x(); y[i] = candCrossList.get(i).get_Point().y(); - y_err[i] = candCrossList.get(i).get_PointErr().y(); z[i] = candCrossList.get(i).get_Point().z(); - theta_x[i] = candCrossList.get(i).get_Dir().x()/candCrossList.get(i).get_Dir().z(); - theta_x_err[i] = calcTanErr(candCrossList.get(i).get_Dir().x(),candCrossList.get(i).get_Dir().z(),candCrossList.get(i).get_DirErr().x(),candCrossList.get(i).get_DirErr().z()); - theta_y[i] = candCrossList.get(i).get_Dir().y()/candCrossList.get(i).get_Dir().z(); - theta_y_err[i] = calcTanErr(candCrossList.get(i).get_Dir().y(),candCrossList.get(i).get_Dir().z(),candCrossList.get(i).get_DirErr().y(),candCrossList.get(i).get_DirErr().z()); + x_err[i] = candCrossList.get(i).get_PointErr().x(); + y_err[i] = candCrossList.get(i).get_PointErr().x(); + + theta_x[i] = candCrossList.get(i).get_Dir().x() / candCrossList.get(i).get_Dir().z(); + theta_x_err[i] = calcTanErr(candCrossList.get(i).get_Dir().x(), candCrossList.get(i).get_Dir().z(), candCrossList.get(i).get_DirErr().x(), candCrossList.get(i).get_DirErr().z()); } lineFit = new LineFitter(); boolean linefitstatusOK = lineFit.fitStatus(z, theta_x, new double[3], theta_x_err, 3); - - // tan_thetax = alpha*z + beta; - // x = a*z^2 +b*z +c - if (linefitstatusOK) { - double alpha = lineFit.getFit().slope(); - double beta = lineFit.getFit().intercept(); - - - double a = alpha/2; - double b = beta; - - double sum_inv_xerr = 0; - double sum_X_ov_errX = 0; - for (int i =0; i<3; i++) { - x[i]-=a*z[i]*z[i]+b*z[i]; - sum_inv_xerr += 1./x_err[i]; - sum_X_ov_errX += x[i]/x_err[i]; - } - if(sum_inv_xerr==0) { - return; - } - double c = sum_X_ov_errX/sum_inv_xerr; - - tanTheta_x_fitCoeff[0] = alpha; - tanTheta_x_fitCoeff[1] = beta; - - x_fitCoeff[0] = a; - x_fitCoeff[1] = b; - x_fitCoeff[2] = c; - } TrajChisqProbFitXZ = lineFit.getFit().getProb(); - lineFit = new LineFitter(); - linefitstatusOK = lineFit.fitStatus(z, theta_y, new double[3], theta_y_err, 3); - - // tan_thetay = alpha*z + beta; - // y = a*z^2 +b*z +c - if (linefitstatusOK) { - double alpha = lineFit.getFit().slope(); - double beta = lineFit.getFit().intercept(); - - double a = alpha/2; - double b = beta; - - double sum_inv_yerr = 0; - double sum_Y_ov_errY = 0; - for (int i =0; i<3; i++) { - y[i]-=a*z[i]*z[i]+b*z[i]; - sum_inv_yerr += 1./y_err[i]; - sum_Y_ov_errY += y[i]/y_err[i]; - } - if(sum_inv_yerr==0) { - return; + x_fitCoeff = quadraticLRFit(z, x, x_err); + y_fitCoeff = quadraticLRFit(z, y, y_err); + } + + private double[] quadraticLRFit(double[] x, double[] y, double[] err) { + double[] ret = {0., 0., 0.}; + + Matrix A = new Matrix(3, 3); + Matrix V = new Matrix(3, 1); + double sum1 = 0.0; + double sum2 = 0.0; + double sum3 = 0.0; + double sum4 = 0.0; + double sum5 = 0.0; + double sum6 = 0.0; + double sum7 = 0.0; + double sum8 = 0.0; + for (int i = 0; i < x.length; ++i) { + double y1 = y[i]; + double x1 = x[i]; + double x2 = x1 * x1; + double x3 = x2 * x1; + double x4 = x2 * x2; + double e2 = err[i] * err[i]; + sum1 += x4 / e2; + sum2 += x3 / e2; + sum3 += x2 / e2; + sum4 += x1 / e2; + sum5 += 1.0 / e2; + sum6 += y1 * x2 / e2; + sum7 += y1 * x1 / e2; + sum8 += y1 / e2; + } + A.set(0, 0, sum1); + A.set(0, 1, sum2); + A.set(0, 2, sum3); + A.set(1, 0, sum2); + A.set(1, 1, sum3); + A.set(1, 2, sum4); + A.set(2, 0, sum3); + A.set(2, 1, sum4); + A.set(2, 2, sum5); + V.set(0, 0, sum6); + V.set(1, 0, sum7); + V.set(2, 0, sum8); + Matrix Ainv = A.inverse(); + Matrix X; + try { + X = Ainv.times(V); + for (int i = 0; i < 3; ++i) { + ret[i] = X.get(i, 0); } - double c = sum_Y_ov_errY/sum_inv_yerr; - tanTheta_y_fitCoeff[0] = alpha; - tanTheta_y_fitCoeff[1] = beta; - - y_fitCoeff[0] = a; - y_fitCoeff[1] = b; - y_fitCoeff[2] = c; + } catch (ArithmeticException e) { + // TODO Auto-generated catch block } - TrajChisqProbFitYZ = lineFit.getFit().getProb(); + return (ret); } + /** * * @param num diff --git a/reconstruction/dc/src/main/java/org/jlab/service/dc/DCEngine.java b/reconstruction/dc/src/main/java/org/jlab/service/dc/DCEngine.java index c7dde27cbf..80064a0363 100644 --- a/reconstruction/dc/src/main/java/org/jlab/service/dc/DCEngine.java +++ b/reconstruction/dc/src/main/java/org/jlab/service/dc/DCEngine.java @@ -10,6 +10,8 @@ import org.jlab.io.base.DataEvent; import org.jlab.rec.dc.Constants; import org.jlab.rec.dc.banks.Banks; +import org.jlab.clas.tracking.kalmanfilter.zReference.KFitter; +import org.jlab.clas.tracking.kalmanfilter.zReference.DAFilter; public class DCEngine extends ReconstructionEngine { @@ -31,6 +33,9 @@ public class DCEngine extends ReconstructionEngine { private String inBankPrefix = null; private String outBankPrefix = null; private double[][] shifts = new double[Constants.NREG][6]; + protected boolean useDAF = true; + private String dafChi2Cut = null; + private String dafAnnealingFactorsTB = null; public static final Logger LOGGER = Logger.getLogger(ReconstructionEngine.class.getName()); @@ -99,6 +104,20 @@ else if(this.getEngineConfigString("dcT2DFunc").equalsIgnoreCase("Polynomial")) outBankPrefix = this.getEngineConfigString("outputBankPrefix"); } + //Set if use DAF + if(this.getEngineConfigString("useDAF")!=null) + useDAF=Boolean.valueOf(this.getEngineConfigString("useDAF")); + + if(this.getEngineConfigString("dafChi2Cut")!=null) { + dafChi2Cut=this.getEngineConfigString("dafChi2Cut"); + DAFilter.setDafChi2Cut(Double.valueOf(dafChi2Cut)); + } + + if(this.getEngineConfigString("dafAnnealingFactorsTB")!=null){ + dafAnnealingFactorsTB=this.getEngineConfigString("dafAnnealingFactorsTB"); + KFitter.setDafAnnealingFactorsTB(dafAnnealingFactorsTB); + } + // Set geometry shifts for alignment code if(this.getEngineConfigString("alignmentShifts")!=null) { String[] alignmentShift = this.getEngineConfigString("alignmentShifts").split(","); diff --git a/reconstruction/dc/src/main/java/org/jlab/service/dc/DCTBEngine.java b/reconstruction/dc/src/main/java/org/jlab/service/dc/DCTBEngine.java index 98b00e3e6a..2bd9d1e9f9 100644 --- a/reconstruction/dc/src/main/java/org/jlab/service/dc/DCTBEngine.java +++ b/reconstruction/dc/src/main/java/org/jlab/service/dc/DCTBEngine.java @@ -1,16 +1,21 @@ package org.jlab.service.dc; import java.util.ArrayList; +import java.util.Collections; import java.util.Comparator; import java.util.HashMap; import java.util.List; import java.util.Map; import java.util.logging.Level; import org.jlab.clas.swimtools.Swim; +import org.jlab.clas.swimtools.Swimmer; +import org.jlab.detector.geant4.v2.DCGeant4Factory; +import org.jlab.geom.prim.Line3D; import org.jlab.geom.prim.Point3D; import org.jlab.geom.prim.Vector3D; import org.jlab.io.base.DataBank; import org.jlab.io.base.DataEvent; +import org.jlab.jnp.matrix.Matrix; import org.jlab.rec.dc.Constants; import org.jlab.rec.dc.banks.HitReader; import org.jlab.rec.dc.banks.RecoBankWriter; @@ -27,12 +32,18 @@ import org.jlab.rec.dc.timetodistance.TimeToDistanceEstimator; import org.jlab.rec.dc.track.Track; import org.jlab.rec.dc.track.TrackCandListFinder; -import org.jlab.rec.dc.track.fit.KFitterDoca; import org.jlab.rec.dc.trajectory.StateVec; import org.jlab.rec.dc.trajectory.Trajectory; import org.jlab.rec.dc.trajectory.TrajectoryFinder; import org.jlab.utils.groups.IndexedTable; +import org.jlab.clas.tracking.kalmanfilter.Surface; +import org.jlab.clas.tracking.kalmanfilter.zReference.KFitter; +import org.jlab.clas.tracking.kalmanfilter.zReference.KFitterStraight; +import org.jlab.clas.tracking.kalmanfilter.zReference.StateVecs; +import org.jlab.clas.tracking.utilities.MatrixOps.Libr; +import org.jlab.clas.tracking.utilities.RungeKuttaDoca; + public class DCTBEngine extends DCEngine { private TimeToDistanceEstimator tde = null; @@ -60,7 +71,6 @@ public void setDropBanks() { @Override public boolean processDataEvent(DataEvent event) { - int run = this.getRun(event); if(run==0) return true; @@ -165,9 +175,6 @@ public boolean processDataEvent(DataEvent event) { DataBank trkbank = event.getBank(this.getBanks().getInputTracksBank()); //DataBank trkcovbank = event.getBank("TimeBasedTrkg::TBCovMat"); int trkrows = trkbank.rows(); - //if(trkbank.rows()!=trkcovbank.rows()) { - // return true; // HB tracks not saved correctly - //} Track[] TrackArray = new Track[trkrows]; for (int i = 0; i < trkrows; i++) { Track HBtrk = new Track(); @@ -182,15 +189,6 @@ public boolean processDataEvent(DataEvent event) { trkbank.getFloat("tx", i), trkbank.getFloat("ty", i)); HBFinalSV.setZ(trkbank.getFloat("z", i)); HBtrk.setFinalStateVec(HBFinalSV); -// Matrix initCMatrix = new Matrix(); -// initCMatrix.set(new double[][]{ -// {trkcovbank.getFloat("C11", i), trkcovbank.getFloat("C12", i), trkcovbank.getFloat("C13", i), trkcovbank.getFloat("C14", i), trkcovbank.getFloat("C15", i)}, -// {trkcovbank.getFloat("C21", i), trkcovbank.getFloat("C22", i), trkcovbank.getFloat("C23", i), trkcovbank.getFloat("C24", i), trkcovbank.getFloat("C25", i)}, -// {trkcovbank.getFloat("C31", i), trkcovbank.getFloat("C32", i), trkcovbank.getFloat("C33", i), trkcovbank.getFloat("C34", i), trkcovbank.getFloat("C35", i)}, -// {trkcovbank.getFloat("C41", i), trkcovbank.getFloat("C42", i), trkcovbank.getFloat("C43", i), trkcovbank.getFloat("C44", i), trkcovbank.getFloat("C45", i)}, -// {trkcovbank.getFloat("C51", i), trkcovbank.getFloat("C52", i), trkcovbank.getFloat("C53", i), trkcovbank.getFloat("C54", i), trkcovbank.getFloat("C55", i)} -// }); -// HBtrk.set_CovMat(initCMatrix); TrackArray[HBtrk.get_Id()-1] = HBtrk; // TrackArray[HBtrk.get_Id()-1].set_Status(0); } @@ -218,6 +216,7 @@ public boolean processDataEvent(DataEvent event) { } TrackCandListFinder trkcandFinder = new TrackCandListFinder("TimeBased"); TrajectoryFinder trjFind = new TrajectoryFinder(); + for (Track TrackArray1 : TrackArray) { if (TrackArray1 == null || TrackArray1.get_ListOfHBSegments() == null || TrackArray1.get_ListOfHBSegments().size() < 5) { continue; @@ -228,44 +227,96 @@ public boolean processDataEvent(DataEvent event) { continue; } crosses.addAll(TrackArray1); - //if(TrackArray[i].get_FitChi2()>200) { - // resetTrackParams(TrackArray[i], new DCSwimmer()); - //} - KFitterDoca kFit = new KFitterDoca(TrackArray1, Constants.getInstance().dcDetector, true, dcSwim, 0); - StateVec fn = new StateVec(); - kFit.runFitter(TrackArray1.get(0).get_Sector()); - if (kFit.setFitFailed==false && kFit.finalStateVec!=null) { - // set the state vector at the last measurement site - fn.set(kFit.finalStateVec.x, kFit.finalStateVec.y, kFit.finalStateVec.tx, kFit.finalStateVec.ty); - //set the track parameters if the filter does not fail - TrackArray1.set_P(1./Math.abs(kFit.finalStateVec.Q)); - TrackArray1.set_Q((int)Math.signum(kFit.finalStateVec.Q)); - trkcandFinder.setTrackPars(TrackArray1, new Trajectory(), trjFind, fn, kFit.finalStateVec.z, Constants.getInstance().dcDetector, dcSwim, beamXoffset, beamYoffset); - // candidate parameters are set from the state vector - if (TrackArray1.fit_Successful == false) { - continue; - } - TrackArray1.set_FitChi2(kFit.chi2); - TrackArray1.set_FitNDF(kFit.NDF); - TrackArray1.setStateVecs(kFit.kfStateVecsAlongTrajectory); - TrackArray1.set_FitConvergenceStatus(kFit.ConvStatus); - //TrackArray[i].set_Id(TrackArray[i].size()+1); - //TrackArray[i].set_CovMat(kFit.finalCovMat.covMat); - if (TrackArray1.get_Vtx0().toVector3D().mag() > 500) { - continue; - } - // get CovMat at vertex - Point3D VTCS = crosses.get(0).getCoordsInSector(TrackArray1.get_Vtx0().x(), TrackArray1.get_Vtx0().y(), TrackArray1.get_Vtx0().z()); - TrackArray1.set_CovMat(kFit.propagateToVtx(crosses.get(0).get_Sector(), VTCS.z())); - if (TrackArray1.isGood()) { - trkcands.add(TrackArray1); - } + if(Math.abs(Swimmer.getTorScale()) < 0.001){ + KFitterStraight kFZRef = new KFitterStraight(true, 30, 1, dcSwim, Constants.getInstance().Z, Libr.JNP); + List measSurfaces = getMeasSurfaces(TrackArray1, Constants.getInstance().dcDetector); + StateVecs svs = new StateVecs(); + org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec initSV = svs.new StateVec(0); + getInitState(TrackArray1, measSurfaces.get(0).measPoint.z(), initSV, kFZRef, dcSwim, new float[3]); + kFZRef.initFromHB(measSurfaces, initSV, TrackArray1.get(0).get(0).get(0).get_Beta()); + kFZRef.runFitter(); + List kfStateVecsAlongTrajectory = setKFStateVecsAlongTrajectory(kFZRef); + + StateVec fn = new StateVec(); + if (kFZRef.setFitFailed==false && kFZRef.finalStateVec!=null) { + // set the state vector at the last measurement site + fn.set(kFZRef.finalStateVec.x, kFZRef.finalStateVec.y, kFZRef.finalStateVec.tx, kFZRef.finalStateVec.ty); + //set the track parameters if the filter does not fail + TrackArray1.set_P(1./Math.abs(kFZRef.finalStateVec.Q)); + TrackArray1.set_Q((int)Math.signum(kFZRef.finalStateVec.Q)); + + trkcandFinder.setTrackPars(TrackArray1, new Trajectory(), trjFind, fn, kFZRef.finalStateVec.z, Constants.getInstance().dcDetector, dcSwim, beamXoffset, beamYoffset); + // candidate parameters are set from the state vector + if (TrackArray1.fit_Successful == false) { + continue; + } + + TrackArray1.set_FitChi2(kFZRef.chi2); + TrackArray1.set_FitNDF(kFZRef.NDF); + TrackArray1.setStateVecs(kfStateVecsAlongTrajectory); + TrackArray1.set_FitConvergenceStatus(kFZRef.ConvStatus); + if (TrackArray1.get_Vtx0().toVector3D().mag() > 500) { + continue; + } + + // get CovMat at vertex + Point3D VTCS = crosses.get(0).getCoordsInSector(TrackArray1.get_Vtx0().x(), TrackArray1.get_Vtx0().y(), TrackArray1.get_Vtx0().z()); + TrackArray1.set_CovMat(kFZRef.propagateToVtx(crosses.get(0).get_Sector(), VTCS.z())); + + if (TrackArray1.isGood()) { + trkcands.add(TrackArray1); + } + } } - } - + else{ + KFitter kFZRef = new KFitter(true, 30, 1, dcSwim, Constants.getInstance().Z, Libr.JNP); + List measSurfaces = getMeasSurfaces(TrackArray1, Constants.getInstance().dcDetector); + StateVecs svs = new StateVecs(); + org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec initSV = svs.new StateVec(0); + getInitState(TrackArray1, measSurfaces.get(0).measPoint.z(), initSV, kFZRef, dcSwim, new float[3]); + kFZRef.initFromHB(measSurfaces, initSV, TrackArray1.get(0).get(0).get(0).get_Beta(), useDAF); + kFZRef.runFitter(useDAF); + + List kfStateVecsAlongTrajectory = setKFStateVecsAlongTrajectory(kFZRef); + + StateVec fn = new StateVec(); + if (kFZRef.setFitFailed==false && kFZRef.finalStateVec!=null) { + // set the state vector at the last measurement site + fn.set(kFZRef.finalStateVec.x, kFZRef.finalStateVec.y, kFZRef.finalStateVec.tx, kFZRef.finalStateVec.ty); + //set the track parameters if the filter does not fail + TrackArray1.set_P(1./Math.abs(kFZRef.finalStateVec.Q)); + TrackArray1.set_Q((int)Math.signum(kFZRef.finalStateVec.Q)); + + trkcandFinder.setTrackPars(TrackArray1, new Trajectory(), trjFind, fn, kFZRef.finalStateVec.z, Constants.getInstance().dcDetector, dcSwim, beamXoffset, beamYoffset); + // candidate parameters are set from the state vector + if (TrackArray1.fit_Successful == false) { + continue; + } + + TrackArray1.set_FitChi2(kFZRef.chi2); + TrackArray1.set_FitNDF(kFZRef.NDF); + TrackArray1.set_NDFDAF(kFZRef.getNDFDAF()); + TrackArray1.setStateVecs(kfStateVecsAlongTrajectory); + TrackArray1.set_FitConvergenceStatus(kFZRef.ConvStatus); + if (TrackArray1.get_Vtx0().toVector3D().mag() > 500) { + continue; + } + + // get CovMat at vertex + Point3D VTCS = crosses.get(0).getCoordsInSector(TrackArray1.get_Vtx0().x(), TrackArray1.get_Vtx0().y(), TrackArray1.get_Vtx0().z()); + TrackArray1.set_CovMat(kFZRef.propagateToVtx(crosses.get(0).get_Sector(), VTCS.z())); + + if (TrackArray1.isGood()) { + trkcands.add(TrackArray1); + } + } + } + + } + if(!trkcands.isEmpty()) { - //trkcandFinder.removeOverlappingTracks(trkcands); // remove overlaps + //trkcandFinder.removeOverlappingTracks(trkcands); // remove overlaps for(Track trk: trkcands) { int trkId = trk.get_Id(); // reset the id @@ -309,9 +360,295 @@ public boolean processDataEvent(DataEvent event) { return true; } + + public List setKFStateVecsAlongTrajectory(KFitter kFZRef) { + List 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.setProjector(svc.getProjector()); + sv.setProjectorDoca(svc.getProjectorDoca()); + sv.setDAFWeight(svc.getFinalDAFWeight()); + sv.setIsDoubleHit(svc.getIsDoubleHit()?1:0); + kfStateVecsAlongTrajectory.add(sv); + } + + return kfStateVecsAlongTrajectory; + } + + public List setKFStateVecsAlongTrajectory(KFitterStraight kFZRef) { + List 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.setProjector(svc.getProjector()); + sv.setProjectorDoca(svc.getProjectorDoca()); + kfStateVecsAlongTrajectory.add(sv); + } + + return kfStateVecsAlongTrajectory; + } + + public void getInitState(Track trkcand, double z0, org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec initStateVec, KFitter kf, Swim dcSwim, float[] bf) { + if (trkcand != null && trkcand.getFinalStateVec()!=null ) { + initStateVec.x = trkcand.getFinalStateVec().x(); + initStateVec.y = trkcand.getFinalStateVec().y(); + initStateVec.z = trkcand.getFinalStateVec().getZ(); + initStateVec.tx = trkcand.getFinalStateVec().tanThetaX(); + initStateVec.ty = trkcand.getFinalStateVec().tanThetaY(); + initStateVec.Q = ((double) trkcand.get_Q())/trkcand.get_P(); + + RungeKuttaDoca rk = new RungeKuttaDoca(); + rk.SwimToZ(trkcand.get(0).get_Sector(), initStateVec, dcSwim, z0, bf); + + double ex = Constants.TBINITIALSTATEUNCSCALE * Constants.TBINITIALSTATEXUNC; + double ey = Constants.TBINITIALSTATEUNCSCALE * Constants.TBINITIALSTATEYUNC; + double etx = Constants.TBINITIALSTATEUNCSCALE * Constants.TBINITIALSTATETXUNC; + double ety = Constants.TBINITIALSTATEUNCSCALE * Constants.TBINITIALSTATETYUNC; + double eQ = Constants.TBINITIALSTATEUNCSCALE * Constants.TBINITIALSTATEQUNC; + + //Matrix initCMatrix = new Matrix(FTF); + Matrix initCMatrix = new Matrix(); + initCMatrix.set(ex * ex, 0, 0, 0, 0, + 0, ey * ey, 0, 0, 0, + 0, 0, etx * etx, 0, 0, + 0, 0, 0, ety * ety, 0, + 0, 0, 0, 0, eQ * eQ + ); + initStateVec.CM = initCMatrix; + + } else { + kf.setFitFailed = true; + } + + } + + public void getInitState(Track trkcand, double z0, org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec initStateVec, KFitterStraight kf, Swim dcSwim, float[] bf) { + if (trkcand != null && trkcand.getFinalStateVec()!=null ) { + initStateVec.x = trkcand.getFinalStateVec().x(); + initStateVec.y = trkcand.getFinalStateVec().y(); + initStateVec.z = trkcand.getFinalStateVec().getZ(); + initStateVec.tx = trkcand.getFinalStateVec().tanThetaX(); + initStateVec.ty = trkcand.getFinalStateVec().tanThetaY(); + initStateVec.Q = ((double) trkcand.get_Q())/trkcand.get_P(); + + RungeKuttaDoca rk = new RungeKuttaDoca(); + rk.SwimToZ(trkcand.get(0).get_Sector(), initStateVec, dcSwim, z0, bf); + + double ex = Constants.TBINITIALSTATEUNCSCALE * Constants.TBINITIALSTATEXUNC; + double ey = Constants.TBINITIALSTATEUNCSCALE * Constants.TBINITIALSTATEYUNC; + double etx = Constants.TBINITIALSTATEUNCSCALE * Constants.TBINITIALSTATETXUNC; + double ety = Constants.TBINITIALSTATEUNCSCALE * Constants.TBINITIALSTATETYUNC; + double eQ = Constants.TBINITIALSTATEUNCSCALE * Constants.TBINITIALSTATEQUNC; + + //Matrix initCMatrix = new Matrix(FTF); + Matrix initCMatrix = new Matrix(); + initCMatrix.set(ex * ex, 0, 0, 0, 0, + 0, ey * ey, 0, 0, 0, + 0, 0, etx * etx, 0, 0, + 0, 0, 0, ety * ety, 0, + 0, 0, 0, 0, eQ * eQ + ); + initStateVec.CM = initCMatrix; + + } else { + kf.setFitFailed = true; + } + + } + + private org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec reset(org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec sv, org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec stateVec) { + StateVecs svs = new StateVecs(); + sv = svs.new StateVec(stateVec.k); + sv.x = stateVec.x; + sv.y = stateVec.y; + sv.tx = stateVec.tx; + sv.ty = stateVec.ty; + sv.z = stateVec.z; + sv.Q = stateVec.Q; + + return sv; + } + private void swimToSite(int sector, double z0, + org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec SVplus, org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec SVminus, RungeKuttaDoca rk, Swim dcSwim, float[] bf) { + rk.SwimToZ(sector, SVplus, dcSwim, z0, bf); + rk.SwimToZ(sector, SVminus, dcSwim, z0, bf); + } + double[] F(int sector, double z0, org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec stateVec, RungeKuttaDoca rk, Swim dcSwim, float[] bf) { + double[] _F = new double[5]; + org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec SVplus = null; + org.jlab.clas.tracking.kalmanfilter.AStateVecs.StateVec SVminus = null; + + SVplus = this.reset(SVplus, stateVec); + SVminus = this.reset(SVminus, stateVec); + + double delt_x = 0.05; + SVplus.x += delt_x/2.; + SVminus.x-= delt_x/2.; + + this.swimToSite(sector, z0, SVplus, SVminus, rk, dcSwim, bf); + + _F[0] = (SVplus.x - SVminus.x)/delt_x; + + SVplus = this.reset(SVplus, stateVec); + SVminus = this.reset(SVminus, stateVec); + + double delt_y = 0.05; + SVplus.y += delt_y/2.; + SVminus.y-= delt_y/2.; + + this.swimToSite(sector, z0, SVplus, SVminus, rk, dcSwim, bf); + + _F[1] = (SVplus.y - SVminus.y)/delt_y; + + SVplus = this.reset(SVplus, stateVec); + SVminus = this.reset(SVminus, stateVec); + + double delt_tx = 0.001; + SVplus.tx += delt_tx/2.; + SVminus.tx-= delt_tx/2.; + this.swimToSite(sector, z0, SVplus, SVminus, rk, dcSwim, bf); + + _F[2] = (SVplus.tx - SVminus.tx)/delt_tx; + + SVplus = this.reset(SVplus, stateVec); + SVminus = this.reset(SVminus, stateVec); + + double delt_ty = 0.001; + SVplus.ty += delt_ty/2.; + SVminus.ty-= delt_ty/2.; + + this.swimToSite(sector, z0, SVplus, SVminus, rk, dcSwim, bf); + + _F[3] = (SVplus.ty - SVminus.ty)/delt_ty; + + SVplus = this.reset(SVplus, stateVec); + SVminus = this.reset(SVminus, stateVec); + + + _F[4] = 0.01/Math.abs(SVplus.Q); + + return _F; + + } + + public List getMeasSurfaces(Track trk, DCGeant4Factory DcDetector) { + List hOTS = new ArrayList<>(); // the list of hits on track + FittedHit hitOnTrk; + for(int s = 0; s < trk.get_ListOfHBSegments().size(); s++) { + for(int h = 0; h < trk.get_ListOfHBSegments().get(s).size(); h++) { + trk.get_ListOfHBSegments().get(s).get(h).calc_CellSize(DcDetector); + hitOnTrk = trk.get_ListOfHBSegments().get(s).get(h); + int slayr = trk.get_ListOfHBSegments().get(s).get(h).get_Superlayer(); + + double sl1 = trk.get_ListOfHBSegments().get(s).get_fittedCluster().get_clusterLineFitSlope(); + double it1 = trk.get_ListOfHBSegments().get(s).get_fittedCluster().get_clusterLineFitIntercept(); + + double Z = hitOnTrk.get_Z(); + double X = sl1 * Z + it1; + HitOnTrack hot = new HitOnTrack(slayr, X, Z, + hitOnTrk.get_WireMaxSag(), + hitOnTrk.get_WireLine()); + + hot._doca[0] = trk.get_ListOfHBSegments().get(s).get(h).get_Doca(); + + double LR = Math.signum(trk.get_ListOfHBSegments().get(s).get(h).get_XWire()-trk.get_ListOfHBSegments().get(s).get(h).get_X()); + hot._doca[0]*=-LR; + hot._hitError = trk.get_ListOfHBSegments().get(s).get(h).get_DocaErr()*trk.get_ListOfHBSegments().get(s).get(h).get_DocaErr(); + //LOGGER.log(Level.FINE, " Z "+Z+" ferr "+(float)(hot._Unc /(hot._hitError/4.))); + hot._Unc[0] = hot._hitError; + hot.region = trk.get_ListOfHBSegments().get(s).get(h).get_Region(); + hot.sector = trk.get_ListOfHBSegments().get(s).get(h).get_Sector(); + hot.superlayer = trk.get_ListOfHBSegments().get(s).get(h).get_Superlayer(); + hot.layer = trk.get_ListOfHBSegments().get(s).get(h).get_Layer(); + hOTS.add(hot); + } + } + + Collections.sort(hOTS); // sort the collection in order of increasing Z value (i.e. going downstream from the target) + // identify double hits and take the average position + for (int i = 0; i < hOTS.size(); i++) { + if (i > 0) { + if (Math.abs(hOTS.get(i - 1)._Z - hOTS.get(i)._Z)<0.01) { + hOTS.get(i - 1)._doca[1] = hOTS.get(i)._doca[0]; + hOTS.get(i - 1)._Unc[1] = hOTS.get(i)._Unc[0]; + hOTS.get(i - 1)._wireLine[1] = hOTS.get(i)._wireLine[0]; + hOTS.remove(i); + hOTS.get(i - 1).nMeas = 2; + } + } + } + + List surfaces = new ArrayList<>(hOTS.size()); + + for (int i = 0; i < hOTS.size(); i++) { + Surface surf = new Surface(hOTS.get(i).region, hOTS.get(i)._doca, hOTS.get(i)._Unc, hOTS.get(i)._hitError, hOTS.get(i)._wireLine); + surf.setSector(hOTS.get(i).sector); + surf.setSuperLayer(hOTS.get(i).superlayer); + surf.setLayer(hOTS.get(i).layer); + surf.setNMeas(hOTS.get(i).nMeas); + surfaces.add(i, surf); + } + + return surfaces; + } + + public class HitOnTrack implements Comparable { + + public double _hitError; + public double _X; + public double _Z; + public double[] _Unc = new double[2]; + public double _tilt; + public double[] _doca = new double[2]; + public double _wireMaxSag; + public Line3D[] _wireLine = new Line3D[2]; + public int region; + public int superlayer; + public int sector; + public int layer; + public int nMeas = 1; + + + public HitOnTrack(int superlayer, double X, double Z, double wiremaxsag, Line3D wireLine) { + _X = X; + _Z = Z; + _wireMaxSag = wiremaxsag; + _wireLine[0] = wireLine; + _doca[0] = -99; + _doca[1] = -99; + _Unc[0] = 1; + _Unc[1] = 1; + + //use stereo angle in fit based on wire direction + _tilt = 90-Math.toDegrees(wireLine.direction().asUnit().angle(new Vector3D(1,0,0))); + } + + @Override + public int compareTo(HitOnTrack o) { + if (this._Z == o._Z) { + return 0; + } + if (this._Z > o._Z) { + return 1; + } else { + return -1; + } + } + } + + private int get_Status(Track track) { int miss = 0; diff --git a/reconstruction/dc/src/test/java/org/jlab/service/dc/DCReconstructionTest.java b/reconstruction/dc/src/test/java/org/jlab/service/dc/DCReconstructionTest.java index 645a242215..0954c9cd3a 100644 --- a/reconstruction/dc/src/test/java/org/jlab/service/dc/DCReconstructionTest.java +++ b/reconstruction/dc/src/test/java/org/jlab/service/dc/DCReconstructionTest.java @@ -85,8 +85,8 @@ public void testDCReconstruction() { assertEquals(testEvent.getBank("TimeBasedTrkg::TBTracks").getFloat("Vtx0_x", 0) < 0.2, true); assertEquals(testEvent.getBank("TimeBasedTrkg::TBTracks").getFloat("Vtx0_x", 0) > -0.2, true); - assertEquals(testEvent.getBank("TimeBasedTrkg::TBTracks").getFloat("Vtx0_y", 0) < 0.18, true); - assertEquals(testEvent.getBank("TimeBasedTrkg::TBTracks").getFloat("Vtx0_y", 0) > -0.228, true); + assertEquals(testEvent.getBank("TimeBasedTrkg::TBTracks").getFloat("Vtx0_y", 0) < 0.5, true); + assertEquals(testEvent.getBank("TimeBasedTrkg::TBTracks").getFloat("Vtx0_y", 0) > -0.5, true); assertEquals(testEvent.getBank("TimeBasedTrkg::TBTracks").getFloat("Vtx0_z", 0) < 0.885, true); assertEquals(testEvent.getBank("TimeBasedTrkg::TBTracks").getFloat("Vtx0_z", 0) > -0.0753, true); diff --git a/reconstruction/urwell/src/main/java/org/jlab/service/urwell/URWellConstants.java b/reconstruction/urwell/src/main/java/org/jlab/service/urwell/URWellConstants.java index 732c923fb1..a825418809 100644 --- a/reconstruction/urwell/src/main/java/org/jlab/service/urwell/URWellConstants.java +++ b/reconstruction/urwell/src/main/java/org/jlab/service/urwell/URWellConstants.java @@ -25,5 +25,5 @@ public class URWellConstants { // cluster public final static double COINCTIME = 100; - + }