Skip to content

Commit 4d2395b

Browse files
tongtongcaotongtongcao
committed
update calculated doca in KF processing (#1108)
1 parent 2061e91 commit 4d2395b

File tree

4 files changed

+105
-47
lines changed

4 files changed

+105
-47
lines changed

‎common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AMeasVecs.java‎

Lines changed: 15 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
import org.jlab.geom.prim.Line3D;
1111
import org.jlab.geom.prim.Point3D;
1212
import org.jlab.geom.prim.Transformation3D;
13+
import org.jlab.geom.prim.Vector3D;
1314

1415
/**
1516
*
@@ -57,7 +58,9 @@ public double[] dhDoca(int k, StateVec stateVec) {
5758

5859
Surface surf = this.measurements.get(stateVec.k).surface;
5960
Point3D point = new Point3D(stateVec.x, stateVec.y, stateVec.z);
60-
double h = hDoca(point, surf.wireLine[0]);
61+
Vector3D dir = new Vector3D(stateVec.tx, stateVec.ty, 1);
62+
Line3D line = new Line3D(point, dir);
63+
double h = hDoca(line, surf.wireLine[0]);
6164

6265
double signMeas = 1;
6366
double sign = 1;
@@ -73,7 +76,7 @@ public double[] dhDoca(int k, StateVec stateVec) {
7376

7477
//USE THE DOUBLE HIT
7578
if(surf.doca[1]!=-99) {
76-
h = hDoca(point, surf.wireLine[1]);
79+
h = hDoca(line, surf.wireLine[1]);
7780

7881
signMeas = Math.signum(surf.doca[1]);
7982
sign = Math.signum(h);
@@ -85,13 +88,20 @@ public double[] dhDoca(int k, StateVec stateVec) {
8588
}
8689

8790
// Return a signed doca for DC
88-
public double hDoca(Point3D point, Line3D wireLine) {
91+
// Suppose that trajectory is line at the given layer, which is defined by the state vector with point(x, y, z) and dir(tx, ty, 1)
92+
public double hDoca(Line3D trajLine, Line3D wireLine) {
8993

94+
// Define sign of calculated doca to be consistent with LR definition of measured doca
9095
Line3D WL = new Line3D();
9196
WL.copy(wireLine);
92-
WL.copy(WL.distance(point));
97+
WL.copy(WL.distance(trajLine.origin()));
9398

94-
return WL.length()*Math.signum(WL.direction().x());
99+
// Get a line, which is commonly perpendicular to trajectory line and wire line
100+
Line3D cpLine = new Line3D();
101+
cpLine.copy(trajLine);
102+
cpLine.copy(cpLine.distance(wireLine));
103+
104+
return cpLine.length()*Math.signum(WL.direction().x());
95105
}
96106

97107
public double dhURWell(StateVec stateVec) {

‎common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/KFitter.java‎

Lines changed: 45 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,8 @@
1515
import org.jlab.clas.tracking.utilities.RungeKuttaDoca;
1616
import org.jlab.clas.tracking.utilities.MatrixOps.Libr;
1717
import org.jlab.geom.prim.Point3D;
18+
import org.jlab.geom.prim.Vector3D;
19+
import org.jlab.geom.prim.Line3D;
1820
import org.jlab.jnp.matrix.*;
1921

2022
/**
@@ -592,7 +594,7 @@ private boolean filter(int k, boolean forward, double annealingFactor) {
592594

593595
double[] K = new double[5];
594596
double V = effectiveVar;
595-
double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), mVec.surface.wireLine[0]);
597+
double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), sVec.tx, sVec.ty, mVec.surface.wireLine[0]);
596598
Matrix CaInv = this.filterCovMat(H, sVec.CM, V);
597599
if (CaInv != null) {
598600
Matrix5x5.copy(CaInv, cMat);
@@ -607,7 +609,9 @@ private boolean filter(int k, boolean forward, double annealingFactor) {
607609
}
608610

609611
Point3D point = new Point3D(sVec.x, sVec.y, mVec.surface.measPoint.z());
610-
double h = mv.hDoca(point, mVec.surface.wireLine[0]);
612+
Vector3D dir = new Vector3D(sVec.tx, sVec.ty, 1);
613+
Line3D line = new Line3D(point, dir);
614+
double h = mv.hDoca(line, mVec.surface.wireLine[0]);
611615

612616
c2 = (effectiveDoca - h) * (effectiveDoca - h) / V;
613617

@@ -623,7 +627,9 @@ private boolean filter(int k, boolean forward, double annealingFactor) {
623627
+ K[4] * (effectiveDoca - h);
624628

625629
Point3D pointFiltered = new Point3D(x_filt, y_filt, mVec.surface.measPoint.z());
626-
double h0 = mv.hDoca(pointFiltered, mVec.surface.wireLine[0]);
630+
Vector3D dirFiltered = new Vector3D(tx_filt, ty_filt, 1);
631+
Line3D lineFiltered = new Line3D(pointFiltered, dirFiltered);
632+
double h0 = mv.hDoca(lineFiltered, mVec.surface.wireLine[0]);
627633

628634
double residual = effectiveDoca - h0;
629635
updatedWeights_singleHit = daf.calc_updatedWeight_singleHit(residual, annealingFactor);
@@ -644,7 +650,7 @@ private boolean filter(int k, boolean forward, double annealingFactor) {
644650

645651
double[] K = new double[5];
646652
double V = effectiveVar;
647-
double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), mVec.surface.wireLine[indexReferenceWire]);
653+
double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), sVec.tx, sVec.ty, mVec.surface.wireLine[indexReferenceWire]);
648654
Matrix CaInv = this.filterCovMat(H, sVec.CM, V);
649655
if (CaInv != null) {
650656
Matrix5x5.copy(CaInv, cMat);
@@ -659,7 +665,9 @@ private boolean filter(int k, boolean forward, double annealingFactor) {
659665
}
660666

661667
Point3D point = new Point3D(sVec.x, sVec.y, mVec.surface.measPoint.z());
662-
double h = mv.hDoca(point, mVec.surface.wireLine[indexReferenceWire]);
668+
Vector3D dir = new Vector3D(sVec.tx, sVec.ty, 1);
669+
Line3D line = new Line3D(point, dir);
670+
double h = mv.hDoca(line, mVec.surface.wireLine[indexReferenceWire]);
663671

664672
c2 = (effectiveDoca - h) * (effectiveDoca - h) / V;
665673

@@ -675,8 +683,10 @@ private boolean filter(int k, boolean forward, double annealingFactor) {
675683
+ K[4] * (effectiveDoca - h);
676684

677685
Point3D pointFiltered = new Point3D(x_filt, y_filt, mVec.surface.measPoint.z());
678-
double h0 = mv.hDoca(pointFiltered, mVec.surface.wireLine[0]);
679-
double h1 = mv.hDoca(pointFiltered, mVec.surface.wireLine[1]);
686+
Vector3D dirFiltered = new Vector3D(tx_filt, ty_filt, 1);
687+
Line3D lineFiltered = new Line3D(pointFiltered, dirFiltered);
688+
double h0 = mv.hDoca(lineFiltered, mVec.surface.wireLine[0]);
689+
double h1 = mv.hDoca(lineFiltered, mVec.surface.wireLine[1]);
680690
double[] residuals = {mVec.surface.doca[0] - h0, mVec.surface.doca[1] - h1};
681691
updatedWeights_doubleHits = daf.calc_updatedWeights_doubleHits(residuals, annealingFactor);
682692
}
@@ -723,7 +733,7 @@ private boolean filter(int k, boolean forward) {
723733

724734
double[] K = new double[5];
725735
double V = mVec.surface.unc[0] * KFScale;
726-
double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), mVec.surface.wireLine[0]);
736+
double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), sVec.tx, sVec.ty, mVec.surface.wireLine[0]);
727737
Matrix CaInv = this.filterCovMat(H, sVec.CM, V);
728738
Matrix cMat = new Matrix();
729739
if (CaInv != null) {
@@ -739,7 +749,9 @@ private boolean filter(int k, boolean forward) {
739749
}
740750

741751
Point3D point = new Point3D(sVec.x, sVec.y, mVec.surface.measPoint.z());
742-
double h = mv.hDoca(point, mVec.surface.wireLine[0]);
752+
Vector3D dir = new Vector3D(sVec.tx, sVec.ty, 1);
753+
Line3D line = new Line3D(point, dir);
754+
double h = mv.hDoca(line, mVec.surface.wireLine[0]);
743755

744756
double signMeas = 1;
745757
double sign = 1;
@@ -772,8 +784,7 @@ private boolean filter(int k, boolean forward) {
772784
if (mVec.surface.doca[1] != -99) {
773785
// now filter using the other Hit
774786
V = mVec.surface.unc[1] * KFScale;
775-
H = mv.H(x_filt, y_filt, mVec.surface.measPoint.z(),
776-
mVec.surface.wireLine[1]);
787+
H = mv.H(x_filt, y_filt, mVec.surface.measPoint.z(), tx_filt, ty_filt, mVec.surface.wireLine[1]);
777788
CaInv = this.filterCovMat(H, cMat, V);
778789
if (CaInv != null) {
779790
for (int i = 0; i < 5; i++) {
@@ -789,8 +800,9 @@ private boolean filter(int k, boolean forward) {
789800
}
790801

791802
Point3D point2 = new Point3D(x_filt, y_filt, mVec.surface.measPoint.z());
792-
793-
h = mv.hDoca(point2, mVec.surface.wireLine[1]);
803+
Vector3D dir2 = new Vector3D(tx_filt, ty_filt, 1);
804+
Line3D line2 = new Line3D(point2, dir2);
805+
h = mv.hDoca(line2, mVec.surface.wireLine[1]);
794806

795807
signMeas = Math.signum(mVec.surface.doca[1]);
796808
sign = Math.signum(h);
@@ -887,7 +899,9 @@ public void calcFinalChisq(int sector, boolean nofilter) {
887899
double V0 = mv.measurements.get(0).surface.unc[0];
888900

889901
Point3D point = new Point3D(svc.x, svc.y, mv.measurements.get(0).surface.measPoint.z());
890-
double h0 = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[0]);
902+
Vector3D dir = new Vector3D(svc.tx, svc.ty, 1);
903+
Line3D line = new Line3D(point, dir);
904+
double h0 = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[0]);
891905

892906
svc.setProjector(mv.measurements.get(0).surface.wireLine[0].origin().x());
893907
svc.setProjectorDoca(h0);
@@ -898,7 +912,7 @@ public void calcFinalChisq(int sector, boolean nofilter) {
898912
//USE THE DOUBLE HIT
899913
if (mv.measurements.get(0).surface.doca[1] != -99) {
900914
V0 = mv.measurements.get(0).surface.unc[1];
901-
h0 = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[1]);
915+
h0 = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[1]);
902916
res = (mv.measurements.get(0).surface.doca[1] - h0);
903917
chi2 += (mv.measurements.get(0).surface.doca[1] - h0) * (mv.measurements.get(0).surface.doca[1] - h0) / V0;
904918
nRj[mv.measurements.get(0).region - 1] += res * res / mv.measurements.get(0).error;
@@ -920,8 +934,10 @@ public void calcFinalChisq(int sector, boolean nofilter) {
920934
double V = mv.measurements.get(k1 + 1).surface.unc[0];
921935

922936
point = new Point3D(sv.transported(forward).get(k1 + 1).x, sv.transported(forward).get(k1 + 1).y, mv.measurements.get(k1 + 1).surface.measPoint.z());
937+
dir = new Vector3D(sv.transported(forward).get(k1 + 1).tx, sv.transported(forward).get(k1 + 1).ty, 1);
938+
line = new Line3D(point, dir);
923939

924-
double h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[0]);
940+
double h = mv.hDoca(line, mv.measurements.get(k1 + 1).surface.wireLine[0]);
925941
svc = sv.transported(forward).get(k1 + 1);
926942
path += svc.deltaPath;
927943
svc.setPathLength(path);
@@ -934,7 +950,7 @@ public void calcFinalChisq(int sector, boolean nofilter) {
934950
//USE THE DOUBLE HIT
935951
if (mv.measurements.get(k1 + 1).surface.doca[1] != -99) {
936952
V = mv.measurements.get(k1 + 1).surface.unc[1];
937-
h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[1]);
953+
h = mv.hDoca(line, mv.measurements.get(k1 + 1).surface.wireLine[1]);
938954
res = (mv.measurements.get(k1 + 1).surface.doca[1] - h);
939955
chi2 += (mv.measurements.get(k1 + 1).surface.doca[1] - h) * (mv.measurements.get(k1 + 1).surface.doca[1] - h) / V;
940956
nRj[mv.measurements.get(k1 + 1).region - 1] += res * res / V;
@@ -980,6 +996,8 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
980996
svc.setPathLength(path);
981997

982998
Point3D point = new Point3D(svc.x, svc.y, mv.measurements.get(0).surface.measPoint.z());
999+
Vector3D dir = new Vector3D(svc.tx, svc.ty, 1);
1000+
Line3D line = new Line3D(point, dir);
9831001
if(mv.measurements.get(0).surface.doca[1] == -99) {
9841002
StateVec sVecPreviousFiltered = sv.filtered(true).get(0);
9851003
double daf_weight = 1;
@@ -993,7 +1011,7 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
9931011
double effectiveDoca = daf.get_EffectiveDoca();
9941012
double effectiveVar = daf.get_EffectiveVar();
9951013

996-
double h = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[0]);
1014+
double h = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[0]);
9971015
double res = (effectiveDoca - h);
9981016
chi2 += res*res / effectiveVar;
9991017
ndfDAF += daf_weight;
@@ -1018,20 +1036,20 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
10181036
double effectiveVar = daf.get_EffectiveVar();
10191037
int indexReferenceWire = daf.get_IndexReferenceWire();
10201038

1021-
double h = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[indexReferenceWire]);
1039+
double h = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[indexReferenceWire]);
10221040
double res = (effectiveDoca - h);
10231041
chi2 += res*res / effectiveVar;
10241042
ndfDAF += (daf_weights[0] + daf_weights[1]);
10251043

1026-
h = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[0]);
1044+
h = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[0]);
10271045
svc.setProjectorDoca(h);
10281046
svc.setProjector(mv.measurements.get(0).surface.wireLine[0].origin().x());
10291047
svc.setFinalDAFWeight(daf_weights[0]);
10301048
svc.setIsDoubleHit(true);
10311049
kfStateVecsAlongTrajectory.add(svc);
10321050

10331051
StateVec svc2 = sv.new StateVec(svc);
1034-
h = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[1]);
1052+
h = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[1]);
10351053
svc2.setProjectorDoca(h);
10361054
svc2.setProjector(mv.measurements.get(0).surface.wireLine[1].origin().x());
10371055
svc2.setFinalDAFWeight(daf_weights[1]);
@@ -1052,6 +1070,8 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
10521070
svc.setPathLength(path);
10531071

10541072
point = new Point3D(sv.transported(forward).get(k1 + 1).x, sv.transported(forward).get(k1 + 1).y, mv.measurements.get(k1 + 1).surface.measPoint.z());
1073+
dir = new Vector3D(sv.transported(forward).get(k1 + 1).tx, sv.transported(forward).get(k1 + 1).ty, 1);
1074+
line = new Line3D(point, dir);
10551075
if(mv.measurements.get(k1 + 1).surface.doca[1] == -99) {
10561076
StateVec sVecPreviousFiltered = sv.filtered(true).get(k1 + 1);
10571077
double daf_weight = 1;
@@ -1065,7 +1085,7 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
10651085
double effectiveDoca = daf.get_EffectiveDoca();
10661086
double effectiveVar = daf.get_EffectiveVar();
10671087

1068-
double h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[0]);
1088+
double h = mv.hDoca(line, mv.measurements.get(k1 + 1).surface.wireLine[0]);
10691089
double res = (effectiveDoca - h);
10701090
chi2 += res*res / effectiveVar;
10711091
ndfDAF += daf_weight;
@@ -1090,20 +1110,20 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
10901110
double effectiveVar = daf.get_EffectiveVar();
10911111
int indexReferenceWire = daf.get_IndexReferenceWire();
10921112

1093-
double h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[indexReferenceWire]);
1113+
double h = mv.hDoca(line, mv.measurements.get(k1 + 1).surface.wireLine[indexReferenceWire]);
10941114
double res = (effectiveDoca - h);
10951115
chi2 += res*res / effectiveVar;
10961116
ndfDAF += (daf_weights[0] + daf_weights[1]);
10971117

1098-
h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[0]);
1118+
h = mv.hDoca(line, mv.measurements.get(k1 + 1).surface.wireLine[0]);
10991119
svc.setProjectorDoca(h);
11001120
svc.setProjector(mv.measurements.get(k1 + 1).surface.wireLine[0].origin().x());
11011121
svc.setFinalDAFWeight(daf_weights[0]);
11021122
svc.setIsDoubleHit(true);
11031123
kfStateVecsAlongTrajectory.add(svc);
11041124

11051125
StateVec svc2 = sv.new StateVec(svc);
1106-
h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[1]);
1126+
h = mv.hDoca(line, mv.measurements.get(k1 + 1).surface.wireLine[1]);
11071127
svc2.setProjectorDoca(h);
11081128
svc2.setProjector(mv.measurements.get(k1 + 1).surface.wireLine[1].origin().x());
11091129
svc2.setFinalDAFWeight(daf_weights[1]);

0 commit comments

Comments
 (0)