diff --git a/etc/bankdefs/hipo4/alert.json b/etc/bankdefs/hipo4/alert.json index 64d4faeec1..99032b8c64 100644 --- a/etc/bankdefs/hipo4/alert.json +++ b/etc/bankdefs/hipo4/alert.json @@ -408,6 +408,118 @@ "name": "sum_residuals", "type": "F", "info": "Sum of residuals (mm)" + }, { + "name": "atof_region", + "type": "I", + "info": "a flag to know if a track reach S1, S2, or S3; is n if Sn is reached; is 0 otherwise" + }, { + "name": "atof_s1_x", + "type": "F", + "info": "x position of the track if it reaches the surface s1" + }, { + "name": "atof_s1_y", + "type": "F", + "info": "y position of the track if it reaches the surface s1" + }, { + "name": "atof_s1_z", + "type": "F", + "info": "z position of the track if it reaches the surface s1" + }, { + "name": "atof_s1_path", + "type": "F", + "info": "total path from the beamline to the surface s1" + }, { + "name": "atof_s1_p", + "type": "F", + "info": "momentum whith which the track reaches the surface s1" + }, { + "name": "atof_s2_x", + "type": "F", + "info": "x position of the track if it reaches the surface s2" + }, { + "name": "atof_s2_y", + "type": "F", + "info": "y position of the track if it reaches the surface s2" + }, { + "name": "atof_s2_z", + "type": "F", + "info": "z position of the track if it reaches the surface s2" + }, { + "name": "atof_s2_path", + "type": "F", + "info": "total path from the beamline to the surface s2" + }, { + "name": "atof_s2_p", + "type": "F", + "info": "momentum whith which the track reaches the surface s2" + }, { + "name": "atof_s3_x", + "type": "F", + "info": "x position of the track if it reaches the surface s3" + }, { + "name": "atof_s3_y", + "type": "F", + "info": "y position of the track if it reaches the surface s3" + }, { + "name": "atof_s3_z", + "type": "F", + "info": "z position of the track if it reaches the surface s3" + }, { + "name": "atof_s3_path", + "type": "F", + "info": "total path from the beamline to the surface s3" + }, { + "name": "atof_s3_p", + "type": "F", + "info": "momentum whith which the track reaches the surface s3" + }, { + "name": "atof_s1_comp", + "type": "I", + "info": "sector*10000 + layer*100 + component : atof component on s1" + }, { + "name": "atof_s2_comp", + "type": "I", + "info": "sector*10000 + layer*100 + component : atof component on s1" + }, { + "name": "atof_s3_comp", + "type": "I", + "info": "sector*10000 + layer*100 + component : atof component on s1" + }, { + "name": "atof_s1_sigma_x", + "type": "F", + "info": "error in atof_s1_x" + }, { + "name": "atof_s1_sigma_y", + "type": "F", + "info": "error in atof_s1_y" + }, { + "name": "atof_s1_sigma_z", + "type": "F", + "info": "error in atof_s1_z" + }, { + "name": "atof_s2_sigma_x", + "type": "F", + "info": "error in atof_s2_x" + }, { + "name": "atof_s2_sigma_y", + "type": "F", + "info": "error in atof_s2_y" + }, { + "name": "atof_s2_sigma_z", + "type": "F", + "info": "error in atof_s2_z" + }, { + "name": "atof_s3_sigma_x", + "type": "F", + "info": "error in atof_s3_x" + }, { + "name": "atof_s3_sigma_y", + "type": "F", + "info": "error in atof_s3_y" + }, { + "name": "atof_s3_sigma_z", + "type": "F", + "info": "error in atof_s3_z" } ] }, { 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 0884ab547f..675ecdb21e 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 @@ -1,5 +1,6 @@ package org.jlab.rec.ahdc.Banks; +import org.apache.commons.math3.linear.RealMatrix; import org.jlab.io.base.DataBank; import org.jlab.io.base.DataEvent; import org.jlab.rec.ahdc.AI.InterCluster; @@ -7,6 +8,7 @@ import org.jlab.rec.ahdc.Cluster.Cluster; import org.jlab.rec.ahdc.DocaCluster.DocaCluster; import org.jlab.rec.ahdc.Hit.Hit; +import org.jlab.rec.ahdc.KalmanFilter.Stepper; import org.jlab.rec.ahdc.PreCluster.PreCluster; import org.jlab.rec.ahdc.Track.Track; @@ -153,6 +155,63 @@ public DataBank fillAHDCKFTrackBank(DataEvent event, ArrayList tracks) { bank.setFloat("chi2", row, (float) track.get_chi2()); bank.setFloat("sum_residuals", row, (float) track.get_sum_residuals()); + // track projection on ATOF surface S1 + Stepper stepper_s1 = track.get_ATOF_S1_stepper(); + if (stepper_s1 != null) { + bank.setFloat("atof_s1_x", row, (float) stepper_s1.y[0]); + bank.setFloat("atof_s1_y", row, (float) stepper_s1.y[1]); + bank.setFloat("atof_s1_z", row, (float) stepper_s1.y[2]); + bank.setFloat("atof_s1_path", row, (float) stepper_s1.sTot); + bank.setFloat("atof_s1_p", row, (float) stepper_s1.p()); + } + + // track projection on ATOF surface S2 + Stepper stepper_s2 = track.get_ATOF_S2_stepper(); + if (stepper_s2 != null) { + bank.setFloat("atof_s2_x", row, (float) stepper_s2.y[0]); + bank.setFloat("atof_s2_y", row, (float) stepper_s2.y[1]); + bank.setFloat("atof_s2_z", row, (float) stepper_s2.y[2]); + bank.setFloat("atof_s2_path", row, (float) stepper_s2.sTot); + bank.setFloat("atof_s2_p", row, (float) stepper_s2.p()); + } + + // track projection on ATOF surface S3 + Stepper stepper_s3 = track.get_ATOF_S3_stepper(); + if (stepper_s3 != null) { + bank.setFloat("atof_s3_x", row, (float) stepper_s3.y[0]); + bank.setFloat("atof_s3_y", row, (float) stepper_s3.y[1]); + bank.setFloat("atof_s3_z", row, (float) stepper_s3.y[2]); + bank.setFloat("atof_s3_path", row, (float) stepper_s3.sTot); + bank.setFloat("atof_s3_p", row, (float) stepper_s3.p()); + } + + bank.setInt("atof_region", row, track.get_ATOF_region()); + + // tmp + bank.setInt("atof_s1_comp", row, track.get_ATOF_S1_component()); + bank.setInt("atof_s2_comp", row, track.get_ATOF_S2_component()); + bank.setInt("atof_s3_comp", row, track.get_ATOF_S3_component()); + + // error on atof_sn_(x,y,z) + RealMatrix atof_s1_errorMatrix = track.get_ATOF_S1_errorMatrix(); + RealMatrix atof_s2_errorMatrix = track.get_ATOF_S2_errorMatrix(); + RealMatrix atof_s3_errorMatrix = track.get_ATOF_S3_errorMatrix(); + if (atof_s1_errorMatrix != null) { + bank.setFloat("atof_s1_sigma_x", row, (float) Math.sqrt(atof_s1_errorMatrix.getEntry(0, 0))); + bank.setFloat("atof_s1_sigma_y", row, (float) Math.sqrt(atof_s1_errorMatrix.getEntry(1, 1))); + bank.setFloat("atof_s1_sigma_z", row, (float) Math.sqrt(atof_s1_errorMatrix.getEntry(2, 2))); + } + if (atof_s2_errorMatrix != null) { + bank.setFloat("atof_s2_sigma_x", row, (float) Math.sqrt(atof_s2_errorMatrix.getEntry(0, 0))); + bank.setFloat("atof_s2_sigma_y", row, (float) Math.sqrt(atof_s2_errorMatrix.getEntry(1, 1))); + bank.setFloat("atof_s2_sigma_z", row, (float) Math.sqrt(atof_s2_errorMatrix.getEntry(2, 2))); + } + if (atof_s3_errorMatrix != null) { + bank.setFloat("atof_s3_sigma_x", row, (float) Math.sqrt(atof_s3_errorMatrix.getEntry(0, 0))); + bank.setFloat("atof_s3_sigma_y", row, (float) Math.sqrt(atof_s3_errorMatrix.getEntry(1, 1))); + bank.setFloat("atof_s3_sigma_z", row, (float) Math.sqrt(atof_s3_errorMatrix.getEntry(2, 2))); + } + row++; } 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 008e892104..5defe95d2f 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 @@ -2,6 +2,7 @@ import org.apache.commons.math3.linear.Array2DRowRealMatrix; import org.apache.commons.math3.linear.ArrayRealVector; +import org.apache.commons.math3.linear.MatrixUtils; import org.apache.commons.math3.linear.RealMatrix; import org.apache.commons.math3.linear.RealVector; import org.jlab.detector.calib.utils.DatabaseConstantProvider; @@ -9,8 +10,9 @@ import org.jlab.geom.prim.Line3D; import org.jlab.geom.prim.Point3D; import org.jlab.geom.detector.alert.AHDC.AlertDCFactory; +import org.jlab.rec.ahdc.KalmanFilter.KFHit; -public class Hit implements Comparable { +public class Hit implements Comparable, KFHit { private final int id; private final int superLayerId; @@ -110,6 +112,7 @@ public Line3D getLine() { return wireLine; } + @Override public double getRadius() { return radius; } @@ -174,11 +177,13 @@ public void setTrackId(int _trackId) { this.trackId = _trackId; } - public RealVector get_Vector() { + @Override + public RealVector MeasurementVector() { return new ArrayRealVector(new double[]{this.doca}); } - public RealMatrix get_MeasurementNoise() { + @Override + public RealMatrix MeasurementNoiseMatrix() { double mean_error = 0.471; // mm (no difference between adc and time) double error_on_adc = (1.15146*raw_adc + 437.63)/(3.21187*raw_adc + 878.855); // mm double error_on_time = (0.4423*time + 13.7215)/(0.846038*time + 31.9867); // mm @@ -188,11 +193,43 @@ public RealMatrix get_MeasurementNoise() { //return new Array2DRowRealMatrix(new double[][]{{0.09}}); } - // a signature for KalmanFilter.Hit_beam - public RealVector get_Vector_beam() { - return null; + // Projection function + @Override + public RealVector ProjectionFunction(RealVector x) { + double d = this.distance(new Point3D(x.getEntry(0), x.getEntry(1), x.getEntry(2))); + return MatrixUtils.createRealVector(new double[]{d}); + } + + // Jacobian matrix of the measurement with respect to (x, y, z, px, py, pz) + @Override + public RealMatrix ProjectionMatrix(RealVector x) { + + double ddocadx = partialProjectionMatrix(x, 0); + double ddocady = partialProjectionMatrix(x, 1); + double ddocadz = partialProjectionMatrix(x, 2); + double ddocadpx = partialProjectionMatrix(x, 3); + double ddocadpy = partialProjectionMatrix(x, 4); + double ddocadpz = partialProjectionMatrix(x, 5); + + return MatrixUtils.createRealMatrix(new double[][]{ + {ddocadx, ddocady, ddocadz, ddocadpx, ddocadpy, ddocadpz}}); + } + + private double partialProjectionMatrix(RealVector x, int i) { + 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 = this.ProjectionFunction(x_plus).getEntry(0); + double doca_minus = this.ProjectionFunction(x_minus).getEntry(0); + + return (doca_plus - doca_minus) / (2 * h); } + @Override public double distance(Point3D point3D) { return this.wireLine.distance(point3D).length(); } 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 deleted file mode 100644 index fd25c2e94d..0000000000 --- a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Hit_beam.java +++ /dev/null @@ -1,52 +0,0 @@ -package org.jlab.rec.ahdc.KalmanFilter; - -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 { - - private double x,y,z; - private double r,phi; - Line3D beamline; - - 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/KFHit.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/KFHit.java new file mode 100644 index 0000000000..3a2e693192 --- /dev/null +++ b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/KFHit.java @@ -0,0 +1,22 @@ +package org.jlab.rec.ahdc.KalmanFilter; + +import org.apache.commons.math3.linear.RealMatrix; +import org.apache.commons.math3.linear.RealVector; +import org.jlab.geom.prim.Point3D; +/** + * An interface to unify the hits used the Kalman Filter (e.g AHDC hits, ATOF hits, beamline) + * + * @author Felix Touchte Codjo + */ +public interface KFHit { + public double distance(Point3D point3D); + public double getRadius(); + /** Return the measurement encoded in this KFHit */ + public RealVector MeasurementVector(); + /** Return the measurement noise matrix for this this KFHit */ + public RealMatrix MeasurementNoiseMatrix(); + /** Compute the measure for a given state vector */ + public RealVector ProjectionFunction(RealVector x); + /** Compute the Jacobian matrix of the {@link #ProjectionFunction(RealVector)} with respect of the components of the state vector */ + public RealMatrix ProjectionMatrix(RealVector x); +} 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 7263d6638f..8fb0ecaafd 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 @@ -2,7 +2,6 @@ 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; @@ -22,24 +21,23 @@ public class KFitter { private RealVector stateEstimation; private RealMatrix errorCovariance; - public final Stepper stepper; + private Stepper stepper; private final Propagator propagator; 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 double[] vertex_resolutions = {0.09,1e10}; // default values // dr^2 and dz^2 in mm^2 - 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; + public KFitter(final RealVector initialStateEstimate, final RealMatrix initialErrorCovariance, final Propagator propagator, final HashMap materialHashMap) { + this.stateEstimation = initialStateEstimate.copy(); + this.errorCovariance = initialErrorCovariance.copy(); + this.stepper = new Stepper(initialStateEstimate.toArray()); this.propagator = propagator; this.materialHashMap = materialHashMap; } - public void predict(Hit hit, boolean direction) throws Exception { + public void predict(KFHit hit, boolean direction) throws Exception { // Initialization stepper.initialize(direction); Stepper stepper1 = new Stepper(stepper.y); @@ -86,32 +84,12 @@ public void predict(Hit hit, boolean direction) throws Exception { errorCovariance = (transitionMatrix.multiply(errorCovariance.multiply(transitionMatrixT))).add(processNoise); } - public void correct(Hit hit) { - RealVector z; - RealMatrix measurementNoise; - RealMatrix measurementMatrix; - RealVector h; - // check if the hit is the beamline - if (hit.getRadius() < 1) { - // the diagonal elements are the squared errors in r, phi, z - measurementNoise = - new Array2DRowRealMatrix( - new double[][]{ - {vertex_resolutions[0], 0.0000, 0.0000}, - {0.00, 1e10, 0.0000}, - {0.00, 0.0000, vertex_resolutions[1]} - });//3x3 - measurementMatrix = H_beam(stateEstimation);//6x3 - h = h_beam(stateEstimation);//3x1 - 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 - } + public void correct(KFHit hit) { + RealVector z = hit.MeasurementVector(); + RealVector h = hit.ProjectionFunction(stateEstimation); + + RealMatrix measurementNoise = hit.MeasurementNoiseMatrix(); + RealMatrix measurementMatrix = hit.ProjectionMatrix(stateEstimation); RealMatrix measurementMatrixT = measurementMatrix.transpose(); // S = H * P(k) * H' + R @@ -138,6 +116,7 @@ public void correct(Hit hit) { stepper.y = stateEstimation.toArray(); } + // specific to AHDC hits public double residual(Hit hit) { double d = hit.distance( new Point3D( stateEstimation.getEntry(0), stateEstimation.getEntry(1), stateEstimation.getEntry(2) ) ); return hit.getDoca()-d; @@ -147,7 +126,7 @@ public void ResetErrorCovariance(final RealMatrix initialErrorCovariance){ this.errorCovariance = initialErrorCovariance; } - private RealMatrix F(Hit hit, Stepper stepper1) throws Exception { + private RealMatrix F(KFHit hit, Stepper stepper1) throws Exception { double[] dfdx = subfunctionF(hit, stepper1, 0); double[] dfdy = subfunctionF(hit, stepper1, 1); @@ -160,7 +139,7 @@ private RealMatrix F(Hit hit, Stepper stepper1) throws Exception { {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(Hit hit, Stepper stepper1, int i) throws Exception { + double[] subfunctionF(KFHit 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); @@ -184,87 +163,6 @@ private RealMatrix F(Hit hit, Stepper stepper1) throws Exception { return new double[]{dxdi, dydi, dzdi, dpxdi, dpydi, dpzdi}; } - // 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}); - } - - // 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, 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); - - return MatrixUtils.createRealMatrix(new double[][]{ - {ddocadx, ddocady, ddocadz, ddocadpx, ddocadpy, ddocadpz}}); - } - - double subfunctionH(RealVector x, Hit hit, int i) { - 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, hit).getEntry(0); - double doca_minus = h(x_minus, hit).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); - double yy = x.getEntry(1); - double zz = x.getEntry(2); - - double r = Math.hypot(xx, yy); - double phi = Math.atan2(yy, xx); - - 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); - double yy = x.getEntry(1); - - double drdx = (xx) / (Math.hypot(xx, yy)); - double drdy = (yy) / (Math.hypot(xx, yy)); - double drdz = 0.0; - double drdpx = 0.0; - double drdpy = 0.0; - double drdpz = 0.0; - - double dphidx = -(yy) / (xx * xx + yy * yy); - double dphidy = (xx) / (xx * xx + yy * yy); - double dphidz = 0.0; - double dphidpx = 0.0; - double dphidpy = 0.0; - double dphidpz = 0.0; - - double dzdx = 0.0; - double dzdy = 0.0; - double dzdz = 1.0; - double dzdpx = 0.0; - double dzdpy = 0.0; - double dzdpz = 0.0; - - return MatrixUtils.createRealMatrix( - new double[][]{ - {drdx, drdy, drdz, drdpx, drdpy, drdpz}, - {dphidx, dphidy, dphidz, dphidpx, dphidpy, dphidpz}, - {dzdx, dzdy, dzdz, dzdpx, dzdpy, dzdpz} - }); - } public RealVector getStateEstimationVector() { return stateEstimation.copy(); @@ -273,10 +171,8 @@ public RealVector getStateEstimationVector() { public RealMatrix getErrorCovarianceMatrix() { return errorCovariance.copy(); } - - public void setVertexResolution(double[] res) { - vertex_resolutions[0] = res[0]; - vertex_resolutions[1] = res[1]; - } + + /** Return a copy of the stepper. It is like a snapshot of the propagation. */ + public Stepper getStepper() {return new Stepper(stepper);} } 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 272857efbd..936a29bd12 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,22 +1,23 @@ package org.jlab.rec.ahdc.KalmanFilter; import java.util.ArrayList; -import java.util.Collections; import java.util.HashMap; +import java.util.Random; import org.apache.commons.math3.linear.ArrayRealVector; -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.PDGDatabase; import org.jlab.clas.pdg.PDGParticle; import org.jlab.clas.tracking.kalmanfilter.Material; -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 org.apache.commons.math3.linear.RealMatrixFormat; +import org.jlab.detector.calib.utils.DatabaseConstantProvider; +import org.jlab.geom.base.Component; +import org.jlab.geom.detector.alert.ATOF.AlertTOFDetector; +import org.jlab.geom.detector.alert.ATOF.AlertTOFFactory; +import org.jlab.geom.prim.Point3D; + /** * This is the main routine of the Kalman Filter. The fit is done by a KFitter @@ -31,135 +32,129 @@ public class KalmanFilter { public KalmanFilter(PDGParticle particle, int Niter) {this.particle = particle; this.Niter = Niter;} - public KalmanFilter(ArrayList tracks, DataEvent event, final double magfield, boolean IsMC) {propagation(tracks, event, magfield, IsMC);} + HashMap materialHashMap = MaterialMap.generateMaterials(); private PDGParticle particle; private int Niter = 40; // number of iterations for the Kalman Filter private boolean IsVtxDefined = false; // implemented but not used yet - private double[] vertex_resolutions = {0.09, 1e10}; // {error in r squared in mm^2, error in z squared in mm^2} - // mm, CLAS and AHDC don't necessary have the same alignement (ZERO), this parameter may be subject to calibration - private double clas_alignement = -54; + private double vz_constraint = 0; + // mm, they are the misalignement with respect to the AHDC: the are defined in ALERTEngine + private double clas_alignement = 0; + private double atof_alignement = 0; + + private int counter = 0; // number of utilisation of the Kalman Filter + HashMap> ATOF_hits_predicted = new HashMap<>(); // trackid vs (sector, layer, wedge) + AlertTOFDetector ATOFdet = null; // a reference to the ATOF geometry - public void propagation(ArrayList tracks, DataEvent event, final double magfield, boolean IsMC) { + public void propagation(ArrayList tracks, final double magfield, boolean IsMC) { try { - double vz_constraint = 0; // to be linked to the electron vertex + counter++; // Initialization --------------------------------------------------------------------- final int numberOfVariables = 6; final double tesla = 0.001; final double[] B = {0.0, 0.0, magfield / 10 * tesla}; - HashMap materialHashMap = MaterialMap.generateMaterials(); - // Recover the vertex of the electron - if (event.hasBank("REC::Particle")) { - DataBank recBank = event.getBank("REC::Particle"); - int row = 0; - while ((!IsVtxDefined) && row < recBank.rows()) { - if (recBank.getInt("pid", row) == 11) { - IsVtxDefined = true; - vz_constraint = 10*recBank.getFloat("vz",row) - (IsMC ? 0 : clas_alignement); // mm - //////////////////////////////////////// - /// compute electron resolution here - /// it depends en p and theta - /// the fine tuning will be done later - /// //////////////////////////////////// - //double px = recBank.getFloat("px",row); - //double py = recBank.getFloat("py",row); - //double pz = recBank.getFloat("pz",row); - //double p = Math.sqrt(px*px+py*py+pz*pz); - //double theta = Math.acos(pz/p); - vertex_resolutions[0] = 0.09; - vertex_resolutions[1] = 64;//4 + 1e10*theta + 1e10*p; - } - row++; - } - } - + + // Loop over tracks for (Track track : tracks) { - // Initialize state vector + /// Initialize state vector double x0 = 0.0; double y0 = 0.0; - double z0 = IsVtxDefined ? vz_constraint : track.get_Z0(); + double z0 = (IsVtxDefined && counter < 2) ? vz_constraint : track.get_Z0(); double px0 = track.get_px(); double py0 = track.get_py(); double pz0 = track.get_pz(); double[] y = new double[]{x0, y0, z0, px0, py0, pz0}; - // Read list of hits + + /// Read list of hits + RadialKFHit beam_hit = track.getBeamlineHit(); ArrayList AHDC_hits = track.getHits(); - Collections.sort(AHDC_hits); // sorted following the compareTo() method in Hit.java + ArrayList ATOF_hits = track.getATOFHits(); - // Start propagation - Stepper stepper = new Stepper(y); + /// Initialize propagator RungeKutta4 RK4 = new RungeKutta4(particle, 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); - 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); - if (IsVtxDefined) TrackFitter.setVertexResolution(vertex_resolutions); + /// Initialization of the Kalman Fitter + RealVector initialStateEstimate = new ArrayRealVector(y); + RealMatrix initialErrorCovariance = track.getErrorCovarianceMatrix(); + KFitter TrackFitter = new KFitter(initialStateEstimate, initialErrorCovariance, propagator, materialHashMap); // Loop over number of iterations for (int k = 0; k < Niter; k++) { - // Forward propagation + + // Forward propagation in AHDC 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--) { + + // Take into account ATOF hits + if (!AHDC_hits.isEmpty() && !ATOF_hits.isEmpty()) { + // Forward propagation in ATOF + for (KFHit hit : ATOF_hits) { + TrackFitter.predict(hit, true); + TrackFitter.correct(hit); + } + // Backward propagation in ATOF + for (int i = ATOF_hits.size()-2; i >= 0; i--){ + KFHit hit = ATOF_hits.get(i); + TrackFitter.predict(hit, false); + TrackFitter.correct(hit); + } + // Backward propagation to the last AHDC layer + { + Hit hit = AHDC_hits.getLast(); + TrackFitter.predict(hit, false); + TrackFitter.correct(hit); + } + } + + // Backward propagation (from AHDC last layer to first AHDC 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) + + // Backward propagation (from first AHDC layer to beamline) { - Hit hit = new Hit_beam(0, 0, vz_constraint); - TrackFitter.predict(hit, false); - TrackFitter.correct(hit); + TrackFitter.predict(beam_hit, false); + TrackFitter.correct(beam_hit); } - } - - /*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()));*/ + } // end loop over nb. iteration - + /// Output: position and momentum RealVector x_out = TrackFitter.getStateEstimationVector(); track.setPositionAndMomentumVec(x_out.toArray()); + track.setErrorCovarianceMatrix(TrackFitter.getErrorCovarianceMatrix()); + + /// Post fit propagation (no correction) + KFitter PostFitPropagator = new KFitter(TrackFitter.getStateEstimationVector(), initialErrorCovariance, new Propagator(RK4), materialHashMap); - // 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); + // Forward propagation in AHDC 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)); - } + hit.setResidual(PostFitPropagator.residual(hit)); // output: residual } - - // Fill track and hit bank + + // Fill values for AHDC::kftrack // TO DO : s and p_drift have to be checked to be sure they represent what we want - double s = PostFitPropagator.stepper.sTot; - double p_drift = PostFitPropagator.stepper.p(); + Stepper current_stepper = PostFitPropagator.getStepper(); + double s = current_stepper.sTot; + double p_drift = current_stepper.p(); int sum_adc = 0; double sum_residuals = 0; double chi2 = 0; for (Hit hit : AHDC_hits) { sum_adc += hit.getADC(); sum_residuals += hit.getResidual(); - chi2 += Math.pow(hit.getResidual(),2)/hit.get_MeasurementNoise().getEntry(0,0); + chi2 += Math.pow(hit.getResidual(),2)/hit.MeasurementNoiseMatrix().getEntry(0,0); } track.set_sum_adc(sum_adc); track.set_sum_residuals(sum_residuals); @@ -168,12 +163,264 @@ public void propagation(ArrayList tracks, DataEvent event, final double m track.set_dEdx(sum_adc/s); track.set_path(s); track.set_n_hits(AHDC_hits.size()); + + + //////// + /// Revision of the code: I'm here + /// ///// + + // Projection towards the ATOF surfaces + // R1 : lower surface of an ATOF bar + // R2 : upper surface of an ATOF bar = lower surface of an ATOF wedge + // R3 : upper surface of an ATOF wedge + Point3D pR1 = ATOFdet.getSector(0).getSuperlayer(0).getLayer(0).getComponent(10).getVolumePoint(0); + Point3D pR2 = ATOFdet.getSector(0).getSuperlayer(0).getLayer(0).getComponent(10).getVolumePoint(2); + Point3D pR3 = ATOFdet.getSector(0).getSuperlayer(1).getLayer(0).getComponent(0).getVolumePoint(2); + double R1 = Math.hypot(pR1.x(), pR1.y()); + double R2 = Math.hypot(pR2.x(), pR2.y()); + double R3 = Math.hypot(pR3.x(), pR3.y()); + { + // From last AHDC hit to surface R1 + RadialSurfaceKFHit hitR1 = new RadialSurfaceKFHit(R1); + PostFitPropagator.predict(hitR1, true); + Stepper stepperR1 = PostFitPropagator.getStepper(); + double[] vecR1 = PostFitPropagator.getStateEstimationVector().toArray(); + Point3D posR1 = new Point3D(vecR1[0], vecR1[1], vecR1[2]); + posR1.translateXYZ(0, 0, atof_alignement); + int[] idR1 = predict_bar(ATOFdet, posR1); + track.set_ATOF_S1_stepper(stepperR1); + if (Math.abs(stepperR1.r() - R1) < 1.5*stepperR1.h) { + track.set_ATOF_region(1); + if (counter < 2 && idR1 != null) { + track.set_ATOF_S1_component(10000*idR1[0] + 100*idR1[1] + idR1[2]); + track.set_ATOF_S1_errorMatrix(PostFitPropagator.getErrorCovarianceMatrix()); + } + } + + + // System.out.println("###########################################################"); + // System.out.println("ATOF surface R1 : " + R1); + // System.out.printf (" final position : x (%f) y (%f) z (%f) --> R (%f)\n", posR1.x(), posR1.y(), posR1.z(), Math.hypot(posR1.x(), posR1.y())); + // System.out.println(" stepper.h : " + PostFitPropagator.getStepper().h); + // System.out.printf (" ---> sector (%2d) layer (%2d) component (%2d)\n", idR1[0], idR1[1], idR1[2]); + + // From surface R1 to surface R2 + RadialSurfaceKFHit hitR2 = new RadialSurfaceKFHit(R2); + PostFitPropagator.predict(hitR2, true); + Stepper stepperR2 = PostFitPropagator.getStepper(); + double[] vecR2 = PostFitPropagator.getStateEstimationVector().toArray(); + Point3D posR2 = new Point3D(vecR2[0], vecR2[1], vecR2[2]); + posR2.translateXYZ(0, 0, atof_alignement); + int[] idR2 = predict_wedge(ATOFdet, posR2); + track.set_ATOF_S2_stepper(stepperR2); + if (Math.abs(stepperR2.r() - R2) < 1.5*stepperR2.h) { + track.set_ATOF_region(2); + if (counter < 2 && idR2 != null) { + track.set_ATOF_S2_component(10000*idR2[0] + 100*idR2[1] + idR2[2]); + track.set_ATOF_S2_errorMatrix(PostFitPropagator.getErrorCovarianceMatrix()); + } + } + // From surface R2 to surface R3 + RadialSurfaceKFHit hitR3 = new RadialSurfaceKFHit(R3); + PostFitPropagator.predict(hitR3, true); + Stepper stepperR3 = PostFitPropagator.getStepper(); + double[] vecR3 = PostFitPropagator.getStateEstimationVector().toArray(); + Point3D posR3 = new Point3D(vecR3[0], vecR3[1], vecR3[2]); + posR3.translateXYZ(0, 0, atof_alignement); + int[] idR3 = predict_wedge(ATOFdet, posR3); + track.set_ATOF_S3_stepper(stepperR3); + if (Math.abs(stepperR3.r() - R3) < 1.5*stepperR3.h) { + track.set_ATOF_region(3); + if (counter < 2 && idR3 != null) { + track.set_ATOF_S3_component(10000*idR3[0] + 100*idR3[1] + idR3[2]); + track.set_ATOF_S3_errorMatrix(PostFitPropagator.getErrorCovarianceMatrix()); + } + } + + } // end propagation towards ATOF surface + + + + + }//end of loop on track candidates } catch (Exception e) { - //e.printStackTrace(); - //System.out.println("======> Kalman Filter Error"); + e.printStackTrace(); + //System.out.println("Error in Kalman Filter"); + } + } + public void set_Niter(int Niter) {this.Niter = Niter;} + public int get_Niter() {return this.Niter;} + public void set_particle(PDGParticle particle) {this.particle = particle;} + public PDGParticle get_particle() {return this.particle;} + public HashMap> get_ATOF_hits_predicted() {return this.ATOF_hits_predicted;} + + // Test + public static void main(String[] args) { + // ATOF detector + AlertTOFDetector atof = (new AlertTOFFactory()).createDetectorCLAS(new DatabaseConstantProvider()); + + int Npts = 1000; + int counter = 0; + int err = 0; + for (int i = 0; i < Npts; i++) { + Random rand = new Random(); + int true_sector = rand.nextInt(15); + int true_layer = rand.nextInt(4); + int true_wedge = rand.nextInt(10); + + Component comp = atof.getSector(true_sector).getSuperlayer(1).getLayer(true_layer).getComponent(true_wedge); + // top face + Point3D p0 = comp.getVolumePoint(0); + Point3D p1 = comp.getVolumePoint(1); + Point3D p2 = comp.getVolumePoint(2); + Point3D p3 = comp.getVolumePoint(3); + // bottom face + Point3D p4 = comp.getVolumePoint(4); + Point3D p5 = comp.getVolumePoint(5); + Point3D p6 = comp.getVolumePoint(6); + Point3D p7 = comp.getVolumePoint(7); + + // Random point int he current wedge volume + //Point3D pt = p0.lerp(p7, Math.random()); + //Point3D pt = comp.getMidpoint(); + double t0 = rand.nextDouble(1); + double t1 = rand.nextDouble(1-t0); + double t2 = rand.nextDouble(1-t0-t1); + double t3 = 1-t0-t1-t2; + //System.out.printf("t0 + t1 + t2 + t3 = %f\n", t0+t1+t2+t3); + double t4 = rand.nextDouble(1); + double t5 = rand.nextDouble(1-t4); + double t6 = rand.nextDouble(1-t4-t5); + double t7 = 1-t4-t5-t6; + //System.out.printf("t4 + t5 + t6 + t7 = %f\n", t4+t5+t6+t7); + double x_top = t0*p0.x() + t1*p1.x() + t2*p2.x() + t3*p3.x(); + double y_top = t0*p0.y() + t1*p1.y() + t2*p2.y() + t3*p3.y(); + double x_bot = t4*p4.x() + t5*p5.x() + t6*p6.x() + t7*p7.x(); + double y_bot = t4*p4.y() + t5*p5.y() + t6*p6.y() + t7*p7.y(); + Point3D pt_top = new Point3D(x_top, y_top, p0.z()); + Point3D pt_bot = new Point3D(x_bot, y_bot, p4.z()); + Point3D pt = pt_top.lerp(pt_bot, Math.random()); + //System.out.printf("distance from midpoint : %f\n", comp.getMidpoint().distance(pt)); + + // Test the algoritm + int[] res = KalmanFilter.predict_wedge(atof, pt); + + if (res[0] == true_sector && res[1] == true_layer && res[2] == true_wedge) { + counter++; + } else { + err++; + System.out.printf("%d) Initial wedge : sector (% 2d) layer (% 2d) wedge (% 2d)\n", err, true_sector, true_layer, true_wedge); + System.out.printf("%d) Predicted wedge : sector (% 2d) layer (% 2d) wedge (% 2d)\n", err, res[0], res[1], res[2]); + } + } + System.out.printf("Nb of testing : %d\n", Npts); + System.out.printf("Nb of success : %d (%.2f %%)\n", counter, 100.0*counter/Npts); + } + /** + * @param pt is defined in the center of the ATOF + */ + static public int[] predict_wedge(AlertTOFDetector atof, Point3D pt) { + // find the wedge + int wedge = -1; + double dz = 1e10; + for (int c = 0; c < atof.getSector(0).getSuperlayer(1).getLayer(0).getNumComponents(); c++) { + Point3D midpoint = atof.getSector(0).getSuperlayer(1).getLayer(0).getComponent(c).getMidpoint(); + if (Math.abs(midpoint.z()-pt.z()) < dz) { + dz = Math.abs(midpoint.z()-pt.z()); + wedge = c; + } + } + if (wedge == -1) return null; + // find sector and layer + int sector = -1; + int layer = -1; + double d = 1e10; + for (int s = 0; s < atof.getNumSectors(); s++) { + for (int l = 0; l < atof.getSector(s).getSuperlayer(1).getNumLayers(); l++) { + Point3D midpoint = atof.getSector(s).getSuperlayer(1).getLayer(l).getComponent(wedge).getMidpoint(); + if (midpoint.distance(pt) < d) { + d = midpoint.distance(pt); + sector = s; + layer = l; + } + } + } + if (sector == -1 || layer == -1) { + return null; + } else { + return new int[] {sector, layer, wedge}; } } - void set_Niter(int Niter) {this.Niter = Niter;} - void set_particle(PDGParticle particle) {this.particle = particle;} + + /** + * @param pt is defined in the center of the ATOF + */ + static public int[] predict_bar(AlertTOFDetector atof, Point3D pt) { + // find sector and layer + int sector = -1; + int layer = -1; + double d = 1e10; + for (int s = 0; s < atof.getNumSectors(); s++) { + for (int l = 0; l < atof.getSector(s).getSuperlayer(0).getNumLayers(); l++) { + Point3D midpoint = atof.getSector(s).getSuperlayer(0).getLayer(l).getComponent(10).getMidpoint(); + double distance = midpoint.vectorTo(pt).rho(); + if (distance < d) { + d = distance; + sector = s; + layer = l; + } + } + } + if (sector == -1 || layer == -1) { + return null; + } else { + return new int[] {sector, layer, 10}; + } + } + + public ArrayList get_adjacent_wedges(int[] identifiers) { + int sector = identifiers[0]; + int layer = identifiers[1]; + int wedge = identifiers[2]; + + // find adjacent layer and sector + int sector_plus = sector; + int sector_minus = sector; + int layer_plus = layer+1; + int layer_minus = layer-1; + if (layer == 0) { + sector_plus = sector; + sector_minus = Math.floorMod(sector-1, 15); + layer_plus = layer+1; + layer_minus = 3; + } + else if (layer == 3) { + sector_plus = Math.floorMod(sector+1, 15); + sector_minus = sector; + layer_plus = 0; + layer_minus = layer-1; + } + // Here are all the adjacents wedges (maximum 8) + ArrayList listOfWedges = new ArrayList<>(); + listOfWedges.add(new int[]{sector_plus, layer_plus, wedge}); + listOfWedges.add(new int[]{sector_plus, layer_plus, wedge}); + if (wedge-1 >= 0) { + listOfWedges.add(new int[]{sector, layer, wedge-1}); + listOfWedges.add(new int[]{sector_plus, layer_plus, wedge-1}); + listOfWedges.add(new int[]{sector_minus, layer_minus, wedge-1}); + } + if (wedge+1 <= 9) { + listOfWedges.add(new int[]{sector, layer, wedge+1}); + listOfWedges.add(new int[]{sector_plus, layer_plus, wedge+1}); + listOfWedges.add(new int[]{sector_minus, layer_minus, wedge+1}); + } + return listOfWedges; + } + + public void set_ATOF_detector(AlertTOFDetector atof) { this.ATOFdet = atof;} + public void set_clas_alignement(double _shift) {this.clas_alignement = _shift;} + public void set_atof_alignement(double _shift) {this.atof_alignement = _shift;} + public void set_vz_constraint(double _vz) {this.vz_constraint = _vz;} + public void set_vertex_flag(boolean _flag) {this.IsVtxDefined = _flag;} } 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 e85021addb..6f4f58a835 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,13 +1,10 @@ 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 org.jlab.clas.tracking.kalmanfilter.Material; import java.util.HashMap; -import org.jlab.rec.ahdc.Hit.Hit; /** * Is responsible of the propagation @@ -27,7 +24,7 @@ public Propagator(RungeKutta4 rungeKutta4) { } // Propagate the stepper toward the next hit - void propagate(Stepper stepper, Hit hit, HashMap materialHashMap) { + void propagate(Stepper stepper, KFHit hit, HashMap materialHashMap) { // Do not allow more than 10000 steps (very critical cases) final int maxNbOfStep = 10000; @@ -95,9 +92,8 @@ void propagate(Stepper stepper, Hit hit, HashMap materialHashM } 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 + // so, go back to the previous step (the best we have) and stop the propagation stepper.copyContent(prev_stepper); - stepper.h = 0.5; break; } } @@ -161,7 +157,7 @@ void propagate(Stepper stepper, Hit hit, HashMap materialHashM } - public RealVector f(Stepper stepper, Hit hit, HashMap materialHashMap) { + public RealVector f(Stepper stepper, KFHit hit, HashMap materialHashMap) { propagate(stepper, hit, materialHashMap); return new ArrayRealVector(stepper.y); } diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/RadialKFHit.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/RadialKFHit.java new file mode 100644 index 0000000000..220118255b --- /dev/null +++ b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/RadialKFHit.java @@ -0,0 +1,105 @@ +package org.jlab.rec.ahdc.KalmanFilter; + +import org.apache.commons.math3.linear.ArrayRealVector; +import org.apache.commons.math3.linear.MatrixUtils; +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.Point3D; + +/** + * Implement a hit for which the state vector is a 3x1 matrix (r, phi, z) + * e.g beamline, ATOF wedges/bars + * + * @author Mathieu Ouillon + * @author Éric Fuchey + * @author Felix Touchte Codjo + */ +public class RadialKFHit implements KFHit { + + private double r,phi,z; + Line3D line; + RealMatrix measurementNoise = null; + + public RadialKFHit(double x, double y , double z) { + this.z = z; + this.r = Math.hypot(x,y); + this.phi = Math.atan2(y,x); + line = new Line3D(x,y,0,x,y,1); // a line parallel to the beam axis + } + + @Override + public RealVector MeasurementVector() { + return new ArrayRealVector(new double[] {this.r, this.phi, this.z}); + } + + @Override + public RealMatrix MeasurementNoiseMatrix() { + return measurementNoise; + } + + public void setMeasurementNoise(RealMatrix measurementNoise) { + this.measurementNoise= measurementNoise; + } + + @Override + public double distance(Point3D point3D) { + return this.line.distance(point3D).length(); + } + + @Override + public double getRadius() { + return r; + } + + // Projection function + @Override + public RealVector ProjectionFunction(RealVector x) { + + double xx = x.getEntry(0); + double yy = x.getEntry(1); + double zz = x.getEntry(2); + + double r = Math.hypot(xx, yy); + double phi = Math.atan2(yy, xx); + + 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) + @Override + public RealMatrix ProjectionMatrix(RealVector x) { + + double xx = x.getEntry(0); + double yy = x.getEntry(1); + + double drdx = (xx) / (Math.hypot(xx, yy)); + double drdy = (yy) / (Math.hypot(xx, yy)); + double drdz = 0.0; + double drdpx = 0.0; + double drdpy = 0.0; + double drdpz = 0.0; + + double dphidx = -(yy) / (xx * xx + yy * yy); + double dphidy = (xx) / (xx * xx + yy * yy); + double dphidz = 0.0; + double dphidpx = 0.0; + double dphidpy = 0.0; + double dphidpz = 0.0; + + double dzdx = 0.0; + double dzdy = 0.0; + double dzdz = 1.0; + double dzdpx = 0.0; + double dzdpy = 0.0; + double dzdpz = 0.0; + + return MatrixUtils.createRealMatrix( + new double[][]{ + {drdx, drdy, drdz, drdpx, drdpy, drdpz}, + {dphidx, dphidy, dphidz, dphidpx, dphidpy, dphidpz}, + {dzdx, dzdy, dzdz, dzdpx, dzdpy, dzdpz} + }); + } + +} diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/RadialSurfaceKFHit.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/RadialSurfaceKFHit.java new file mode 100644 index 0000000000..6e633877bb --- /dev/null +++ b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/RadialSurfaceKFHit.java @@ -0,0 +1,79 @@ +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.MatrixUtils; +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.Point3D; + +/** + * Implement a hit for which the state vector is a 3x1 matrix (r, phi, z) + * e.g beamline, ATOF wedges/bars + * + * @author Mathieu Ouillon + * @author Éric Fuchey + * @author Felix Touchte Codjo + */ +public class RadialSurfaceKFHit implements KFHit { + + private double r; + + public RadialSurfaceKFHit(double r) { + this.r = r; + } + + @Override + public RealVector MeasurementVector() { + return new ArrayRealVector(new double[] {this.r}); + } + + @Override + public RealMatrix MeasurementNoiseMatrix() { + return new Array2DRowRealMatrix(new double[][]{{1e-8}}); + } + + @Override + public double distance(Point3D point3D) { + return Math.abs(this.r - Math.hypot(point3D.x(), point3D.y())); + } + + @Override + public double getRadius() { + return r; + } + + // Projection function + @Override + public RealVector ProjectionFunction(RealVector x) { + + double xx = x.getEntry(0); + double yy = x.getEntry(1); + + double r = Math.hypot(xx, yy); + + return MatrixUtils.createRealVector(new double[]{r}); + } + + // Jacobian matrix of the measurement for the beamline with respect to (x, y, z, px, py, pz) + @Override + public RealMatrix ProjectionMatrix(RealVector x) { + + double xx = x.getEntry(0); + double yy = x.getEntry(1); + + double drdx = (xx) / (Math.hypot(xx, yy)); + double drdy = (yy) / (Math.hypot(xx, yy)); + double drdz = 0.0; + double drdpx = 0.0; + double drdpy = 0.0; + double drdpz = 0.0; + + return MatrixUtils.createRealMatrix( + new double[][]{ + {drdx, drdy, drdz, drdpx, drdpy, drdpz} + }); + } + +} diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Track/Track.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Track/Track.java index e9944b7846..969cb7eaab 100644 --- a/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Track/Track.java +++ b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Track/Track.java @@ -1,10 +1,14 @@ package org.jlab.rec.ahdc.Track; +import org.apache.commons.math3.linear.MatrixUtils; +import org.apache.commons.math3.linear.RealMatrix; import org.apache.commons.math3.linear.RealVector; import org.jlab.rec.ahdc.AI.InterCluster; import org.jlab.rec.ahdc.Cluster.Cluster; import org.jlab.rec.ahdc.HelixFit.HelixFitObject; import org.jlab.rec.ahdc.Hit.Hit; +import org.jlab.rec.ahdc.KalmanFilter.RadialKFHit; +import org.jlab.rec.ahdc.KalmanFilter.Stepper; import org.jlab.rec.ahdc.PreCluster.PreCluster; import org.jlab.rec.ahdc.PreCluster.PreClusterFinder; import org.jlab.rec.ahdc.AI.PreClustering; @@ -18,7 +22,9 @@ public class Track { private List _Clusters = new ArrayList<>(); private List _InterClusters = new ArrayList<>(); private boolean _Used = false; - private final ArrayList hits = new ArrayList<>(); + private final ArrayList hits = new ArrayList<>(); // AHDC hits + private ArrayList ATOF_hits = new ArrayList<>(); + private RadialKFHit beamline_hit = null; private int trackId = -1; ///< id of the track private int n_hits = 0; ///< number of hits @@ -35,6 +41,36 @@ public class Track { private double dEdx = 0; ///< deposited energy per path length (adc/mm) private double p_drift = 0; ///< momentum in the drift region (MeV) private double path = 0; ///< length of the track (mm) + // for the error matrix: first 3 lines in mm^2; last 3 lines in MeV^2 (in the beamline) + RealMatrix errorCovarianceMatrix = MatrixUtils.createRealMatrix(new double[][]{ + {50 , 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 , 900 , 0.0 , 0.0 , 0.0}, + {0.0 , 0.0 , 0.0 , 100 , 0.0 , 0.0}, + {0.0 , 0.0 , 0.0 , 0.0 , 100 , 0.0}, + {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 900}}); + // Position and momentum when the track crosses the ATOF surface + // S1 : lower surface of an ATOF bar + // S2 : upper surface of an ATOF bar = lower surface of an ATOF wedge + // S3 : upper surface of an ATOF wedge + Stepper ATOF_S1_stepper; + Stepper ATOF_S2_stepper; + Stepper ATOF_S3_stepper; + double ATOF_S1_radius; + double ATOF_S2_radius; + double ATOF_S3_radius; + int ATOF_region = 0; // is n if the trach reaches Sn, 0 otherwise (i.e does not reach S1) + + // tmp + int ATOF_S1_component = -1; + int ATOF_S2_component = -1; + int ATOF_S3_component = -1; + + // tmp : compatible with stepper: need to access error in x,y,z + RealMatrix ATOF_S1_errorMatrix; + RealMatrix ATOF_S2_errorMatrix; + RealMatrix ATOF_S3_errorMatrix; + // AHDC::aiprediction private int predicted_ATOF_sector = -1; @@ -57,13 +93,15 @@ public Track(ArrayList hitslist) { this.y0 = 0.0; this.z0 = 0.0; double p = 150.0;//MeV/c - //take first hit. - Hit hit = hitslist.get(0); - double phi = Math.atan2(hit.getY(), hit.getX()); - //hitslist. - this.px0 = p*Math.sin(phi); - this.py0 = p*Math.cos(phi); - this.pz0 = 0.0; + if (hitslist.size() > 0) { + //take first hit. + Hit hit = hitslist.get(0); + double phi = Math.atan2(hit.getY(), hit.getX()); + //hitslist. + this.px0 = p*Math.sin(phi); + this.py0 = p*Math.cos(phi); + this.pz0 = 0.0; + } } public void setPositionAndMomentum(HelixFitObject helixFitObject) { @@ -107,6 +145,17 @@ public ArrayList getHits() { return hits; } + public ArrayList getATOFHits() { + return this.ATOF_hits; + } + + public RadialKFHit getBeamlineHit() { + return this.beamline_hit; + } + + public void setATOFHits(ArrayList _ATOF_hits) {this.ATOF_hits = _ATOF_hits;} + public void setBeamlineHit(RadialKFHit _beamline_hit) {this.beamline_hit = _beamline_hit;} + @Override public String toString() { return "Track{" + "_Clusters=" + _Clusters + '}'; @@ -187,6 +236,8 @@ public void set_trackId(int _trackId) { public double get_dEdx() {return dEdx;} public double get_p_drift() {return p_drift;} public double get_path() {return path;} + public RealMatrix getErrorCovarianceMatrix() {return errorCovarianceMatrix;} + public void setErrorCovarianceMatrix(RealMatrix errorCovarianceMatrix) {this.errorCovarianceMatrix = errorCovarianceMatrix;} // AHDC::aiprediction public void set_predicted_ATOF_sector(int s) {predicted_ATOF_sector = s;} @@ -196,4 +247,30 @@ public void set_trackId(int _trackId) { public int get_predicted_ATOF_layer() {return predicted_ATOF_layer;} public int get_predicted_ATOF_wedge() {return predicted_ATOF_wedge;} + // Projection of the Track on the ATOF surfaces + public void set_ATOF_S1_stepper(Stepper _stepper) {this.ATOF_S1_stepper = _stepper;} + public void set_ATOF_S2_stepper(Stepper _stepper) {this.ATOF_S2_stepper = _stepper;} + public void set_ATOF_S3_stepper(Stepper _stepper) {this.ATOF_S3_stepper = _stepper;} + public void set_ATOF_region(int _n) {this.ATOF_region = _n;} + public Stepper get_ATOF_S1_stepper() {return this.ATOF_S1_stepper;} + public Stepper get_ATOF_S2_stepper() {return this.ATOF_S2_stepper;} + public Stepper get_ATOF_S3_stepper() {return this.ATOF_S3_stepper;} + public int get_ATOF_region() {return this.ATOF_region;} + + // tmp + public int get_ATOF_S1_component() {return this.ATOF_S1_component;} + public int get_ATOF_S2_component() {return this.ATOF_S2_component;} + public int get_ATOF_S3_component() {return this.ATOF_S3_component;} + public void set_ATOF_S1_component(int _comp) {this.ATOF_S1_component = _comp;} + public void set_ATOF_S2_component(int _comp) {this.ATOF_S2_component = _comp;} + public void set_ATOF_S3_component(int _comp) {this.ATOF_S3_component = _comp;} + + public RealMatrix get_ATOF_S1_errorMatrix() {return this.ATOF_S1_errorMatrix;} + public RealMatrix get_ATOF_S2_errorMatrix() {return this.ATOF_S2_errorMatrix;} + public RealMatrix get_ATOF_S3_errorMatrix() {return this.ATOF_S3_errorMatrix;} + public void set_ATOF_S1_errorMatrix(RealMatrix _matrix) { this.ATOF_S1_errorMatrix = _matrix;} + public void set_ATOF_S2_errorMatrix(RealMatrix _matrix) { this.ATOF_S2_errorMatrix = _matrix;} + public void set_ATOF_S3_errorMatrix(RealMatrix _matrix) { this.ATOF_S3_errorMatrix = _matrix;} + + } diff --git a/reconstruction/alert/src/main/java/org/jlab/service/alert/ALERTEngine.java b/reconstruction/alert/src/main/java/org/jlab/service/alert/ALERTEngine.java index 5465ec29d8..4a560ae110 100644 --- a/reconstruction/alert/src/main/java/org/jlab/service/alert/ALERTEngine.java +++ b/reconstruction/alert/src/main/java/org/jlab/service/alert/ALERTEngine.java @@ -4,13 +4,17 @@ import ai.djl.translate.TranslateException; import java.io.File; import java.util.ArrayList; +import java.util.Collections; +import java.util.Iterator; import java.util.concurrent.atomic.AtomicInteger; +import java.util.HashMap; import org.jlab.clas.reco.ReconstructionEngine; import org.jlab.clas.swimtools.Swim; import org.jlab.detector.calib.utils.DatabaseConstantProvider; import org.jlab.geom.base.Detector; import org.jlab.geom.detector.alert.ATOF.AlertTOFFactory; +import org.jlab.geom.detector.alert.ATOF.AlertTOFDetector; import org.jlab.io.base.DataBank; import org.jlab.io.base.DataEvent; import org.jlab.io.hipo.HipoDataSource; @@ -20,11 +24,14 @@ import org.jlab.rec.alert.banks.RecoBankWriter; import org.jlab.rec.alert.projections.TrackProjector; import org.jlab.rec.atof.hit.ATOFHit; +import org.jlab.rec.ahdc.KalmanFilter.RadialKFHit; import org.jlab.rec.ahdc.KalmanFilter.KalmanFilter; import org.jlab.rec.ahdc.Hit.Hit; import org.jlab.geom.detector.alert.AHDC.AlertDCDetector; import org.jlab.geom.detector.alert.AHDC.AlertDCFactory; import org.jlab.rec.ahdc.Track.Track; +import org.apache.commons.math3.linear.Array2DRowRealMatrix; +import org.apache.commons.math3.linear.RealMatrix; import org.jlab.clas.pdg.PDGDatabase; import org.jlab.clas.pdg.PDGParticle; import java.util.logging.Logger; @@ -57,7 +64,7 @@ public class ALERTEngine extends ReconstructionEngine { */ private RecoBankWriter rbc; static final Logger LOGGER = Logger.getLogger(ModelPrePID.class.getName()); - Detector ATOF; // ALERT ATOF detector + AlertTOFDetector ATOF; // ALERT ATOF detector private AlertDCDetector AHDC; // ALERT AHDC detector /** @@ -141,6 +148,7 @@ public boolean processDataEvent(DataEvent event) { if (run.get() == 0 || (run.get() != 0 && run.get() != newRun)) { run.set(newRun); } + //Do we need to read the event vx,vy,vz? //If not, this part can be moved in the initialization of the engine. @@ -150,6 +158,7 @@ public boolean processDataEvent(DataEvent event) { float magField[] = new float[3]; swim.BfieldLab(eventVx, eventVy, eventVz, magField); this.b = Math.sqrt(Math.pow(magField[0],2) + Math.pow(magField[1],2) + Math.pow(magField[2],2)); + TrackProjector projector = new TrackProjector(); projector.setB(this.b); @@ -159,7 +168,6 @@ public boolean processDataEvent(DataEvent event) { /// --------------------------------------------------------------------------------------- /// Track matching using AI --------------------------------------------------------------- - if (event == null) return false; // TODO: is it useful? if (!event.hasBank("AHDC::track")) return false; DataBank bank_AHDCtracks = event.getBank("AHDC::track"); @@ -319,8 +327,42 @@ public boolean processDataEvent(DataEvent event) { /// Kalmam Filter /// /////////////////////////////////////// - // read the list of tracks/hits from the banks AHDC::track and AHDC::hits + /// Pre conditions if (!event.hasBank("AHDC::track")) {return false;} + if (!event.hasBank("AHDC::hits")) {return false;} + + /// tmp: misalignement with respect to the center of the AHDC (mm) + double clas_alignement = +54; + double atof_alignement = -32.7; + + /// Read the electron vertex + double vz_electron = 0; + double[] vz_error2 = {0.09, 1e10, 1e10}; // mm^2, radians^2, mm^2, error on r, phi, z + boolean IsVertexDefined = false; + if (event.hasBank("REC::Particle")) { + DataBank recBank = event.getBank("REC::Particle"); + for (int row = 0; row < recBank.rows(); row++) { + if (recBank.getInt("pid", row) == 11) { + vz_electron = 10*recBank.getFloat("vz",row); // conversion in mm + IsVertexDefined = true; + + + //double px = recBank.getFloat("px",row); + //double py = recBank.getFloat("py",row); + //double pz = recBank.getFloat("pz",row); + //double p = Math.sqrt(px*px+py*py+pz*pz); + //double theta = Math.acos(pz/p); + + // set the resolutions on r and z! to be done + vz_error2[0] = 0.09; // should depend on p and theta + vz_error2[2] = 64; // should depend on p and theta + + break; // only look at the first electron + } + } + } + + /// Read the list of tracks/hits from the banks AHDC::track and AHDC::hits DataBank trackBank = event.getBank("AHDC::track"); DataBank hitBank = event.getBank("AHDC::hits"); ArrayList AHDC_tracks = new ArrayList<>(); @@ -337,7 +379,7 @@ public boolean processDataEvent(DataEvent event) { double doca = hitBank.getDouble("doca", hit_row); double time = hitBank.getDouble("time", hit_row); double tot = hitBank.getDouble("timeOverThreshold", hit_row); - // warning : adc is the calibrated one, we need the adc for the Kalman filter + // warning : adc is the calibrated one, we need the adc for the Kalman filter ! Hit hit = new Hit(id, superlayer, layer, wire, doca, adc, time); hit.setWirePosition(AHDC); hit.setTrackId(trackid); @@ -346,34 +388,166 @@ public boolean processDataEvent(DataEvent event) { AHDC_hits.add(hit); } } - AHDC_tracks.add(new Track(AHDC_hits)); - // Initialise the position and the momentum using the information of the AHDC::track - // position : mm - // momentum : MeV - double x = trackBank.getFloat("x", row); - double y = trackBank.getFloat("y", row); - double z = trackBank.getFloat("z", row); - double px = trackBank.getFloat("px", row); - double py = trackBank.getFloat("py", row); - double pz = trackBank.getFloat("pz", row); - double[] vec = {x, y, z, px, py, pz}; - AHDC_tracks.get(row).setPositionAndMomentumVec(vec); - AHDC_tracks.get(row).set_trackId(trackid); + if (AHDC_hits.size() > 0) { + Collections.sort(AHDC_hits); // sorted following the compareTo() method in Hit.java + Track track = new Track(AHDC_hits); + // Initialise the position and the momentum using the information of the AHDC::track + // position : mm + // momentum : MeV + double x = trackBank.getFloat("x", row); + double y = trackBank.getFloat("y", row); + double z = trackBank.getFloat("z", row); + double px = trackBank.getFloat("px", row); + double py = trackBank.getFloat("py", row); + double pz = trackBank.getFloat("pz", row); + double[] vec = {x, y, z, px, py, pz}; + track.setPositionAndMomentumVec(vec); + track.set_trackId(trackid); + AHDC_tracks.add(track); + } } - // intialise the Kalman Filter + + /// Associate the electron vertex (the beamline hit) to each track + boolean IsMC = event.hasBank("MC::Particle"); + double vz_constraint = vz_electron + (IsMC ? 0 : clas_alignement); // we don't have the misalignment in simulation + for (Track track : AHDC_tracks) { + RadialKFHit hit_beam = new RadialKFHit(0, 0, vz_constraint); + RealMatrix measurementNoise = new Array2DRowRealMatrix( + new double[][]{ + {vz_error2[0], 0.0000 , 0.0000}, + {0.0000 , vz_error2[1], 0.0000}, + {0.0000 , 0.0000 , vz_error2[2]} + });//3x3; + hit_beam.setMeasurementNoise(measurementNoise); + track.setBeamlineHit(hit_beam); + } + + /// Look for ATOF wedge hits predicted by the AI + HashMap map_ATOF_hits = new HashMap<>(); + for (Pair pair : matched_ATOF_hit_id) { + int trackid = pair.getKey(); + int atofid = pair.getValue(); + if (trackid > 0 && atofid > 0) { + // recover the wedge + for (int row = 0; row < bank_ATOFHits.rows(); row++) { + if (bank_ATOFHits.getShort("id", row) == atofid) { + double x = bank_ATOFHits.getFloat("x", row); + double y = bank_ATOFHits.getFloat("y", row); + double z = bank_ATOFHits.getFloat("z", row); + z += atof_alignement; // there is a shift between AHDC and ATOF (still don't know why) ! + RadialKFHit hit = new RadialKFHit(x, y, z); + // error on r + double wedge_width = 20; //mm + double dr2 = Math.pow(wedge_width, 2)/12; // mm^2 + // error on phi + double open_angle = Math.toRadians(6); // deg + double dphi2 = Math.pow(open_angle, 2)/12; + // error on z + double wedge_length = 27.7; //mm + double dz2 = Math.pow(wedge_length, 2)/12; + + RealMatrix measurementNoise = new Array2DRowRealMatrix( + new double[][]{ + {dr2, 0.0000, 0.0000}, + {0.00, dphi2, 0.0000}, + {0.00, 0.0000, dz2} + });//3x3; + hit.setMeasurementNoise(measurementNoise); + map_ATOF_hits.put(trackid, hit); + } + } + } + } + + /// Associate the ATOF hits to each track + for (Track track : AHDC_tracks) { + RadialKFHit hit = map_ATOF_hits.get(track.get_trackId()); + ArrayList list = new ArrayList<>(); + if (hit != null) list.add(hit); // for now, we only consider one hit in the ATOF + track.setATOFHits(list); + } + + /// Intialise the Kalman Filter double magfieldfactor = runBank.getFloat("solenoid", 0); double magfield = 50*magfieldfactor; - boolean IsMC = event.hasBank("MC::Particle"); PDGParticle proton = PDGDatabase.getParticleById(2212); int Niter = 40; KalmanFilter KF = new KalmanFilter(proton, Niter); - /////////////////////////////////////////////////////// - // first propagation : each AHDC_tracks will be fitted - /////////////////////////////////////////////////////// - KF.propagation(AHDC_tracks, event, magfield, IsMC); - ///////////////////////////////////////////// - // write the AHDC::kftrack bank in the event - ///////////////////////////////////////////// + KF.set_ATOF_detector(ATOF); // Reference the ATOF geometry in the Kalman Filter + KF.set_clas_alignement(clas_alignement); + KF.set_atof_alignement(atof_alignement); + KF.set_vz_constraint(vz_constraint); + KF.set_vertex_flag(IsVertexDefined); + + /// Do a first propagation + KF.propagation(AHDC_tracks, magfield, IsMC); + + // // Get ATOF wedges predicted + // HashMap> ATOF_hits_predicted = KF.get_ATOF_hits_predicted(); + // for (Track track : AHDC_tracks) { + // int trackid = track.get_trackId(); + // // if the AI has no match for this track, look for the predicted ones + // if (ATOF_hits.get(trackid) == null) { + // ArrayList predicted_wedges = ATOF_hits_predicted.get(trackid); + // boolean IsHitSelected = false; + // int i = 0; + // while (i < predicted_wedges.size() && !IsHitSelected) { + // int sector = predicted_wedges.get(i)[0]; + // int layer = predicted_wedges.get(i)[1]; + // int wedge = predicted_wedges.get(i)[2]; + // for (int row = 0; row < bank_ATOFHits.rows(); row++) { + // if (bank_ATOFHits.getInt("sector", row) == sector && bank_ATOFHits.getInt("layer", row) == layer && bank_ATOFHits.getInt("component", row) == wedge) { + // // create a RadialKFHit + // double x = bank_ATOFHits.getFloat("x", row); + // double y = bank_ATOFHits.getFloat("y", row); + // double z = bank_ATOFHits.getFloat("z", row); + // z -= 32.3; // there is a shift between AHDC and ATOF (still don't know why) ! + // RadialKFHit hit = new RadialKFHit(x, y, z); + // // error on r + // double wedge_width = 20; //mm + // double dr2 = Math.pow(wedge_width, 2)/12; // mm^2 + // // error on phi + // double open_angle = Math.toRadians(6); // deg + // double dphi2 = Math.pow(open_angle, 2)/12; + // // error on z + // double wedge_length = 27.7; //mm + // double dz2 = Math.pow(wedge_length, 2)/12; + + // RealMatrix measurementNoise = new Array2DRowRealMatrix( + // new double[][]{ + // {dr2, 0.0000, 0.0000}, + // {0.00, dphi2, 0.0000}, + // {0.00, 0.0000, dz2} + // });//3x3; + // hit.setMeasurementNoise(measurementNoise); + // ATOF_hits.put(trackid, hit); + + // IsHitSelected = true; + // break; + // } + // } // end loop over ATOF bank rows + // } //end loop over predicted ATOF hits + // } // end if + // } // end loop over tracks + + /// Clean AHDC bad hits + double sigma = 0.5; // mm + for (Track track : AHDC_tracks) { + ArrayList AHDC_hits = track.getHits(); + Iterator it = AHDC_hits.iterator(); + while (it.hasNext()) { + Hit hit = it.next(); + if (Math.abs(hit.getResidual()) > 3*sigma) { + it.remove(); + } + } + } + + /// Second propagation : each AHDC_tracks will be fitted + KF.set_Niter(15); + KF.propagation(AHDC_tracks, magfield, IsMC); + + /// write the AHDC::kftrack bank in the event org.jlab.rec.ahdc.Banks.RecoBankWriter ahdc_writer = new org.jlab.rec.ahdc.Banks.RecoBankWriter(); DataBank recoKFTracksBank = ahdc_writer.fillAHDCKFTrackBank(event, AHDC_tracks); event.appendBank(recoKFTracksBank); @@ -384,7 +558,7 @@ public boolean processDataEvent(DataEvent event) { AHDC_hits.addAll(track.getHits()); } DataBank recoKFHitsBank = ahdc_writer.fillAHDCHitsBank(event, AHDC_hits); - event.appendBank(recoKFHitsBank); // remark: only hits assocuated to a track are saved + event.appendBank(recoKFHitsBank); // remark: only hits associated to a track are saved return true;