Clean bad AHDC hits and use ATOF wedges in the Kalman Filter (#1309)

* extends the bank AHDC::kftrack and allow re-cooking of AHDC only

* convenient use of PID in RK4

* create KFHit and refactor the kalman filter

* activate atof hit

* rename methods

* remove unused variables and methods

* use a logger

* fix innovation vector for RadialKFHit

* cleaning

* let registerOutputBank removes the banks

Author: ftouchte

etc/bankdefs/hipo4/alert.json

@@ -408,6 +408,74 @@
         "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_match",
+      "type": "B",
+      "info": "1 if there is a ATOF match, 0 if not"
       }
     ]
   }, {

reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Banks/RecoBankWriter.java

@@ -7,6 +7,7 @@
 import org.jlab.rec.ahdc.AHDCCluster.AHDCCluster;
 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.alert.Track.Track;
 
@@ -153,6 +154,39 @@ public DataBank fillAHDCKFTrackBank(DataEvent event, ArrayList<Track> 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());
+			bank.setByte("atof_match", row, (byte) ((track.getATOFHits().size() > 0) ? 1 : 0));
+
 			row++;
 		}
 

reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Hit/Hit.java

@@ -2,15 +2,17 @@
 
 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;
 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;
+import org.jlab.rec.ahdc.KalmanFilter.KFHit;
 
-public class Hit implements Comparable<Hit> {
+public class Hit implements Comparable<Hit>, KFHit {
 
 	private final int    id;
 	private final int    superLayerId;
@@ -33,6 +35,16 @@ public class Hit implements Comparable<Hit> {
 	private int	trackId;
 
     //updated constructor with ADC
+	/**
+	 * 
+	 * @param _Id is AHDC::adc row id + 1
+	 * @param _Super_layer super layer id
+	 * @param _Layer layer id
+	 * @param _Wire wire id 
+	 * @param _Doca distance from the timing information using the time2distance, matches {@link Hit.#time}
+	 * @param _ADC raw ADC
+	 * @param _Time calibrated time
+	 */
 	public Hit(int _Id, int _Super_layer, int _Layer, int _Wire, double _Doca, double _ADC, double _Time) {
 		this.id           = _Id;
 		this.superLayerId = _Super_layer;
@@ -110,6 +122,7 @@ public Line3D getLine() {
         return wireLine;
     }
 
+	@Override
     public double getRadius() {
         return radius;
     }
@@ -136,6 +149,7 @@ public double getY() {
 
 	public double getPhi() {return phi;}
 
+	/** Get calibrated ADC */
 	public double getADC() {return adc;}
 
 	public double getResidual() {
@@ -146,18 +160,22 @@ public void setResidual(double resid) {
 		this.residual = resid;
 	}
 
+	/** Set calibrated ToT */
 	public void setToT(double _tot) {
 		this.tot = _tot;
 	}
 
+	/** Get calibrated ToT */
 	public double getToT() {
 		return tot;
 	}
 
+	/** Set calibrated ADC */
 	public void setADC(double _adc) {
 		this.adc = _adc;
 	}
 
+	/** Get raw ADC */
 	public double getRawADC() {
 		return raw_adc;
 	}
@@ -174,11 +192,13 @@ public void setTrackId(int _trackId) {
 		this.trackId = _trackId;
 	}
 
-	public RealVector get_Vector() {
+	@Override
+	public RealVector getMeasurementVector() {
 		return new ArrayRealVector(new double[]{this.doca});
 	}
 
-    public RealMatrix get_MeasurementNoise() {
+	@Override
+    public RealMatrix getMeasurementNoiseMatrix() {
 		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 +208,50 @@ 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 getProjectionVector(RealVector x) {
+		double d = this.distance(new Point3D(x.getEntry(0), x.getEntry(1), x.getEntry(2)));
+		return MatrixUtils.createRealVector(new double[]{d});
 	}
 
+	@Override
+	public RealVector getInnovationVector(RealVector x) {
+		RealVector measured = getMeasurementVector();
+		RealVector predicted = getProjectionVector(x);
+		return measured.subtract(predicted);
+	}
+
+	// Jacobian matrix of the measurement with respect to (x, y, z, px, py, pz)
+	@Override
+	public RealMatrix getProjectionMatrix(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.getProjectionVector(x_plus).getEntry(0);
+		double doca_minus = this.getProjectionVector(x_minus).getEntry(0);
+
+		return (doca_plus - doca_minus) / (2 * h);
+	}
+
+	@Override
 	public double distance(Point3D point3D) {
 		return this.wireLine.distance(point3D).length();
 	}

reconstruction/alert/src/main/java/org/jlab/rec/ahdc/KalmanFilter/Hit_beam.java

@@ -0,0 +1,29 @@
+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 getMeasurementVector();
+	/** Return the measurement noise matrix for this this KFHit */
+	public RealMatrix getMeasurementNoiseMatrix();
+	/** Compute the measurement for a given state vector */
+	public RealVector getProjectionVector(RealVector x);
+	/** Compute the Jacobian matrix of the {@link #getProjectionVector(RealVector)} with respect of the components of the state vector */
+	public RealMatrix getProjectionMatrix(RealVector x);
+
+	/**
+	 * Compute the innovation by subtracting {@link #getMeasurementVector()} and {@link #getProjectionVector(RealVector)} 
+	 * @param x current state estimation
+	 * @return return the innovation vector
+	 */
+	public RealVector getInnovationVector(RealVector x);
+}