diff --git a/etc/bankdefs/hipo4/alert.json b/etc/bankdefs/hipo4/alert.json index 611babce13..7999d13c2b 100644 --- a/etc/bankdefs/hipo4/alert.json +++ b/etc/bankdefs/hipo4/alert.json @@ -181,6 +181,14 @@ "name": "Doca", "type": "D", "info": "distance od closest approch (mm)" + }, { + "name": "residual", + "type": "D", + "info": "residual (mm)" + }, { + "name": "residual_prefit", + "type": "D", + "info": "residual pre-fit (mm)" } ] }, { 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 72b9b5f2fe..16e880dcb2 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 @@ -24,6 +24,8 @@ public DataBank fillAHDCHitsBank(DataEvent event, ArrayList hitList) { bank.setByte("superlayer", i, (byte) hitList.get(i).getSuperLayerId()); bank.setInt("wire", i, hitList.get(i).getWireId()); bank.setDouble("Doca", i, hitList.get(i).getDoca()); + bank.setDouble("residual", i, hitList.get(i).getResidual()); + bank.setDouble("residual_prefit", i, hitList.get(i).getResidualPrefit()); } return bank; 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 6ee4bab6a0..9182365273 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 @@ -9,6 +9,7 @@ public class Hit implements Comparable { private final int layerId; private final int wireId; private final double doca; + private final double adc; private double phi; private double radius; @@ -16,14 +17,20 @@ public class Hit implements Comparable { private boolean use = false; private double x; private double y; + private double residual_prefit; + private double residual; - public Hit(int _Id, int _Super_layer, int _Layer, int _Wire, double _Doca) { + //updated constructor with ADC + public Hit(int _Id, int _Super_layer, int _Layer, int _Wire, double _Doca, double _ADC) { this.id = _Id; this.superLayerId = _Super_layer; this.layerId = _Layer; this.wireId = _Wire; this.doca = _Doca; + this.adc = _ADC; wirePosition(); + this.residual_prefit = 0.0; + this.residual = 0.0; } private void wirePosition() { @@ -130,4 +137,22 @@ public double getY() { } public double getPhi() {return phi;} + + public double getADC() {return adc;} + + public double getResidual() { + return residual; + } + + public double getResidualPrefit() { + return residual_prefit; + } + + public void setResidual(double resid) { + this.residual = resid; + } + + public void setResidualPrefit(double resid) { + this.residual_prefit = resid; + } } 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 cef052afef..91e7a1daad 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 @@ -32,9 +32,10 @@ public void fetch_AHDCHits(DataEvent event) { int layer = number % 10; int superlayer = (int) (number % 100) / 10; int wire = bankDGTZ.getShort("component", i); + double adc = bankDGTZ.getInt("ADC", i); double doca = bankDGTZ.getShort("ped", i) / 1000.0; - hits.add(new Hit(id, superlayer, layer, wire, doca)); + hits.add(new Hit(id, superlayer, layer, wire, doca, adc)); } } this.set_AHDCHits(hits); @@ -75,4 +76,4 @@ public void set_TrueAHDCHits(ArrayList _TrueAHDCHits) { this._TrueAHDCHits = _TrueAHDCHits; } -} \ No newline at end of file +} 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 index fa4d1dc477..a07e4299df 100644 --- 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 @@ -19,13 +19,17 @@ public class Hit implements Comparable { private final double r; private final double phi; private final double doca; - private final double adc; + private double adc; private final double numWires; private final Line3D line3D; - - // 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 + 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, double r, double doca) { this.superLayer = superLayer; this.layer = layer; @@ -34,6 +38,8 @@ public Hit(int superLayer, int layer, int wire, int numWire, double r, double do this.doca = doca; this.numWires = numWire; this.adc = 0;//placeholder + this.hitidx = -1; + this.hitsign = 0; final double DR_layer = 4.0;//OK final double round = 360.0;//OK @@ -104,42 +110,65 @@ public Hit(int superLayer, int layer, int wire, int numWire, double r, double do Line3D wireLine = new Line3D(lPoint, rPoint); //wireLine.show(); this.line3D = wireLine; - } - - //hit measurement vector in cylindrical coordinates: r, phi, z - public RealVector get_Vector() { - // final double costhster = Math.cos(thster); - // final double sinthster = Math.cos(thster); - RealVector wire_meas = new ArrayRealVector(new double[]{this.r(), this.phi(), 0}); - // Array2DRowRealMatrix stereo_rotation = new Array2DRowRealMatrix(new double[][]{{1, 0.0, 0.0}, {0, costhster, -sinthster}, {0, sinthster, costhster}});//rotation of wire: needed? - return wire_meas;//.multiply(stereo_rotation); + + //calculate the "virtual" left and right wires accounting for the DOCA + double deltaphi = Math.asin(this.doca/R_layer); + double wx_plus = -R_layer * Math.sin( alphaW_layer * (this.wire-1) - deltaphi );//OK + double wy_plus = -R_layer * Math.cos( alphaW_layer * (this.wire-1) - deltaphi );//OK + + double wx_plus_end = -R_layer * Math.sin( alphaW_layer * (this.wire-1) + thster * (Math.pow(-1, this.superLayer-1)) - deltaphi );//OK + double wy_plus_end = -R_layer * Math.cos( alphaW_layer * (this.wire-1) + thster * (Math.pow(-1, this.superLayer-1)) - deltaphi );//OK + + line = new Line3D(wx_plus, wy_plus, -zl/2, wx_plus_end, wy_plus_end, zl/2); + lPoint = new Point3D(); + rPoint = new Point3D(); + lPlane.intersection(line, lPoint); + rPlane.intersection(line, rPoint); + + wireLine = new Line3D(lPoint, rPoint); + this.line3D_plus = wireLine; + + double wx_minus = -R_layer * Math.sin( alphaW_layer * (this.wire-1) + deltaphi );//OK + double wy_minus = -R_layer * Math.cos( alphaW_layer * (this.wire-1) + deltaphi );//OK + + double wx_minus_end = -R_layer * Math.sin( alphaW_layer * (this.wire-1) + thster * (Math.pow(-1, this.superLayer-1)) + deltaphi );//OK + double wy_minus_end = -R_layer * Math.cos( alphaW_layer * (this.wire-1) + thster * (Math.pow(-1, this.superLayer-1)) + deltaphi );//OK + + line = new Line3D(wx_minus, wy_minus, -zl/2, wx_minus_end, wy_minus_end, zl/2); + lPoint = new Point3D(); + rPoint = new Point3D(); + lPlane.intersection(line, lPoint); + rPlane.intersection(line, rPoint); + + wireLine = new Line3D(lPoint, rPoint); + this.line3D_minus = wireLine; + } - //hit measurement vector in 1 dimension: minimize distance - doca - public RealVector get_Vector_simple() { + //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: minimize distance - doca - adds hit "sign" - public RealVector get_Vector_sign(int sign) { - // Attempt: multiply doca by sign - return new ArrayRealVector(new double[]{sign*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() { - final double costhster = Math.cos(thster); - final double sinthster = Math.cos(thster); - //dR = 0.1m dphi = pi dz = L/2 - Array2DRowRealMatrix wire_noise = new Array2DRowRealMatrix(new double[][]{{0.1, 0.0, 0.0}, {0.0, Math.atan(0.1/this.r), 0.0}, {0.0, 0.0, 150.0/costhster}});//uncertainty matrix in wire coordinates - Array2DRowRealMatrix stereo_rotation = new Array2DRowRealMatrix(new double[][]{{1, 0.0, 0.0}, {0, costhster, -sinthster}, {0, sinthster, costhster}});//rotation of wire - wire_noise.multiply(stereo_rotation); - - return wire_noise.multiply(wire_noise); - // + public RealMatrix get_MeasurementNoise() { + return new Array2DRowRealMatrix(new double[][]{{0.0225}}); } - - public RealMatrix get_MeasurementNoise_simple() { - return new Array2DRowRealMatrix(new double[][]{{0.01}}); + + 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() { @@ -151,8 +180,6 @@ public double doca() { public double phi() {return phi;}//at z = 0; public double phi(double z) { - // double x_0 = r*sin(phi); - // double y_0 = r*cos(phi); double x_z = r*Math.sin( phi + thster * z/(zl*0.5) * (Math.pow(-1, this.superLayer-1)) ); double y_z = r*Math.cos( phi + thster * z/(zl*0.5) * (Math.pow(-1, this.superLayer-1)) ); return Math.atan2(x_z, y_z); @@ -161,10 +188,16 @@ public double phi(double z) { public Line3D line() {return line3D;} public double distance(Point3D point3D) { - //System.out.println("Calculating distance: "); - //this.line3D.show(); - //point3D.show(); - //System.out.println(" d = " + this.line3D.distance(point3D).length()); + 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(); } @@ -211,6 +244,10 @@ public double getADC() { return adc; } + public void setADC(double _adc) { + this.adc = _adc; + } + public Line3D getLine3D() { return line3D; } @@ -218,5 +255,22 @@ public Line3D getLine3D() { 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 e1160642e4..d3221b2939 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 @@ -9,7 +9,7 @@ public class Hit_beam extends Hit { double r,phi; public Hit_beam(int superLayer, int layer, int wire, int numWire, double doca, double x, double y , double z) { - super(0, 0, 0, 0, Math.hypot(x,y), 0); + super(0, 0, 0, 0, Math.hypot(x,y), 0); this.x = x; this.y = y; this.z = z; 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 3feddf5b27..1a184bf5dc 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 @@ -17,7 +17,7 @@ public class KFitter { // 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) { this.stateEstimation = initialStateEstimate; @@ -73,39 +73,39 @@ public void predict(Indicator indicator) throws Exception { } public void correct(Indicator indicator) { - //System.out.println(" state before: (" + stateEstimation.getEntry(0) + ", " + stateEstimation.getEntry(1) + ", " + stateEstimation.getEntry(2) + ", " + stateEstimation.getEntry(3) + ", " + stateEstimation.getEntry(4) + ", " + stateEstimation.getEntry(5) + ");" ); - //System.out.println(" state radius before: " + Math.sqrt( Math.pow(stateEstimation.getEntry(0), 2) + Math.pow(stateEstimation.getEntry(1), 2) ) ); RealVector z, z_plus, z_minus; RealMatrix measurementNoise; RealMatrix measurementMatrix; RealVector h; if (indicator.R == 0.0 && !indicator.direction) { + double z_beam_res_sq = 1.e10;//in mm + if(isvertexdefined)z_beam_res_sq = 4.0;//assuming 2. mm resolution measurementNoise = new Array2DRowRealMatrix( new double[][]{ - // {9.00, 0.0000, 0.0000}, - // {0.00, 1e10, 0.0000}, - // {0.00, 0.0000, 1e10} {0.09, 0.0000, 0.0000}, - {0.00, 1.e10, 0.0000}, - {0.00, 0.0000, 1.e10} + {0.00, 1e10, 0.0000}, + {0.00, 0.0000, z_beam_res_sq} });//3x3 measurementMatrix = H_beam(stateEstimation);//6x3 h = h_beam(stateEstimation);//3x1 z = indicator.hit.get_Vector_beam();//0! } else { - measurementNoise = indicator.hit.get_MeasurementNoise_simple();//1x1 - measurementMatrix = H_simple(stateEstimation, indicator);//6x1 - h = h_simple(stateEstimation, indicator);//.multiply(wire_sign_mat(indicator));//1x1 - z = indicator.hit.get_Vector_simple();//1x1 - - // measurementNoise = indicator.hit.get_MeasurementNoise();//3x3 - // measurementMatrix = H(stateEstimation, indicator);//6x3 - // h = h(stateEstimation, indicator);//3x1 - // z = indicator.hit.get_Vector();//3x1 - - //System.out.println(" h: r " + h.getEntry(0) + " phi " + h.getEntry(1) + " h z " + h.getEntry(2) + " z: r " + z.getEntry(0) + " phi " + z.getEntry(1) + " z " + z.getEntry(2) ); - + //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 } RealMatrix measurementMatrixT = measurementMatrix.transpose(); @@ -128,7 +128,6 @@ public void correct(Indicator indicator) { RealMatrix tmpMatrix = identity.subtract(kalmanGain.multiply(measurementMatrix)); errorCovariance = tmpMatrix.multiply(errorCovariance.multiply(tmpMatrix.transpose())).add(kalmanGain.multiply(measurementNoise.multiply(kalmanGain.transpose()))); - //System.out.println(" state after: (" + stateEstimation.getEntry(0) + ", " + stateEstimation.getEntry(1) + ", " + stateEstimation.getEntry(2) + ", " + stateEstimation.getEntry(3) + ", " + stateEstimation.getEntry(4) + ", " + stateEstimation.getEntry(5) + ");" ); // Give back to the stepper the new stateEstimation stepper.y = stateEstimation.toArray(); } @@ -138,20 +137,21 @@ public double residual(Indicator indicator) { return indicator.hit.doca()-d; } - public double wire_sign(Indicator indicator) {//let's decide: positive when (phi state - phi wire) > 0 + //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)); - //System.out.println(" phi state " + phi_state + " phi wire " + phi_wire);// + " phi state alt? " + Math.atan2(stateEstimation.getEntry(1), stateEstimation.getEntry(0))); - return (phi_state-phi_wire)/Math.abs(phi_state-phi_wire) ; + if( (phi_state-phi_wire)/Math.abs(phi_state-phi_wire)>0 ){ + return +1; + }else{ + return -1; + } } - // public RealMatrix wire_sign_mat(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)); - // System.out.println(" phi state " + phi_state + " phi wire " + phi_wire);// + " phi state alt? " + Math.atan2(stateEstimation.getEntry(1), stateEstimation.getEntry(0))); - // return MatrixUtils.createRealMatrix(new double[][]{{(phi_state-phi_wire)/Math.abs(phi_state-phi_wire)}}); - // } - + public void ResetErrorCovariance(final RealMatrix initialErrorCovariance){ + this.errorCovariance = initialErrorCovariance; + } + private RealMatrix F(Indicator indicator, Stepper stepper1) throws Exception { double[] dfdx = subfunctionF(indicator, stepper1, 0); @@ -166,7 +166,7 @@ private RealMatrix F(Indicator indicator, Stepper stepper1) throws Exception { } double[] subfunctionF(Indicator indicator, Stepper stepper1, int i) throws Exception { - double h = 1e-8; + double h = 1e-8;// in mm Stepper stepper_plus = new Stepper(stepper1.y); Stepper stepper_minus = new Stepper(stepper1.y); @@ -189,57 +189,20 @@ private RealMatrix F(Indicator indicator, Stepper stepper1) throws Exception { return new double[]{dxdi, dydi, dzdi, dpxdi, dpydi, dpzdi}; } - //measurement matrix in cylindrical coordinates: r, phi, z + //measurement matrix in 1 dimension: minimize distance - doca private RealVector h(RealVector x, Indicator indicator) { - //As per my understanding: d -> r wire; phi -> phi wire, z unconstrained - double xx = x.getEntry(0); - double yy = x.getEntry(1); - return MatrixUtils.createRealVector(new double[]{Math.hypot(xx, yy), Math.atan2(yy, xx), x.getEntry(2)}); - } - - //measurement matrix in 1 dimension: minimize distance - doca - private RealVector h_simple(RealVector x, Indicator indicator) { double d = indicator.hit.distance(new Point3D(x.getEntry(0), x.getEntry(1), x.getEntry(2))); - return MatrixUtils.createRealVector(new double[]{d});//would need to have this 3x3 + //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}); } - //measurement noise matrix in cylindrical coordinates: r, phi, z - private RealMatrix H(RealVector x, Indicator indicator) { - // dphi/dx - 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} - }); + 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); + return MatrixUtils.createRealVector(new double[]{d}); } - //measurement matrix in 1 dimension: minimize distance - doca - private RealMatrix H_simple(RealVector x, Indicator indicator) { + //measurement matrix in 1 dimension: minimize distance - doca + private RealMatrix H(RealVector x, Indicator indicator) { double ddocadx = subfunctionH(x, indicator, 0); double ddocady = subfunctionH(x, indicator, 1); @@ -254,15 +217,44 @@ private RealMatrix H_simple(RealVector x, Indicator indicator) { } double subfunctionH(RealVector x, Indicator indicator, int i) { - double h = 1e-8; + 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_simple(x_plus, indicator).getEntry(0); - double doca_minus = h_simple(x_minus, indicator).getEntry(0); + double doca_plus = h(x_plus, indicator).getEntry(0); + double doca_minus = h(x_minus, indicator).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); } @@ -332,4 +324,6 @@ 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 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 2cd0bc5593..85d23fedef 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 @@ -31,46 +31,42 @@ public class KalmanFilter { - public KalmanFilter(ArrayList tracks, DataEvent event) {propagation(tracks, event);} + public KalmanFilter(ArrayList tracks, DataEvent event, boolean IsMC) {propagation(tracks, event, IsMC);} - private void propagation(ArrayList tracks, DataEvent event) { + private final int Niter = 10; + private final boolean IsVtxDefined = false; + + private void propagation(ArrayList tracks, DataEvent event, boolean IsMC) { try { - //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)); + double vz_constraint; + 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; } - - /* - Writer hitsWriter = new FileWriter("hits.dat"); - for (Point3D p : sim_hits) { - hitsWriter.write("" + p.x() + ", " + p.y() + ", " + p.z() + '\n'); - } - hitsWriter.close(); - */ - - // Initialization --------------------------------------------------------------------- final double magfield = +50; final PDGParticle proton = PDGDatabase.getParticleById(2212); @@ -90,46 +86,62 @@ private void propagation(ArrayList tracks, DataEvent event) { //final double py0 = tracks.get(0).get_py(); final double pz0 = tracks.get(0).get_pz(); - final double p_init = java.lang.Math.sqrt(px0*px0+py0*py0+pz0*pz0); + //final double p_init = java.lang.Math.sqrt(px0*px0+py0*py0+pz0*pz0); double[] y = new double[]{x0, y0, z0, px0, py0, pz0}; - //System.out.println("y = " + x0 + ", " + y0 + ", " + z0 + ", " + px0 + ", " + py0 + ", " + pz0 + "; p = " + p_init); // EPAF: *the line below is for TEST ONLY!!!* //double[] y = new double[]{vxmc, vymc, vzmc, pxmc, pymc, pzmc}; - //System.out.println("y = " + vxmc + ", " + vymc + ", " + vzmc + ", " + pxmc + ", " + pymc + ", " + pzmc + "; p = " + java.lang.Math.sqrt(pxmc*pxmc+pymc*pymc+pzmc*pzmc)); - // Initialization hit - //System.out.println("tracks = " + tracks); ArrayList AHDC_hits = tracks.get(0).getHits(); ArrayList KF_hits = new ArrayList<>(); + //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()); for (org.jlab.rec.ahdc.Hit.Hit AHDC_hit : AHDC_hits) { - //System.out.println("Superlayer = " + AHDC_hit.getSuperLayerId() + ", Layer " + AHDC_hit.getLayerId() + ", Wire " + AHDC_hit.getWireId() + ", Nwires " + AHDC_hit.getNbOfWires() + ", Radius " + AHDC_hit.getRadius() + ", DOCA " + AHDC_hit.getDoca()); Hit hit = new Hit(AHDC_hit.getSuperLayerId(), AHDC_hit.getLayerId(), AHDC_hit.getWireId(), AHDC_hit.getNbOfWires(), AHDC_hit.getRadius(), AHDC_hit.getDoca()); - + hit.setADC(AHDC_hit.getADC()); + hit.setHitIdx(AHDC_hit.getId()); + hit.setSign(0); + //System.out.println( " r = " + hit.r() + " hit.phi " + hit.phi() +" hit.doca = " + hit.getDoca() ); // Do delete hit with same radius - // boolean aleardyHaveR = false; - // for (Hit o: KF_hits){ - // if (o.r() == hit.r()){ - // aleardyHaveR = true; - // } + 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); // } - // if (!aleardyHaveR) - KF_hits.add(hit); - } - - - /* - Writer hitsWiresWriter = new FileWriter("hits_wires.dat"); - for (Hit h : KF_hits) { - hitsWiresWriter.write("" + h.getSuperLayer() + ", " + h.getLayer() + ", " + h.getWire() + ", " + h.getDoca() + ", " + h.getNumWires() + ", " + h.getR() + '\n'); } - hitsWiresWriter.close(); - */ - - //System.out.println("KF_hits = " + KF_hits); + double zbeam = 0; + if(IsVtxDefined)zbeam = vz_constraint;//test final ArrayList forwardIndicators = forwardIndicators(KF_hits, materialHashMap); - final ArrayList backwardIndicators = backwardIndicators(KF_hits, materialHashMap); - + final ArrayList backwardIndicators = backwardIndicators(KF_hits, materialHashMap, zbeam); + // Start propagation Stepper stepper = new Stepper(y); RungeKutta4 RK4 = new RungeKutta4(proton, numberOfVariables, B); @@ -141,87 +153,48 @@ private void propagation(ArrayList tracks, DataEvent event) { 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 kFitter = new KFitter(initialStateEstimate, initialErrorCovariance, stepper, propagator); - - /* - Stepper stepper_fisrt = new Stepper(y); - Writer writer_first = new FileWriter("track_first.dat"); - for (Indicator indicator : forwardIndicators) { - stepper_fisrt.initialize(indicator); - propagator.propagateAndWrite(stepper_fisrt, indicator, writer_first); - } - writer_first.close(); - - - - System.out.println("--------- BackWard propagation !! ---------"); - - Writer writer_back = new FileWriter("track_back.dat"); - for (Indicator indicator : backwardIndicators) { - stepper.initialize(indicator); - propagator.propagateAndWrite(stepper, indicator, writer_back); - } - writer_back.close(); - */ - - //Print out hit residuals *before* fit: - // for (Indicator indicator : forwardIndicators) { - // kFitter.predict(indicator); - // if (indicator.haveAHit()) { - // System.out.println(" Pre-fit: indicator R " + indicator.R + "; y = " + kFitter.getStateEstimationVector() + " p = " + kFitter.getMomentum() + " residual: " + kFitter.residual(indicator) + " sign " + kFitter.wire_sign(indicator) ); - // } - // } - - for (int k = 0; k < 10; k++) { - - //System.out.println("--------- ForWard propagation !! ---------"); - + kFitter.setVertexDefined(IsVtxDefined); + + for (int k = 0; k < Niter; k++) { + //System.out.println("--------- ForWard propagation !! ---------"); + //Reset error covariance: + //kFitter.ResetErrorCovariance(initialErrorCovariance); for (Indicator indicator : forwardIndicators) { kFitter.predict(indicator); - //System.out.println("indicator R " + indicator.R + " h " + indicator.h + "; y = " + kFitter.getStateEstimationVector() + " p = " + kFitter.getMomentum()); if (indicator.haveAHit()) { - //System.out.println("Superlayer = " + indicator.hit.getSuperLayer() + ", Layer " + indicator.hit.getLayer() + ", Wire " + indicator.hit.getWire() + ", Nwires " + indicator.hit.getNumWires() + ", Radius " + indicator.hit.getR() + ", DOCA " + indicator.hit.getDoca()); - kFitter.correct(indicator); - //System.out.println("y = " + kFitter.getStateEstimationVector() + " p = " + kFitter.getMomentum()); + 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(kFitter.residual(indicator)); + } + } + kFitter.correct(indicator); } } //System.out.println("--------- BackWard propagation !! ---------"); - for (Indicator indicator : backwardIndicators) { kFitter.predict(indicator); - //System.out.println("indicator R " + indicator.R + " h " + indicator.h + "; y = " + kFitter.getStateEstimationVector() + " p = " + kFitter.getMomentum()); if (indicator.haveAHit()) { - //System.out.println("Superlayer = " + indicator.hit.getSuperLayer() + ", Layer " + indicator.hit.getLayer() + ", Wire " + indicator.hit.getWire() + ", Nwires " + indicator.hit.getNumWires() + ", Radius " + indicator.hit.getR() + ", DOCA " + indicator.hit.getDoca()); kFitter.correct(indicator); - //System.out.println("y = " + kFitter.getStateEstimationVector() + " p = " + kFitter.getMomentum()); } } } - // //Print out residuals *after* fit: - // for (Indicator indicator : forwardIndicators) { - // kFitter.predict(indicator); - // if (indicator.haveAHit()) { - // System.out.println(" Post-fit: indicator R " + indicator.R + "; y = " + kFitter.getStateEstimationVector() + " p = " + kFitter.getMomentum() + " residual: " + kFitter.residual(indicator) + " sign " + kFitter.wire_sign(indicator) ); - // } - // } - - /* - Writer writer_last = new FileWriter("track_last.dat"); - for (Indicator indicator : forwardIndicators) { - stepper.initialize(indicator); - propagator.propagateAndWrite(stepper, indicator, writer_last); - } - writer_last.close(); - */ - - RealVector x_out = kFitter.getStateEstimationVector(); tracks.get(0).setPositionAndMomentumForKF(x_out); - //System.out.println("y_final = " + x_out + " p_final = " + kFitter.getMomentum()); + //Residual calcuation post fit: + for (Indicator indicator : forwardIndicators) { + kFitter.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(kFitter.residual(indicator)); + } + } + } + } } catch (Exception e) { // e.printStackTrace(); } @@ -301,4 +274,17 @@ ArrayList backwardIndicators(ArrayList hitArrayList, HashMap 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/Track/Track.java b/reconstruction/alert/src/main/java/org/jlab/rec/ahdc/Track/Track.java index 8a968ba512..1e9ace5a68 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 @@ -15,7 +15,6 @@ public class Track { private List _Clusters = new ArrayList<>(); private boolean _Used = false; private final ArrayList hits = new ArrayList<>(); - ; private double x0 = 0; private double y0 = 0; @@ -64,6 +63,15 @@ public void setPositionAndMomentum(HelixFitObject helixFitObject) { this.pz0 = helixFitObject.get_pz(); } + public void setPositionAndMomentumVec(double[] x) { + this.x0 = x[0]; + this.y0 = x[1]; + this.z0 = x[2]; + this.px0 = x[3]; + this.py0 = x[4]; + this.pz0 = x[5]; + } + public void setPositionAndMomentumForKF(RealVector x) { this.x0_kf = x.getEntry(0); this.y0_kf = x.getEntry(1); diff --git a/reconstruction/alert/src/main/java/org/jlab/rec/service/AHDCEngine.java b/reconstruction/alert/src/main/java/org/jlab/rec/service/AHDCEngine.java index 9c118a09a2..d57a6b2bec 100644 --- a/reconstruction/alert/src/main/java/org/jlab/rec/service/AHDCEngine.java +++ b/reconstruction/alert/src/main/java/org/jlab/rec/service/AHDCEngine.java @@ -91,7 +91,9 @@ public boolean processDataEvent(DataEvent event) { HitReader hitRead = new HitReader(event, simulation); ArrayList AHDC_Hits = hitRead.get_AHDCHits(); - ArrayList TrueAHDC_Hits = hitRead.get_TrueAHDCHits(); + if(simulation){ + ArrayList TrueAHDC_Hits = hitRead.get_TrueAHDCHits(); + } //System.out.println("AHDC_Hits size " + AHDC_Hits.size()); // II) Create PreCluster @@ -183,12 +185,15 @@ public int compare(Hit a1, Hit a2) { HelixFitJava h = new HelixFitJava(); track.setPositionAndMomentum(h.HelixFit(nbOfPoints, szPos, 1)); + // double p = 150.0;//MeV/c + // double phi = Math.atan2(szPos[0][1], szPos[0][0]); + // double x_0[] = {0.0, 0.0, 0.0, p*Math.sin(phi), p*Math.cos(phi), 0.0}; + // track.setPositionAndMomentumVec(x_0); } // VI) Kalman Filter // System.out.println("AHDC_Tracks = " + AHDC_Tracks); - KalmanFilter kalmanFitter = new KalmanFilter(AHDC_Tracks, event); - + KalmanFilter kalmanFitter = new KalmanFilter(AHDC_Tracks, event, simulation); // VII) Write bank RecoBankWriter writer = new RecoBankWriter();