diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Banks/RecoBankWriter.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Banks/RecoBankWriter.java index 2a4adce81f..f873773ca9 100644 --- a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Banks/RecoBankWriter.java +++ b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Banks/RecoBankWriter.java @@ -8,6 +8,8 @@ import org.jlab.rec.ahdc.Hit.Hit; import org.jlab.rec.ahdc.PreCluster.PreCluster; import org.jlab.rec.ahdc.Track.Track; +import org.apache.commons.math3.linear.RealVector; +import org.apache.commons.math3.linear.RealMatrix; import java.util.ArrayList; diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Hit/Hit.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Hit/Hit.java index 26007ae847..7e4f4546f5 100644 --- a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Hit/Hit.java +++ b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Hit/Hit.java @@ -1,12 +1,17 @@ package org.jlab.rec.ahdc.Hit; +import org.apache.commons.math3.linear.Array2DRowRealMatrix; +import org.apache.commons.math3.linear.ArrayRealVector; +import org.apache.commons.math3.linear.RealMatrix; +import org.apache.commons.math3.linear.RealVector; +import org.jlab.detector.calib.utils.DatabaseConstantProvider; import org.jlab.geom.detector.alert.AHDC.AlertDCDetector; import org.jlab.geom.prim.Line3D; import org.jlab.geom.prim.Point3D; +import org.jlab.geom.detector.alert.AHDC.AlertDCFactory; public class Hit implements Comparable { - private final double thster = Math.toRadians(20.0); private final int id; private final int superLayerId; private final int layerId; @@ -15,7 +20,7 @@ public class Hit implements Comparable { private final double adc; private final double time; - private Line3D wireLine; + private Line3D wireLine; private double phi; private double radius; private int nbOfWires; @@ -26,7 +31,7 @@ public class Hit implements Comparable { private double residual; private int trackId; - //updated constructor with ADC + //updated constructor with ADC public Hit(int _Id, int _Super_layer, int _Layer, int _Wire, double _Doca, double _ADC, double _Time) { this.id = _Id; this.superLayerId = _Super_layer; @@ -45,7 +50,7 @@ public void setWirePosition(AlertDCDetector factory) { //System.out.println(" superlayer " + this.superLayerId + " layer " + this.layerId + " wire " + this.wireId + " R_layer " + R_layer + " wx " + wx + " wy " + wy); wireLine = factory.getSector(1).getSuperlayer(superLayerId).getLayer(layerId).getComponent(wireId).getLine(); Point3D end = wireLine.end(); - this.nbOfWires = factory.getSector(1).getSuperlayer(superLayerId).getLayer(layerId).getNumComponents(); + this.nbOfWires = factory.getSector(1).getSuperlayer(superLayerId).getLayer(layerId).getNumComponents(); this.phi = end.vectorFrom(0, 0, 0).phi(); this.radius = end.distance(0, 0, end.z()); this.x = end.x(); @@ -57,12 +62,27 @@ public String toString() { return "Hit{" + "_Super_layer=" + superLayerId + ", _Layer=" + layerId + ", _Wire=" + wireId + ", _Doca=" + doca + ", _Phi=" + phi + '}'; } + // Should return + // 0 if equality + // +1 if this is bigger than arg0 + // -1 if this is lower than arg0 @Override public int compareTo(Hit arg0) { - if (this.superLayerId == arg0.superLayerId && this.layerId == arg0.layerId && this.wireId == arg0.wireId) { - return 0; - } else { - return 1; + if (this.superLayerId == arg0.superLayerId && this.layerId == arg0.layerId) { // same layer, so they have the same nbOfWires + if (this.wireId == 1 && arg0.wireId == this.nbOfWires) { + return 1; + } + else if (this.wireId == this.nbOfWires && arg0.wireId == 1) { + return -1; + } + else { + return Integer.compare(this.wireId, arg0.wireId); + } + } + else { + int this_value = 10*this.superLayerId + this.layerId; + int value = 10*arg0.superLayerId + arg0.layerId; + return Integer.compare(this_value, value); } } @@ -86,13 +106,13 @@ public double getDoca() { return doca; } - public Line3D getLine() { - return wireLine; - } - - public double getRadius() { - return radius; - } + public Line3D getLine() { + return wireLine; + } + + public double getRadius() { + return radius; + } public int getNbOfWires() { return nbOfWires; @@ -146,4 +166,38 @@ public void setTrackId(int _trackId) { this.trackId = _trackId; } + public RealVector get_Vector() { + return new ArrayRealVector(new double[]{this.doca}); + } + + public RealMatrix get_MeasurementNoise() { + return new Array2DRowRealMatrix(new double[][]{{0.09}}); + } + + // a signature for KalmanFilter.Hit_beam + public RealVector get_Vector_beam() { + return null; + } + + public double distance(Point3D point3D) { + return this.wireLine.distance(point3D).length(); + } + + public static void main(String[] args) { + AlertDCDetector factory = (new AlertDCFactory()).createDetectorCLAS(new DatabaseConstantProvider()); + System.out.println("Run test: comparison between two hits."); + Hit h1 = new Hit(1,1,1,1,0,0,0); + Hit h2 = new Hit(1,1,1,47,0,0,0); + Hit h3 = new Hit(1,2,1,47,0,0,0); + h1.setWirePosition(factory); + h2.setWirePosition(factory); + System.out.println("h1 : " + h1); + System.out.println("h2 : " + h2); + System.out.println("h3 : " + h3); + System.out.println("numWires : " + h1.getNbOfWires()); + System.out.println("h1 compare to h2 : " + h1.compareTo(h2)); + System.out.println("h2 compare to h1 : " + h2.compareTo(h1)); + System.out.println("h1 compare to h3 : " + h1.compareTo(h3)); + } + } diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Hit/HitReader.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Hit/HitReader.java index 7c4f304274..3636fe8c70 100644 --- a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Hit/HitReader.java +++ b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Hit/HitReader.java @@ -35,7 +35,7 @@ public final void fetch_AHDCHits(DataEvent event, AlertDCDetector detector) { } RawDataBank bankDGTZ = new RawDataBank("AHDC::adc"); - bankDGTZ.read(event); + bankDGTZ.read(event); for (int i = 0; i < bankDGTZ.rows(); i++) { int id = bankDGTZ.trueIndex(i) + 1; @@ -82,8 +82,8 @@ public final void fetch_AHDCHits(DataEvent event, AlertDCDetector detector) { double doca = p0 + p1*Math.pow(time,1.0) + p2*Math.pow(time,2.0) + p3*Math.pow(time,3.0) + p4*Math.pow(time,4.0) + p5*Math.pow(time, 5.0); if (time < 0) doca = 0; Hit h = new Hit(id, superlayer, layer, wire, doca, adc, time); - h.setWirePosition(detector); - hits.add(h); + h.setWirePosition(detector); + hits.add(h); } } } diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Hit.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Hit.java deleted file mode 100644 index 0aeabb8af0..0000000000 --- a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Hit.java +++ /dev/null @@ -1,181 +0,0 @@ -package org.jlab.rec.ahdc.KalmanFilter; - -import org.apache.commons.math3.linear.Array2DRowRealMatrix; -import org.apache.commons.math3.linear.ArrayRealVector; -import org.apache.commons.math3.linear.RealMatrix; -import org.apache.commons.math3.linear.RealVector; -import org.jlab.geom.prim.Line3D; -import org.jlab.geom.prim.Plane3D; -import org.jlab.geom.prim.Point3D; -import org.jlab.geom.prim.Vector3D; - -public class Hit implements Comparable { - - private final int superLayer; - private final int layer; - private final int wire; - private final double r; - private final double phi; - private final double doca; - private double adc; - private final double numWires; - private final Line3D line3D; - private final Line3D line3D_plus; - private final Line3D line3D_minus; - private int hitidx; - private int hitsign; - - // Comparison with: common-tools/clas-geometry/src/main/java/org/jlab/geom/detector/alert/AHDC/AlertDCFactory.java - // here, SuperLayer, Layer, Wire, start from 1 - // in AlertDCFactory, same variables start from 1 - public Hit(int superLayer, int layer, int wire, int numWire, Line3D line, double doca) { - this.superLayer = superLayer; - this.layer = layer; - this.wire = wire; - this.r = line.end().distance(0, 0, line.end().z()); - this.doca = doca; - this.numWires = numWire; - this.adc = 0;//placeholder - this.hitidx = -1; - this.hitsign = 0; - - this.phi = line.midpoint().vectorFrom(0,0,0).phi(); - //System.out.println(" superlayer " + this.superLayer + " layer " + this.layer + " wire " + this.wire + " wx " + wx + " wy " + wy + " wx_end " + wx_end + " wy_end " + wy_end + " phi " + this.phi); - - this.line3D = line; - - //calculate the "virtual" left and right wires accounting for the DOCA - double deltaphi = Math.asin(this.doca/r); - this.line3D_plus = new Line3D(); - this.line3D_plus.copy(line); - this.line3D_plus.rotateZ(deltaphi); - - this.line3D_minus = new Line3D(); - this.line3D_minus.copy(line); - this.line3D_minus.rotateZ(deltaphi); - - } - - //hit measurement vector in 1 dimension: minimize distance - doca - public RealVector get_Vector() { - return new ArrayRealVector(new double[]{this.doca}); - } - - //hit measurement vector in 1 dimension with sign: if sign = 0, return doca, otherwise return 0 - public RealVector get_Vector(int sign, boolean goodsign) { - if(sign == 0 || goodsign){ - return new ArrayRealVector(new double[]{this.doca}); - }else{ - return new ArrayRealVector(new double[]{0.0}); - } - } - - public RealMatrix get_MeasurementNoise() { - return new Array2DRowRealMatrix(new double[][]{{0.0225}}); - } - - public RealMatrix get_MeasurementNoise(boolean goodsign) { - if(goodsign){ - return new Array2DRowRealMatrix(new double[][]{{0.0225}}); - }else{ - return new Array2DRowRealMatrix(new double[][]{{2*this.doca*this.doca}}); - } - } - - public double doca() { - return doca; - } - - public double r() {return r;} - - public double phi() {return phi;}//at z = 0; - - public double phi(double z) { - return this.line3D.lerpPoint((z-line3D.origin().z())/line3D.length()).vectorFrom(0,0,0).phi(); - } - - public Line3D line() {return line3D;} - - public double distance(Point3D point3D) { - return this.line3D.distance(point3D).length(); - } - - public double distance(Point3D point3D, int sign, boolean goodsign) { - //if(sign!=0) - //System.out.println(" r " + this.r + " phi " + this.phi + " doca " + this.doca + " sign " + sign + " distance " + this.line3D.distance(point3D).length() + " (sign 0) " + this.line3D_plus.distance(point3D).length() + " (sign+) " + this.line3D_minus.distance(point3D).length() + " (sign-) "); - if(!goodsign){ - if(sign>0)return this.line3D_plus.distance(point3D).length(); - if(sign<0)return this.line3D_minus.distance(point3D).length(); - } - return this.line3D.distance(point3D).length(); - } - - @Override - public int compareTo(Hit o) { - System.out.println("r = " + r + " other r = " + o.r); - return Double.compare(r, o.r); - } - - @Override - public String toString() { - return "Hit{" + "superLayer=" + superLayer + ", layer=" + layer + ", wire=" + wire + ", r=" + r + ", doca=" + doca + '}'; - } - - public RealVector get_Vector_beam() { - return null; - } - - public int getSuperLayer() { - return superLayer; - } - - public int getLayer() { - return layer; - } - - public int getWire() { - return wire; - } - - public double getR() { - return r; - } - - public double getDoca() { - return doca; - } - - public double getADC() { - return adc; - } - - public void setADC(double _adc) { - this.adc = _adc; - } - - public Line3D getLine3D() { - return line3D; - } - - public double getNumWires() { - return numWires; - } - - public int getHitIdx() { - return hitidx; - } - - public void setHitIdx(int idx) { - this.hitidx = idx; - } - - public int getSign() { - return hitsign; - } - - public void setSign(int sign) { - this.hitsign = sign; - } - -} - diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Hit_beam.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Hit_beam.java index a578f7da99..fd25c2e94d 100644 --- a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Hit_beam.java +++ b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Hit_beam.java @@ -3,22 +3,50 @@ import org.apache.commons.math3.linear.ArrayRealVector; import org.apache.commons.math3.linear.RealVector; import org.jlab.geom.prim.Line3D; +import org.jlab.geom.prim.Point3D; +import org.jlab.rec.ahdc.Hit.Hit; +/** + * A weird object that just want to be considered as a Hit + * The methods that matters are : distance(), getLine(), get_Vector_beam(), getRadius() + * + * @author Mathieu Ouillon + * @author Éric Fuchey + * @author Felix Touchte Codjo + */ public class Hit_beam extends Hit { - double x,y,z; - double r,phi; + private double x,y,z; + private double r,phi; + Line3D beamline; - public Hit_beam(int superLayer, int layer, int wire, int numWire, double doca, double x, double y , double z) { - super(0, 0, 0, 0, new Line3D(x,y,0,x,y,1), 0); + public Hit_beam(double x, double y , double z) { + super(0,0,0,0, 0, 0, -1); this.x = x; this.y = y; this.z = z; this.r = Math.hypot(x,y); this.phi = Math.atan2(y,x); + beamline = new Line3D(x,y,0,x,y,1); // a line parallel to the beam axis } + @Override public RealVector get_Vector_beam() { return new ArrayRealVector(new double[] {this.r, this.phi, this.z}); } + + @Override + public Line3D getLine() { + return beamline; + } + + @Override + public double distance(Point3D point3D) { + return this.beamline.distance(point3D).length(); + } + + @Override + public double getRadius() { + return r; + } } diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Indicator.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Indicator.java deleted file mode 100644 index 58e23ec9ba..0000000000 --- a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Indicator.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.jlab.rec.ahdc.KalmanFilter; - -import org.jlab.clas.tracking.kalmanfilter.Material; - -public class Indicator { - - public double R; - public double h; - public Hit hit; - public boolean direction; - public Material material; - - public Indicator(double r, double h, Hit hit, boolean direction, Material material) { - this.R = r; - this.h = h; - this.hit = hit; - this.direction = direction; - this.material = material; - } - - public boolean haveAHit() { - boolean res = this.hit != null; - if (this.R == 0 && !direction) res = true; - return res; - } -} diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/KFitter.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/KFitter.java index 1a184bf5dc..3f19dc0305 100644 --- a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/KFitter.java +++ b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/KFitter.java @@ -1,41 +1,55 @@ package org.jlab.rec.ahdc.KalmanFilter; +import java.util.HashMap; + import org.apache.commons.math3.linear.Array2DRowRealMatrix; import org.apache.commons.math3.linear.MatrixUtils; import org.apache.commons.math3.linear.RealMatrix; import org.apache.commons.math3.linear.RealVector; import org.jlab.clas.pdg.PhysicsConstants; +import org.jlab.clas.tracking.kalmanfilter.Material; import org.jlab.geom.prim.Point3D; - +import org.jlab.rec.ahdc.Hit.Hit; + +/** + * Implement the prediction and the correction stages of the Kalman Filter + * + * @author Mathieu Ouillon + * @author Éric Fuchey + * @author Felix Touchte Codjo + */ public class KFitter { private RealVector stateEstimation; - private RealMatrix errorCovariance; + private RealMatrix errorCovariance; public final Stepper stepper; private final Propagator propagator; - public double chi2 = 0; - // masses/energies in MeV + private final HashMap materialHashMap; + public double chi2 = 0; + // masses/energies in MeV private final double electron_mass_c2 = PhysicsConstants.massElectron() * 1000; private final double proton_mass_c2 = PhysicsConstants.massProton() * 1000; private boolean isvertexdefined = false; - public KFitter(final RealVector initialStateEstimate, final RealMatrix initialErrorCovariance, final Stepper stepper, final Propagator propagator) { + public KFitter(final RealVector initialStateEstimate, final RealMatrix initialErrorCovariance, final Stepper stepper, final Propagator propagator, final HashMap materialHashMap) { this.stateEstimation = initialStateEstimate; this.errorCovariance = initialErrorCovariance; this.stepper = stepper; this.propagator = propagator; + this.materialHashMap = materialHashMap; } - public void predict(Indicator indicator) throws Exception { + public void predict(Hit hit, boolean direction) throws Exception { // Initialization - stepper.initialize(indicator); + stepper.initialize(direction); Stepper stepper1 = new Stepper(stepper.y); + stepper1.initialize(direction); // project the state estimation ahead (a priori state) : xHat(k)- = f(xHat(k-1)) - stateEstimation = propagator.f(stepper, indicator); + stateEstimation = propagator.f(stepper, hit, materialHashMap); // project the covariance matrix ahead - RealMatrix transitionMatrix = F(indicator, stepper1); + RealMatrix transitionMatrix = F(hit, stepper1); RealMatrix transitionMatrixT = transitionMatrix.transpose(); double px = Math.abs(stepper.y[3]); @@ -58,7 +72,7 @@ public void predict(Indicator indicator) throws Exception { double dE = Math.abs(stepper.dEdx); double K = 0.000307075; - double sigma2_dE = indicator.material.getDensity() * K * indicator.material.getZoverA() / beta2 * tmax * s / 10 * (1.0 - beta2 / 2) * 1000 * 1000;//in MeV^2 + double sigma2_dE = stepper.material.getDensity() * K * stepper.material.getZoverA() / beta2 * tmax * s / 10 * (1.0 - beta2 / 2) * 1000 * 1000;//in MeV^2 double dp_prim_ddE = (E + dE) / Math.sqrt((E + dE) * (E + dE) - mass * mass); double sigma2_px = Math.pow(px / p, 2) * Math.pow(dp_prim_ddE, 2) * sigma2_dE; double sigma2_py = Math.pow(py / p, 2) * Math.pow(dp_prim_ddE, 2) * sigma2_dE; @@ -72,13 +86,14 @@ public void predict(Indicator indicator) throws Exception { errorCovariance = (transitionMatrix.multiply(errorCovariance.multiply(transitionMatrixT))).add(processNoise); } - public void correct(Indicator indicator) { - RealVector z, z_plus, z_minus; + public void correct(Hit hit) { + RealVector z; RealMatrix measurementNoise; RealMatrix measurementMatrix; RealVector h; - if (indicator.R == 0.0 && !indicator.direction) { - double z_beam_res_sq = 1.e10;//in mm + // check if the hit is the beamline + if (hit.getRadius() < 1) { + double z_beam_res_sq = 1.e10;//in mm if(isvertexdefined)z_beam_res_sq = 4.0;//assuming 2. mm resolution measurementNoise = new Array2DRowRealMatrix( @@ -89,23 +104,14 @@ public void correct(Indicator indicator) { });//3x3 measurementMatrix = H_beam(stateEstimation);//6x3 h = h_beam(stateEstimation);//3x1 - z = indicator.hit.get_Vector_beam();//0! - } else { - //System.out.println(" hit r " + indicator.hit.r() + " hit phi " + indicator.hit.phi() + " phi wire (-zl/2) " + indicator.hit.phi(-150.0) + " phi wire (0) " + indicator.hit.phi(0.0) + " phi wire (+zl/2) " + indicator.hit.phi(150.) + " state x " + stateEstimation.getEntry(0) + " state y " + stateEstimation.getEntry(1) + " state z " + stateEstimation.getEntry(2) ); - boolean goodsign = true; - if(indicator.hit.getSign()!=0){ - double dphi = Math.atan2(stateEstimation.getEntry(1), stateEstimation.getEntry(0))-indicator.hit.phi(stateEstimation.getEntry(2)); - if(dphi*indicator.hit.getSign()<0)goodsign = false; - //System.out.println(" hit r " + indicator.hit.r() + " phi wire (z) " + indicator.hit.phi(stateEstimation.getEntry(2)) + " phi state " + Math.atan2(stateEstimation.getEntry(1), stateEstimation.getEntry(0)) + " sign " + indicator.hit.getSign() + " good? " + goodsign ); - } - //measurementNoise = indicator.hit.get_MeasurementNoise();//1x1 - measurementNoise = indicator.hit.get_MeasurementNoise(goodsign);//1x1 - measurementMatrix = H(stateEstimation, indicator);//6x1 - //measurementMatrix = H(stateEstimation, indicator, goodsign);//6x1 - h = h(stateEstimation, indicator);//1x1 - //h = h(stateEstimation, indicator, goodsign);//1x1 - z = indicator.hit.get_Vector();//1x1 - //z = indicator.hit.get_Vector(indicator.hit.getSign(), goodsign);//1x1 + z = hit.get_Vector_beam();//0! + } + // else, it is an AHDC hits + else { + measurementNoise = hit.get_MeasurementNoise();//1x1 + measurementMatrix = H(stateEstimation, hit);//6x1 + h = h(stateEstimation, hit);//1x1 + z = hit.get_Vector();//1x1 } RealMatrix measurementMatrixT = measurementMatrix.transpose(); @@ -123,6 +129,7 @@ public void correct(Indicator indicator) { // update estimate with measurement z(k) xHat(k) = xHat(k)- + K * Inn stateEstimation = stateEstimation.add(kalmanGain.operate(innovation)); // update covariance of prediction error P(k) = (I - K * H) * P(k)- + // The Joseph's form is numerically more stable P(k) = (I - K * H) * P(k)- (I - K * H)' - H*R*K' RealMatrix identity = MatrixUtils.createRealIdentityMatrix(kalmanGain.getRowDimension()); // Numerically more stable !! RealMatrix tmpMatrix = identity.subtract(kalmanGain.multiply(measurementMatrix)); @@ -132,52 +139,41 @@ public void correct(Indicator indicator) { stepper.y = stateEstimation.toArray(); } - public double residual(Indicator indicator) { - double d = indicator.hit.distance( new Point3D( stateEstimation.getEntry(0), stateEstimation.getEntry(1), stateEstimation.getEntry(2) ) ); - return indicator.hit.doca()-d; - } - - //function for left-right disambiguation - public int wire_sign(Indicator indicator) {//let's decide: positive when (phi state - phi wire) > 0 - double phi_state = Math.atan2(stateEstimation.getEntry(1), stateEstimation.getEntry(0)); - double phi_wire = indicator.hit.phi(stateEstimation.getEntry(2)); - if( (phi_state-phi_wire)/Math.abs(phi_state-phi_wire)>0 ){ - return +1; - }else{ - return -1; - } + public double residual(Hit hit) { + double d = hit.distance( new Point3D( stateEstimation.getEntry(0), stateEstimation.getEntry(1), stateEstimation.getEntry(2) ) ); + return hit.getDoca()-d; } - public void ResetErrorCovariance(final RealMatrix initialErrorCovariance){ - this.errorCovariance = initialErrorCovariance; - } + public void ResetErrorCovariance(final RealMatrix initialErrorCovariance){ + this.errorCovariance = initialErrorCovariance; + } - private RealMatrix F(Indicator indicator, Stepper stepper1) throws Exception { + private RealMatrix F(Hit hit, Stepper stepper1) throws Exception { - double[] dfdx = subfunctionF(indicator, stepper1, 0); - double[] dfdy = subfunctionF(indicator, stepper1, 1); - double[] dfdz = subfunctionF(indicator, stepper1, 2); - double[] dfdpx = subfunctionF(indicator, stepper1, 3); - double[] dfdpy = subfunctionF(indicator, stepper1, 4); - double[] dfdpz = subfunctionF(indicator, stepper1, 5); + double[] dfdx = subfunctionF(hit, stepper1, 0); + double[] dfdy = subfunctionF(hit, stepper1, 1); + double[] dfdz = subfunctionF(hit, stepper1, 2); + double[] dfdpx = subfunctionF(hit, stepper1, 3); + double[] dfdpy = subfunctionF(hit, stepper1, 4); + double[] dfdpz = subfunctionF(hit, stepper1, 5); return MatrixUtils.createRealMatrix(new double[][]{ {dfdx[0], dfdy[0], dfdz[0], dfdpx[0], dfdpy[0], dfdpz[0]}, {dfdx[1], dfdy[1], dfdz[1], dfdpx[1], dfdpy[1], dfdpz[1]}, {dfdx[2], dfdy[2], dfdz[2], dfdpx[2], dfdpy[2], dfdpz[2]}, {dfdx[3], dfdy[3], dfdz[3], dfdpx[3], dfdpy[3], dfdpz[3]}, {dfdx[4], dfdy[4], dfdz[4], dfdpx[4], dfdpy[4], dfdpz[4]}, {dfdx[5], dfdy[5], dfdz[5], dfdpx[5], dfdpy[5], dfdpz[5]}}); } - double[] subfunctionF(Indicator indicator, Stepper stepper1, int i) throws Exception { + double[] subfunctionF(Hit hit, Stepper stepper1, int i) throws Exception { double h = 1e-8;// in mm Stepper stepper_plus = new Stepper(stepper1.y); Stepper stepper_minus = new Stepper(stepper1.y); - stepper_plus.initialize(indicator); - stepper_minus.initialize(indicator); + stepper_plus.initialize(stepper1.direction); + stepper_minus.initialize(stepper1.direction); stepper_plus.y[i] = stepper_plus.y[i] + h; stepper_minus.y[i] = stepper_minus.y[i] - h; - propagator.f(stepper_plus, indicator); - propagator.f(stepper_minus, indicator); + propagator.f(stepper_plus, hit, materialHashMap); + propagator.f(stepper_minus, hit, materialHashMap); double dxdi = (stepper_plus.y[0] - stepper_minus.y[0]) / (2 * h); double dydi = (stepper_plus.y[1] - stepper_minus.y[1]) / (2 * h); @@ -189,34 +185,27 @@ private RealMatrix F(Indicator indicator, Stepper stepper1) throws Exception { return new double[]{dxdi, dydi, dzdi, dpxdi, dpydi, dpzdi}; } - //measurement matrix in 1 dimension: minimize distance - doca - private RealVector h(RealVector x, Indicator indicator) { - double d = indicator.hit.distance(new Point3D(x.getEntry(0), x.getEntry(1), x.getEntry(2))); - //double d = indicator.hit.distance(new Point3D(x.getEntry(0), x.getEntry(1), x.getEntry(2)), indicator.hit.getSign()); - return MatrixUtils.createRealVector(new double[]{d}); - } - - private RealVector h(RealVector x, Indicator indicator, boolean goodsign) { - double d = indicator.hit.distance(new Point3D(x.getEntry(0), x.getEntry(1), x.getEntry(2)), indicator.hit.getSign(), goodsign); + // Measurement matrix in 1x1 dimension: minimize distance - doca + private RealVector h(RealVector x, Hit hit) { + double d = hit.distance(new Point3D(x.getEntry(0), x.getEntry(1), x.getEntry(2))); return MatrixUtils.createRealVector(new double[]{d}); } - //measurement matrix in 1 dimension: minimize distance - doca - private RealMatrix H(RealVector x, Indicator indicator) { + // Jacobian matrix of the measurement with respect to (x, y, z, px, py, pz) + private RealMatrix H(RealVector x, Hit hit) { - double ddocadx = subfunctionH(x, indicator, 0); - double ddocady = subfunctionH(x, indicator, 1); - double ddocadz = subfunctionH(x, indicator, 2); - double ddocadpx = subfunctionH(x, indicator, 3); - double ddocadpy = subfunctionH(x, indicator, 4); - double ddocadpz = subfunctionH(x, indicator, 5); + double ddocadx = subfunctionH(x, hit, 0); + double ddocady = subfunctionH(x, hit, 1); + double ddocadz = subfunctionH(x, hit, 2); + double ddocadpx = subfunctionH(x, hit, 3); + double ddocadpy = subfunctionH(x, hit, 4); + double ddocadpz = subfunctionH(x, hit, 5); - // As per my understanding: ddocadx,y,z -> = dr/dx,y,z, etc return MatrixUtils.createRealMatrix(new double[][]{ {ddocadx, ddocady, ddocadz, ddocadpx, ddocadpy, ddocadpz}}); } - double subfunctionH(RealVector x, Indicator indicator, int i) { + double subfunctionH(RealVector x, Hit hit, int i) { double h = 1e-8;// in mm RealVector x_plus = x.copy(); RealVector x_minus = x.copy(); @@ -224,41 +213,13 @@ private RealMatrix H(RealVector x, Indicator indicator) { x_plus.setEntry(i, x_plus.getEntry(i) + h); x_minus.setEntry(i, x_minus.getEntry(i) - h); - double doca_plus = h(x_plus, indicator).getEntry(0); - double doca_minus = h(x_minus, indicator).getEntry(0); + double doca_plus = h(x_plus, hit).getEntry(0); + double doca_minus = h(x_minus, hit).getEntry(0); return (doca_plus - doca_minus) / (2 * h); } - - //measurement matrix in 1 dimension: minimize distance - doca - private RealMatrix H(RealVector x, Indicator indicator, boolean goodsign) { - - double ddocadx = subfunctionH(x, indicator, 0, goodsign); - double ddocady = subfunctionH(x, indicator, 1, goodsign); - double ddocadz = subfunctionH(x, indicator, 2, goodsign); - double ddocadpx = subfunctionH(x, indicator, 3, goodsign); - double ddocadpy = subfunctionH(x, indicator, 4, goodsign); - double ddocadpz = subfunctionH(x, indicator, 5, goodsign); - - // As per my understanding: ddocadx,y,z -> = dr/dx,y,z, etc - return MatrixUtils.createRealMatrix(new double[][]{ - {ddocadx, ddocady, ddocadz, ddocadpx, ddocadpy, ddocadpz}}); - } - - double subfunctionH(RealVector x, Indicator indicator, int i, boolean goodsign) { - double h = 1e-8;// in mm - RealVector x_plus = x.copy(); - RealVector x_minus = x.copy(); - - x_plus.setEntry(i, x_plus.getEntry(i) + h); - x_minus.setEntry(i, x_minus.getEntry(i) - h); - - double doca_plus = h(x_plus, indicator, goodsign).getEntry(0); - double doca_minus = h(x_minus, indicator, goodsign).getEntry(0); - - return (doca_plus - doca_minus) / (2 * h); - } - + + // Measurement matrix for the beamline (Hit_beam) in dimeansion 3x1 private RealVector h_beam(RealVector x) { double xx = x.getEntry(0); @@ -270,7 +231,8 @@ private RealVector h_beam(RealVector x) { return MatrixUtils.createRealVector(new double[]{r, phi, zz}); } - + + // Jacobian matrix of the measurement for the beamline with respect to (x, y, z, px, py, pz) private RealMatrix H_beam(RealVector x) { double xx = x.getEntry(0); @@ -305,23 +267,12 @@ private RealMatrix H_beam(RealVector x) { }); } - - private RealVector innovation(RealVector z, Indicator indicator) { - - return z.subtract(h(stateEstimation, indicator)); - } - - /** - * Returns a copy of the current state estimation vector. - * - * @return the state estimation vector - */ public RealVector getStateEstimationVector() { return stateEstimation.copy(); } - public double getMomentum() { - return Math.sqrt(stateEstimation.getEntry(3) * stateEstimation.getEntry(3) + stateEstimation.getEntry(4) * stateEstimation.getEntry(4) + stateEstimation.getEntry(5) * stateEstimation.getEntry(5)); + public RealMatrix getErrorCovarianceMatrix() { + return errorCovariance.copy(); } public void setVertexDefined(boolean isvtxdef) {isvertexdefined = isvtxdef;} diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/KalmanFilter.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/KalmanFilter.java index 7c53a3397b..805a736558 100644 --- a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/KalmanFilter.java +++ b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/KalmanFilter.java @@ -1,5 +1,9 @@ package org.jlab.rec.ahdc.KalmanFilter; +import java.util.ArrayList; +import java.util.Collections; +import java.util.HashMap; + import org.apache.commons.math3.linear.ArrayRealVector; import org.apache.commons.math3.linear.MatrixUtils; import org.apache.commons.math3.linear.RealMatrix; @@ -7,314 +11,145 @@ import org.jlab.clas.pdg.PDGDatabase; import org.jlab.clas.pdg.PDGParticle; import org.jlab.clas.tracking.kalmanfilter.Material; -import org.jlab.clas.tracking.kalmanfilter.Units; -import org.jlab.geom.prim.Point3D; -import org.jlab.io.base.DataBank; import org.jlab.io.base.DataEvent; +import org.jlab.rec.ahdc.Hit.Hit; import org.jlab.rec.ahdc.Track.Track; -import java.util.ArrayList; -import java.util.HashMap; +//import org.apache.commons.math3.linear.RealMatrixFormat; /** - * TODO : - Fix multi hit on the same layer - * - Optimize measurement noise and probably use doca as weight - * - Fix the wire number (-1) - * - Iterate thought multiple tracks per event - * - use a flag for simulation - * - add target in the map - * - move map to initialization engine - * - flag for target material - * - error px0 use MC !! Bad !! FIX IT FAST + * This is the main routine of the Kalman Filter. The fit is done by a KFitter + * + * masses/energies should be in MeV; distances should be in mm + * + * @author Mathieu Ouillon + * @author Éric Fuchey + * @author Felix Touchte Codjo */ -// masses/energies should be in MeV; distances should be in mm - public class KalmanFilter { public KalmanFilter(ArrayList tracks, DataEvent event, final double magfield, boolean IsMC) {propagation(tracks, event, magfield, IsMC);} - private final int Niter = 10; - private final boolean IsVtxDefined = false; + private final int Niter = 40; // number of iterations for the Kalman Filter + private boolean IsVtxDefined = false; // implemented but not used yet private void propagation(ArrayList tracks, DataEvent event, final double magfield, boolean IsMC) { try { - double vz_constraint = 0; - if(IsMC) {//If simulation read MC::Particle Bank ------------------------------------------------ - DataBank bankParticle = event.getBank("MC::Particle"); - double vxmc = bankParticle.getFloat("vx", 0)*10;//mm - double vymc = bankParticle.getFloat("vy", 0)*10;//mm - double vzmc = bankParticle.getFloat("vz", 0)*10;//mm - double pxmc = bankParticle.getFloat("px", 0)*1000;//MeV - double pymc = bankParticle.getFloat("py", 0)*1000;//MeV - double pzmc = bankParticle.getFloat("pz", 0)*1000;//MeV - double p_mc = java.lang.Math.sqrt(pxmc*pxmc+pymc*pymc+pzmc*pzmc); - //System.out.println("MC track: vz: " + vzmc*10 + " px: " + pxmc*1000 + " py: " + pymc*1000 + " pz: " + pzmc*1000 + "; p = " + p_mc*1000);//convert p to MeV, v to mm - - ArrayList sim_hits = new ArrayList<>(); - sim_hits.add(new Point3D(0, 0, vzmc)); - - DataBank bankMC = event.getBank("MC::True"); - for (int i = 0; i < bankMC.rows(); i++) { - if (bankMC.getInt("pid", i) == 2212) { - float x = bankMC.getFloat("avgX", i); - float y = bankMC.getFloat("avgY", i); - float z = bankMC.getFloat("avgZ", i); - // System.out.println("r_sim = " + Math.hypot(x, y)); - sim_hits.add(new Point3D(x, y, z)); - } - } - vz_constraint = vzmc; - } + double vz_constraint = 0; // to be linked to the electron vertex // Initialization --------------------------------------------------------------------- final PDGParticle proton = PDGDatabase.getParticleById(2212); final int numberOfVariables = 6; final double tesla = 0.001; final double[] B = {0.0, 0.0, magfield / 10 * tesla}; - - // Initialization material map - HashMap materialHashMap = materialGeneration(); + HashMap materialHashMap = MaterialMap.generateMaterials(); + + // Loop over tracks int trackId = 0; for (Track track : tracks) { trackId++; track.set_trackId(trackId); - // Initialization State Vector - final double x0 = 0.0; - final double y0 = 0.0; - final double z0 = track.get_Z0(); - //final + // Initialize state vector + double x0 = 0.0; + double y0 = 0.0; + double z0 = track.get_Z0(); double px0 = track.get_px(); - //final double py0 = track.get_py(); - final double pz0 = track.get_pz(); - //final double p_init = java.lang.Math.sqrt(px0*px0+py0*py0+pz0*pz0); + double pz0 = track.get_pz(); + double[] y = new double[]{x0, y0, z0, px0, py0, pz0}; - // Initialization hit - ArrayList AHDC_hits = track.getHits(); - ArrayList KF_hits = new ArrayList<>(); - double ADCTot = 0; - //System.out.println(" px " + y[3] + " py " + y[4] +" pz " + y[5] +" vz " + y[2] + " number of hits: " + AHDC_hits.size() + " MC hits? " + sim_hits.size()); - track.set_n_hits(AHDC_hits.size()); - for (org.jlab.rec.ahdc.Hit.Hit AHDC_hit : AHDC_hits) { - Hit hit = new Hit(AHDC_hit.getSuperLayerId(), AHDC_hit.getLayerId(), AHDC_hit.getWireId(), AHDC_hit.getNbOfWires(), AHDC_hit.getLine(), AHDC_hit.getDoca()); - hit.setADC(AHDC_hit.getADC()); - hit.setHitIdx(AHDC_hit.getId()); - hit.setSign(0); - ADCTot+=AHDC_hit.getADC(); - // set track id - AHDC_hit.setTrackId(trackId); - - //System.out.println( " r = " + hit.r() + " hit.phi " + hit.phi() +" hit.doca = " + hit.getDoca() ); - // Do delete hit with same radius - boolean phi_rollover = false; - boolean aleardyHaveR = false; - for (Hit o: KF_hits){ - if (o.r() == hit.r()){ - aleardyHaveR = true; - // //sign+ means (phi track - phi wire) > 0 - // if(o.phi()>hit.phi()){ - // if(Math.abs(o.phi()-hit.phi())< 2*Math.toRadians(360./o.getNumWires()) ){ - // o.setSign(-1); - // hit.setSign(+1); - // }else{ - // phi_rollover = true; - // hit.setSign(-1); - // o.setSign(+1); - // } - // }else{ - // if(Math.abs(o.phi()-hit.phi())< 2*Math.toRadians(360./o.getNumWires()) ){ - // hit.setSign(-1); - // o.setSign(+1); - // }else{ - // phi_rollover = true; - // o.setSign(-1); - // hit.setSign(+1); - // } - // } - // //System.out.println( " r = " + o.r() + " o.phi = " + o.phi() + " o.doca = " + o.getDoca()*o.getSign() + " hit.phi " + hit.phi() +" hit.doca = " + hit.getDoca()*hit.getSign() + " angle between wires: " + Math.toRadians(360./hit.getNumWires()) + " >= ? angle covered by docas: " + Math.atan( (o.getDoca()+hit.getDoca())/o.r() ) ); - } - } - if(!aleardyHaveR)KF_hits.add(hit); - // if (phi_rollover){ - // KF_hits.add(KF_hits.size()-1, hit); - // }else{ - // KF_hits.add(hit); - // } - } + // Read list of hits + ArrayList AHDC_hits = track.getHits(); + Collections.sort(AHDC_hits); // sorted following the compareTo() method in Hit.java double zbeam = 0; - if(IsVtxDefined)zbeam = vz_constraint;//test - final ArrayList forwardIndicators = forwardIndicators(KF_hits, materialHashMap); - final ArrayList backwardIndicators = backwardIndicators(KF_hits, materialHashMap, zbeam); + if(IsVtxDefined)zbeam = vz_constraint; // Start propagation Stepper stepper = new Stepper(y); RungeKutta4 RK4 = new RungeKutta4(proton, numberOfVariables, B); Propagator propagator = new Propagator(RK4); - // ---------------------------------------------------------------------------------------- - // Initialization of the Kalman Fitter + // for the error matrix: first 3 lines in mm^2; last 3 lines in MeV^2 RealVector initialStateEstimate = new ArrayRealVector(stepper.y); - //first 3 lines in cm^2; last 3 lines in MeV^2 - RealMatrix initialErrorCovariance = MatrixUtils.createRealMatrix(new double[][]{{1.00, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 1.00, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 25.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 1.00, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 1.00, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 25.0}}); - KFitter TrackFitter = new KFitter(initialStateEstimate, initialErrorCovariance, stepper, propagator); + RealMatrix initialErrorCovariance = MatrixUtils.createRealMatrix(new double[][]{{50.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 50.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 900.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 100.00, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 100.00, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 900.0}}); + KFitter TrackFitter = new KFitter(initialStateEstimate, initialErrorCovariance, stepper, propagator, materialHashMap); TrackFitter.setVertexDefined(IsVtxDefined); - + + // Loop over number of iterations for (int k = 0; k < Niter; k++) { - //System.out.println("--------- ForWard propagation !! ---------"); - //Reset error covariance: - //TrackFitter.ResetErrorCovariance(initialErrorCovariance); - for (Indicator indicator : forwardIndicators) { - TrackFitter.predict(indicator); - if (indicator.haveAHit()) { - if( k==0 && indicator.hit.getHitIdx()>0){ - for (org.jlab.rec.ahdc.Hit.Hit AHDC_hit : AHDC_hits){ - if(AHDC_hit.getId()==indicator.hit.getHitIdx())AHDC_hit.setResidualPrefit(TrackFitter.residual(indicator)); - } + // Forward propagation + for (Hit hit : AHDC_hits) { + TrackFitter.predict(hit, true); + TrackFitter.correct(hit); + + } + // Backward propagation (last layer to first layer) + for (int i = AHDC_hits.size() - 2; i >= 0; i--) { + Hit hit = AHDC_hits.get(i); + TrackFitter.predict(hit, false); + TrackFitter.correct(hit); + } + // Backward propagation (first layer to beamline) + { + Hit hit = new Hit_beam(0, 0, zbeam); + TrackFitter.predict(hit, false); + TrackFitter.correct(hit); } - TrackFitter.correct(indicator); - } - } - //System.out.println("--------- BackWard propagation !! ---------"); - for (Indicator indicator : backwardIndicators) { - TrackFitter.predict(indicator); - if (indicator.haveAHit()) { - TrackFitter.correct(indicator); - } - } } + /*RealMatrixFormat format = + new RealMatrixFormat( + "[\n", "\n]", // matrix start/end + "[", "]", // row start/end + ",\n", // column separator + " ; ", // row separator + new java.text.DecimalFormat("0.0000") + ); + System.out.println("=====> Print error matrix"); + System.out.println(format.format(TrackFitter.getErrorCovarianceMatrix()));*/ + RealVector x_out = TrackFitter.getStateEstimationVector(); track.setPositionAndMomentumForKF(x_out); - - //Residual, path and AHDC exit momentum calculation post fit: - KFitter PostFitPropagator = new KFitter(TrackFitter.getStateEstimationVector(), initialErrorCovariance, new Stepper(TrackFitter.getStateEstimationVector().toArray()), new Propagator(RK4)); - for (Indicator indicator : forwardIndicators) { - PostFitPropagator.predict(indicator); - if (indicator.haveAHit()) { - if( indicator.hit.getHitIdx()>0){ - for (org.jlab.rec.ahdc.Hit.Hit AHDC_hit : AHDC_hits){ - if(AHDC_hit.getId()==indicator.hit.getHitIdx())AHDC_hit.setResidual(PostFitPropagator.residual(indicator)); + + // Post fit propagation (no correction) to set the residuals + KFitter PostFitPropagator = new KFitter(TrackFitter.getStateEstimationVector(), initialErrorCovariance, new Stepper(TrackFitter.getStateEstimationVector().toArray()), new Propagator(RK4), materialHashMap); + for (Hit hit : AHDC_hits) { + PostFitPropagator.predict(hit, true); + if( hit.getId()>0){ // for the beamline the hit id is 0, so we only look at AHDC hits + hit.setResidual(PostFitPropagator.residual(hit)); } - } - } } + // Fill track and hit bank + // TO DO : s and p_drift have to be checked to be sure they represent what we want double s = PostFitPropagator.stepper.sTot; - double dEdx = ADCTot / s; double p_drift = PostFitPropagator.stepper.p(); - - // At this stage, all relevants AHDC hits are filled - // Compute sum_adc, sum_residuals and chi2 int sum_adc = 0; double sum_residuals = 0; double chi2 = 0; - for (org.jlab.rec.ahdc.Hit.Hit AHDC_hit : AHDC_hits) { - sum_adc += AHDC_hit.getADC(); - sum_residuals += AHDC_hit.getResidual(); - chi2 += Math.pow(AHDC_hit.getResidual(),2.0); + for (Hit hit : AHDC_hits) { + hit.setTrackId(trackId); + sum_adc += hit.getADC(); + sum_residuals += hit.getResidual(); + chi2 += Math.pow(hit.getResidual(),2.0); } track.set_sum_adc(sum_adc); track.set_sum_residuals(sum_residuals); track.set_chi2(chi2); track.set_p_drift_kf(p_drift); - track.set_dEdx_kf(dEdx); + track.set_dEdx_kf(sum_adc/s); track.set_path_kf(s); + track.set_n_hits(AHDC_hits.size()); }//end of loop on track candidates } catch (Exception e) { - // e.printStackTrace(); + //e.printStackTrace(); + //System.out.println("======> Kalman Filter Error"); } - - } - private HashMap materialGeneration() { - Units units = Units.CM; - - String name_De = "deuteriumGas"; - double thickness_De = 1; - double density_De = 9.37E-4;// 5.5 atm - double ZoverA_De = 0.496499; - double X0_De = 1.3445E+5; // I guess X0 is not even used??? - double IeV_De = 19.2; - - org.jlab.clas.tracking.kalmanfilter.Material deuteriumGas = new org.jlab.clas.tracking.kalmanfilter.Material(name_De, thickness_De, density_De, ZoverA_De, X0_De, IeV_De, units); - - String name_Bo = "BONuS12Gas";//80% He, 20% CO2 - double thickness_Bo = 1; - double density_Bo = 1.39735E-3; - double ZoverA_Bo = 0.49983; - double X0_Bo = 3.69401E+4; - double IeV_Bo = 73.5338; - - org.jlab.clas.tracking.kalmanfilter.Material BONuS12 = new org.jlab.clas.tracking.kalmanfilter.Material(name_Bo, thickness_Bo, density_Bo, ZoverA_Bo, X0_Bo, IeV_Bo, units); - - String name_My = "Mylar"; - double thickness_My = 1; - double density_My = 1.4; - double ZoverA_My = 0.52037; - double X0_My = 28.54; - double IeV_My = 78.7; - - org.jlab.clas.tracking.kalmanfilter.Material Mylar = new org.jlab.clas.tracking.kalmanfilter.Material(name_My, thickness_My, density_My, ZoverA_My, X0_My, IeV_My, units); - - String name_Ka = "Kapton"; - double thickness_Ka = 1; - double density_Ka = 1.42; - double ZoverA_Ka = 0.51264; - double X0_Ka = 28.57; - double IeV_Ka = 79.6; - - org.jlab.clas.tracking.kalmanfilter.Material Kapton = new org.jlab.clas.tracking.kalmanfilter.Material(name_Ka, thickness_Ka, density_Ka, ZoverA_Ka, X0_Ka, IeV_Ka, units); - - return new HashMap() { - { - put("deuteriumGas", deuteriumGas); - put("Kapton", Kapton); - put("Mylar", Mylar); - put("BONuS12Gas", BONuS12); - } - }; - } - - ArrayList forwardIndicators(ArrayList hitArrayList, HashMap materialHashMap) { - ArrayList forwardIndicators = new ArrayList<>(); - //R, h, defined in mm! - forwardIndicators.add(new Indicator(3.0, 0.2, null, true, materialHashMap.get("deuteriumGas"))); - forwardIndicators.add(new Indicator(3.060, 0.001, null, true, materialHashMap.get("Kapton"))); - for (Hit hit : hitArrayList) { - forwardIndicators.add(new Indicator(hit.r(), 0.1, hit, true, materialHashMap.get("BONuS12Gas"))); - } - return forwardIndicators; - } - - ArrayList backwardIndicators(ArrayList hitArrayList, HashMap materialHashMap) { - ArrayList backwardIndicators = new ArrayList<>(); - //R, h, defined in mm! - for (int i = hitArrayList.size() - 2; i >= 0; i--) { - backwardIndicators.add(new Indicator(hitArrayList.get(i).r(), 0.1, hitArrayList.get(i), false, materialHashMap.get("BONuS12Gas"))); - } - backwardIndicators.add(new Indicator(3.060, 1, null, false, materialHashMap.get("BONuS12Gas"))); - backwardIndicators.add(new Indicator(3.0, 0.001, null, false, materialHashMap.get("Kapton"))); - Hit hit = new Hit_beam(0, 0, 0, 0, 0, 0, 0, 0); - backwardIndicators.add(new Indicator(0.0, 0.2, hit, false, materialHashMap.get("deuteriumGas"))); - return backwardIndicators; - } - - ArrayList backwardIndicators(ArrayList hitArrayList, HashMap materialHashMap, double vz) { - ArrayList backwardIndicators = new ArrayList<>(); - //R, h, defined in mm! - for (int i = hitArrayList.size() - 2; i >= 0; i--) { - backwardIndicators.add(new Indicator(hitArrayList.get(i).r(), 0.1, hitArrayList.get(i), false, materialHashMap.get("BONuS12Gas"))); - } - backwardIndicators.add(new Indicator(3.060, 1, null, false, materialHashMap.get("BONuS12Gas"))); - backwardIndicators.add(new Indicator(3.0, 0.001, null, false, materialHashMap.get("Kapton"))); - Hit hit = new Hit_beam(0, 0, 0, 0, 0, 0, 0, vz); - backwardIndicators.add(new Indicator(0.0, 0.2, hit, false, materialHashMap.get("deuteriumGas"))); - return backwardIndicators; - } } diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/MaterialMap.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/MaterialMap.java index cfc0ff6495..8dfd6eb1fb 100644 --- a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/MaterialMap.java +++ b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/MaterialMap.java @@ -5,6 +5,13 @@ import java.util.HashMap; +/** + * TO DO: use/rename the relevant materials for ALERT + * + * @author Mathieu Ouillon + * @author Éric Fuchey + * @author Felix Touchte Codjo + */ public class MaterialMap { public static HashMap generateMaterials() { diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Propagator.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Propagator.java index 8064c250d7..e85021addb 100644 --- a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Propagator.java +++ b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Propagator.java @@ -1,16 +1,23 @@ package org.jlab.rec.ahdc.KalmanFilter; +//import java.util.Arrays; + import org.apache.commons.math3.linear.ArrayRealVector; import org.apache.commons.math3.linear.RealVector; import org.jlab.geom.prim.Point3D; - -import java.io.FileWriter; -import java.io.IOException; -import java.io.Writer; -import java.util.Arrays; - -// All distances here should be in mm. -// Do all those hardcoded values even make sense??? +import org.jlab.clas.tracking.kalmanfilter.Material; +import java.util.HashMap; +import org.jlab.rec.ahdc.Hit.Hit; + +/** + * Is responsible of the propagation + * + * All distances here should be in mm. + * + * @author Mathieu Ouillon + * @author Éric Fuchey + * @author Felix Touchte Codjo + */ public class Propagator { private final RungeKutta4 RK4; @@ -18,138 +25,145 @@ public class Propagator { public Propagator(RungeKutta4 rungeKutta4) { this.RK4 = rungeKutta4; } - - void propagate(Stepper stepper, Indicator indicator) { - // ------------------------------------------------------------ + + // Propagate the stepper toward the next hit + void propagate(Stepper stepper, Hit hit, HashMap materialHashMap) { + + // Do not allow more than 10000 steps (very critical cases) final int maxNbOfStep = 10000; - final double R = indicator.R; + + // Initialise steppers to save the two previous states + Stepper prev_stepper = new Stepper(stepper); + Stepper prev_prev_stepper = new Stepper(stepper); + // Initialize distances, d should always disminish double dMin = Double.MAX_VALUE; - double d = 0; - - // System.out.println("R = " + R); - // stepper.print(); + double prev_dMin = Double.MAX_VALUE; + double prev_prev_dMin = Double.MAX_VALUE; + double d; + + // Intialize radius + double R = stepper.r(); + double prev_R = R; + + // Initialize the stepper size + stepper.h = 0.5; + + // Initialize material + // R = 3 mm is the location of the target + if (R < 3) { + stepper.material = materialHashMap.get("deuteriumGas"); + } else { + stepper.material = materialHashMap.get("BONuS12Gas"); + } - for (int nbStep = 0; nbStep < maxNbOfStep; nbStep++) { - double previous_r = stepper.r(); + // boolean : check if we reach or not the target + boolean target_reached = false; + boolean target_crossed = false; + double thickness = 0; + + // Do the propagation + int nbStep = 0; + while (nbStep < maxNbOfStep) { + nbStep++; + // Save previous states + prev_prev_stepper.copyContent(prev_stepper); + prev_stepper.copyContent(stepper); + prev_R = stepper.r(); + // Take a step RK4.doOneStep(stepper); - double r = stepper.r(); - // stepper.print(); - - - if (stepper.direction) { - if (r >= R - 2 - 1.5 * stepper.h) stepper.h = 1e-2; - if (indicator.hit != null) { - if (r >= R - 2) { - d = indicator.hit.distance(new Point3D(stepper.y[0], stepper.y[1], stepper.y[2])); - if (d < dMin) dMin = d; - } - if (r >= R + 2 || d > dMin) { - // System.out.println("dMin = " + dMin); - break; - } - } else { - if (r >= R) { - break; - } + // compute distance with respect to the hit + // this distance should always disminish + d = hit.distance(new Point3D(stepper.y[0], stepper.y[1], stepper.y[2])); + R = stepper.r(); + // check the evolution + if (d < dMin) { + prev_prev_dMin = prev_dMin; + prev_dMin = dMin; + dMin = d; + } + else { + // check the distance between the steps, they should be very big + if (stepper.distance(prev_stepper) > 0.05) { + // we didn't reach the precision + // so, go back two steps before, reduce the step size + // and redo the propagation + stepper.copyContent(prev_prev_stepper); + dMin = prev_prev_dMin; + stepper.h /= 2; + continue; } - } else { - if (r <= R + 2 + 1.5 * stepper.h) stepper.h = 1e-2; - if (R < 5) { - if (stepper.h < 1e-4) stepper.h = 1e-4; - if ((previous_r - r) < 0) { - break; - } + else { + // the distance between the step is not so big but the distance with respect to the hit starts to increase + // so, go back to the previous step (the best we have) and stop the propagation, set the default step size + stepper.copyContent(prev_stepper); + stepper.h = 0.5; + break; } - if (indicator.hit != null) { - if (r <= R + 2) { - d = indicator.hit.distance(new Point3D(stepper.y[0], stepper.y[1], stepper.y[2])); - if (d < dMin) dMin = d; + } + + // When the propagation is done between the beamline and the first AHDC hit, + // we should take care of the target material + // the target is located at R = 3 mm with a thickness of 0.060 mm (60 µm) + // if R < 5 mm, be careful! + if (R < 5) { + + // We try to cross the target + if (((prev_R < 3 && R > 3) || (prev_R > 3 && R < 3)) && !target_reached) { + // We want to cross the target with a very small step < 0.060 mm + if (Math.abs(R - prev_R) > 0.060) { + // redo the propagation + // so, go back to the previous step and reduce the step size + stepper.copyContent(prev_stepper); + stepper.h /= 5; + dMin = prev_dMin; + continue; } - if (r <= R - 2 || d > dMin) { - // System.out.println("dMin = " + dMin); - break; + // we can consider that we have reached the target and that we are ready to cross it + else { + target_reached = true; + // redo the propagation + // so, go back to the previous step + stepper.copyContent(prev_stepper); + dMin = prev_dMin; + // We are now ready to cross the target + // set the target material + stepper.material = materialHashMap.get("Kapton"); + // update the step size + stepper.h = 0.003; + // initialise the crossed thickness + thickness = 0; + continue; } - } else { - if (r <= R) { - break; + } + + // cross the target for 0.06 mm + if (target_reached && !target_crossed) { + thickness += Math.abs(R - prev_R); + if (thickness > 0.06) { + target_crossed = true; + // reset stepper size + // we go back to a normal propagation + stepper.h = 0.5; + // update stepper material + if (stepper.direction) { // forward propagation + stepper.material = materialHashMap.get("BONuS12Gas"); + } + else { // backward propagation + stepper.material = materialHashMap.get("deuteriumGas"); + } } } - - + } - - } + } - public RealVector f(Stepper stepper, Indicator indicator) { - propagate(stepper, indicator); + public RealVector f(Stepper stepper, Hit hit, HashMap materialHashMap) { + propagate(stepper, hit, materialHashMap); return new ArrayRealVector(stepper.y); } - public void propagateAndWrite(Stepper stepper, Indicator indicator, Writer writer) { - // ------------------------------------------------------------ - final int maxNbOfStep = 10000; - final double R = indicator.R; - - double dMin = Double.MAX_VALUE; - double d = 0; - - System.out.println("R = " + R); - stepper.print(); - try {writer.write("" + Arrays.toString(stepper.y) + '\n');} catch (Exception e) {e.printStackTrace();} - - for (int nbStep = 0; nbStep < maxNbOfStep; nbStep++) { - double previous_r = stepper.r(); - RK4.doOneStep(stepper); - double r = stepper.r(); - stepper.print(); - try {writer.write("" + Arrays.toString(stepper.y) + '\n');} catch (Exception e) {e.printStackTrace();} - - if (stepper.direction) { - if (r >= R - 2 - 1.5 * stepper.h) stepper.h = 1e-2; - if (indicator.hit != null) { - if (r >= R - 2) { - d = indicator.hit.distance(new Point3D(stepper.y[0], stepper.y[1], stepper.y[2])); - System.out.println("d = " + d); - if (d < dMin) dMin = d; - } - if (r >= R + 2 || d > dMin) { - System.out.println("dMin = " + dMin); - break; - } - } else { - if (r >= R) { - break; - } - } - } else { - if (r <= R + 2 + 1.5 * stepper.h) stepper.h = 1e-2; - if (R < 5) { - if (stepper.h < 1e-4) stepper.h = 1e-4; - if ((previous_r - r) < 0) { - break; - } - } - if (indicator.hit != null) { - if (r <= R + 2) { - d = indicator.hit.distance(new Point3D(stepper.y[0], stepper.y[1], stepper.y[2])); - if (d < dMin) dMin = d; - } - if (r <= R - 2 || d > dMin) { - System.out.println("dMin = " + dMin); - break; - } - } else { - if (r <= R) { - break; - } - } - - - } - } - } } diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/RungeKutta4.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/RungeKutta4.java index 8830b959e0..b192ca978e 100644 --- a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/RungeKutta4.java +++ b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/RungeKutta4.java @@ -3,8 +3,15 @@ import org.jlab.clas.pdg.PDGParticle; import org.jlab.clas.pdg.PhysicsConstants; -import java.util.Arrays; +/** + * Compute the next trajectory point using the equation of motion + * the step size is given by the stepper + * doneOneStep() is the main method, it is used in Propagator.propagate() + * + * @author Mathieu Ouillon + * @author Éric Fuchey + */ public class RungeKutta4 { private final int numberOfVariables; private final PDGParticle particle; @@ -96,7 +103,8 @@ public void doOneStep(Stepper stepper) { } private double[] f(double[] y) { - double charge = 1.0; + double charge = 1.0; // the charge should by given by the particle object + // for now, we assume it is a proton double pModuleInverse = 1.0 / Math.sqrt(y[3] * y[3] + y[4] * y[4] + y[5] * y[5]); double k = charge * PhysicsConstants.speedOfLight() * 10 * pModuleInverse; diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Stepper.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Stepper.java index cee2861f9c..1d0f6f63d8 100644 --- a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Stepper.java +++ b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Stepper.java @@ -4,35 +4,68 @@ import java.util.Arrays; +/** + * It is a screenshot of the system during the Kalman Filter procedure + * + * @author Mathieu Ouillon + * @author Éric Fuchey + * @author Felix Touchte Codjo + */ public class Stepper { - public double[] y; - public double h; - public Material material; - public double s; - public double sTot = 0; - public double s_drift = 0; - public boolean is_in_drift = false; - public double dEdx; - public boolean direction; + public double[] y; // position and momentum coordinates + public double h; // step size, managed in Propagator + public Material material; // managed in Propagator + public double s; + public double sTot = 0; + public double s_drift = 0; + public boolean is_in_drift = false; + public double dEdx; // s, sTot, s_drift, is_in_drift, dEdx are used in RungeKutta.doOneStep() + public boolean direction; // true (forward propagation), false (backward propagation) public Stepper(double[] y) { this.y = Arrays.copyOf(y, y.length); } - public void initialize(Indicator indicator) { + // Copy constructor + public Stepper(Stepper stepper) { + this.y = Arrays.copyOf(stepper.y, stepper.y.length); + this.h = stepper.h; + this.material = stepper.material; + this.s = stepper.s; + this.sTot = stepper.sTot; + this.s_drift = stepper.s_drift; + this.is_in_drift = stepper.is_in_drift; + this.dEdx = stepper.dEdx; + this.direction = stepper.direction; + } + + // Copy the contents but do not change the reference + public void copyContent(Stepper stepper) { + this.y = Arrays.copyOf(stepper.y, stepper.y.length); + this.h = stepper.h; + this.material = stepper.material; + this.s = stepper.s; + this.sTot = stepper.sTot; + this.s_drift = stepper.s_drift; + this.is_in_drift = stepper.is_in_drift; + this.dEdx = stepper.dEdx; + this.direction = stepper.direction; + } + + // the initialisation is done in the KFitter.predict() + public void initialize(boolean direction) { this.s = 0; this.dEdx = 0; - this.h = indicator.h; - this.material = indicator.material; - this.direction = indicator.direction; + this.direction = direction; this.is_in_drift = false; - } - + + // radius public double r() { return Math.hypot(y[0], y[1]); } - + + // momentum public double p() { return Math.sqrt(y[3] * y[3] + y[4] * y[4] + y[5] * y[5]); } @@ -45,4 +78,13 @@ public void print() { public String toString() { return "" + sTot + ' ' + y[0] + ' ' + y[1] + ' ' + y[2] + ' ' + y[3] + ' ' + y[4] + ' ' + y[5]; } + + // distance between tow steppers + public double distance(Stepper stepper) { + double dx = this.y[0] - stepper.y[0]; + double dy = this.y[1] - stepper.y[1]; + double dz = this.y[2] - stepper.y[2]; + return Math.sqrt(dx*dx + dy*dy + dz*dz); + } + }