Skip to content

Commit 0a05d0c

Browse files
tongtongcaotongtongcao
committed
update calculated doca in KF processing
1 parent 09b42a5 commit 0a05d0c

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
*
@@ -50,7 +51,9 @@ public double[] dhDoca(int k, StateVec stateVec) {
5051

5152
Surface surf = this.measurements.get(stateVec.k).surface;
5253
Point3D point = new Point3D(stateVec.x, stateVec.y, stateVec.z);
53-
double h = hDoca(point, surf.wireLine[0]);
54+
Vector3D dir = new Vector3D(stateVec.tx, stateVec.ty, 1);
55+
Line3D line = new Line3D(point, dir);
56+
double h = hDoca(line, surf.wireLine[0]);
5457

5558
double signMeas = 1;
5659
double sign = 1;
@@ -66,7 +69,7 @@ public double[] dhDoca(int k, StateVec stateVec) {
6669

6770
//USE THE DOUBLE HIT
6871
if(surf.doca[1]!=-99) {
69-
h = hDoca(point, surf.wireLine[1]);
72+
h = hDoca(line, surf.wireLine[1]);
7073

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

8083
// Return a signed doca for DC
81-
public double hDoca(Point3D point, Line3D wireLine) {
84+
// 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)
85+
public double hDoca(Line3D trajLine, Line3D wireLine) {
8286

87+
// Define sign of calculated doca to be consistent with LR definition of measured doca
8388
Line3D WL = new Line3D();
8489
WL.copy(wireLine);
85-
WL.copy(WL.distance(point));
90+
WL.copy(WL.distance(trajLine.origin()));
8691

87-
return WL.length()*Math.signum(WL.direction().x());
92+
// Get a line, which is commonly perpendicular to trajectory line and wire line
93+
Line3D cpLine = new Line3D();
94+
cpLine.copy(trajLine);
95+
cpLine.copy(cpLine.distance(wireLine));
96+
97+
return cpLine.length()*Math.signum(WL.direction().x());
8898
}
8999

90100
public double dh(int k, 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
}
@@ -725,7 +735,7 @@ private boolean filter(int k, boolean forward) {
725735

726736
double[] K = new double[5];
727737
double V = mVec.surface.unc[0] * KFScale;
728-
double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), mVec.surface.wireLine[0]);
738+
double[] H = mv.H(sVec.x, sVec.y, mVec.surface.measPoint.z(), sVec.tx, sVec.ty, mVec.surface.wireLine[0]);
729739
Matrix CaInv = this.filterCovMat(H, sVec.CM, V);
730740
Matrix cMat = new Matrix();
731741
if (CaInv != null) {
@@ -741,7 +751,9 @@ private boolean filter(int k, boolean forward) {
741751
}
742752

743753
Point3D point = new Point3D(sVec.x, sVec.y, mVec.surface.measPoint.z());
744-
double h = mv.hDoca(point, mVec.surface.wireLine[0]);
754+
Vector3D dir = new Vector3D(sVec.tx, sVec.ty, 1);
755+
Line3D line = new Line3D(point, dir);
756+
double h = mv.hDoca(line, mVec.surface.wireLine[0]);
745757

746758
double signMeas = 1;
747759
double sign = 1;
@@ -774,8 +786,7 @@ private boolean filter(int k, boolean forward) {
774786
if (mVec.surface.doca[1] != -99) {
775787
// now filter using the other Hit
776788
V = mVec.surface.unc[1] * KFScale;
777-
H = mv.H(x_filt, y_filt, mVec.surface.measPoint.z(),
778-
mVec.surface.wireLine[1]);
789+
H = mv.H(x_filt, y_filt, mVec.surface.measPoint.z(), tx_filt, ty_filt, mVec.surface.wireLine[1]);
779790
CaInv = this.filterCovMat(H, cMat, V);
780791
if (CaInv != null) {
781792
for (int i = 0; i < 5; i++) {
@@ -791,8 +802,9 @@ private boolean filter(int k, boolean forward) {
791802
}
792803

793804
Point3D point2 = new Point3D(x_filt, y_filt, mVec.surface.measPoint.z());
794-
795-
h = mv.hDoca(point2, mVec.surface.wireLine[1]);
805+
Vector3D dir2 = new Vector3D(tx_filt, ty_filt, 1);
806+
Line3D line2 = new Line3D(point2, dir2);
807+
h = mv.hDoca(line2, mVec.surface.wireLine[1]);
796808

797809
signMeas = Math.signum(mVec.surface.doca[1]);
798810
sign = Math.signum(h);
@@ -889,7 +901,9 @@ public void calcFinalChisq(int sector, boolean nofilter) {
889901
double V0 = mv.measurements.get(0).surface.unc[0];
890902

891903
Point3D point = new Point3D(svc.x, svc.y, mv.measurements.get(0).surface.measPoint.z());
892-
double h0 = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[0]);
904+
Vector3D dir = new Vector3D(svc.tx, svc.ty, 1);
905+
Line3D line = new Line3D(point, dir);
906+
double h0 = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[0]);
893907

894908
svc.setProjector(mv.measurements.get(0).surface.wireLine[0].origin().x());
895909
svc.setProjectorDoca(h0);
@@ -900,7 +914,7 @@ public void calcFinalChisq(int sector, boolean nofilter) {
900914
//USE THE DOUBLE HIT
901915
if (mv.measurements.get(0).surface.doca[1] != -99) {
902916
V0 = mv.measurements.get(0).surface.unc[1];
903-
h0 = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[1]);
917+
h0 = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[1]);
904918
res = (mv.measurements.get(0).surface.doca[1] - h0);
905919
chi2 += (mv.measurements.get(0).surface.doca[1] - h0) * (mv.measurements.get(0).surface.doca[1] - h0) / V0;
906920
nRj[mv.measurements.get(0).region - 1] += res * res / mv.measurements.get(0).error;
@@ -922,8 +936,10 @@ public void calcFinalChisq(int sector, boolean nofilter) {
922936
double V = mv.measurements.get(k1 + 1).surface.unc[0];
923937

924938
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());
939+
dir = new Vector3D(sv.transported(forward).get(k1 + 1).tx, sv.transported(forward).get(k1 + 1).ty, 1);
940+
line = new Line3D(point, dir);
925941

926-
double h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[0]);
942+
double h = mv.hDoca(line, mv.measurements.get(k1 + 1).surface.wireLine[0]);
927943
svc = sv.transported(forward).get(k1 + 1);
928944
path += (forward ? 1 : -1) * svc.deltaPath;
929945
svc.setPathLength(path);
@@ -936,7 +952,7 @@ public void calcFinalChisq(int sector, boolean nofilter) {
936952
//USE THE DOUBLE HIT
937953
if (mv.measurements.get(k1 + 1).surface.doca[1] != -99) {
938954
V = mv.measurements.get(k1 + 1).surface.unc[1];
939-
h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[1]);
955+
h = mv.hDoca(line, mv.measurements.get(k1 + 1).surface.wireLine[1]);
940956
res = (mv.measurements.get(k1 + 1).surface.doca[1] - h);
941957
chi2 += (mv.measurements.get(k1 + 1).surface.doca[1] - h) * (mv.measurements.get(k1 + 1).surface.doca[1] - h) / V;
942958
nRj[mv.measurements.get(k1 + 1).region - 1] += res * res / V;
@@ -982,6 +998,8 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
982998
svc.setPathLength(path);
983999

9841000
Point3D point = new Point3D(svc.x, svc.y, mv.measurements.get(0).surface.measPoint.z());
1001+
Vector3D dir = new Vector3D(svc.tx, svc.ty, 1);
1002+
Line3D line = new Line3D(point, dir);
9851003
if(mv.measurements.get(0).surface.doca[1] == -99) {
9861004
StateVec sVecPreviousFiltered = sv.filtered(true).get(0);
9871005
double daf_weight = 1;
@@ -995,7 +1013,7 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
9951013
double effectiveDoca = daf.get_EffectiveDoca();
9961014
double effectiveVar = daf.get_EffectiveVar();
9971015

998-
double h = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[0]);
1016+
double h = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[0]);
9991017
double res = (effectiveDoca - h);
10001018
chi2 += res*res / effectiveVar;
10011019
ndfDAF += daf_weight;
@@ -1020,20 +1038,20 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
10201038
double effectiveVar = daf.get_EffectiveVar();
10211039
int indexReferenceWire = daf.get_IndexReferenceWire();
10221040

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

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

10351053
StateVec svc2 = sv.new StateVec(svc);
1036-
h = mv.hDoca(point, mv.measurements.get(0).surface.wireLine[1]);
1054+
h = mv.hDoca(line, mv.measurements.get(0).surface.wireLine[1]);
10371055
svc2.setProjectorDoca(h);
10381056
svc2.setProjector(mv.measurements.get(0).surface.wireLine[1].origin().x());
10391057
svc2.setFinalDAFWeight(daf_weights[1]);
@@ -1054,6 +1072,8 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
10541072
svc.setPathLength(path);
10551073

10561074
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());
1075+
dir = new Vector3D(sv.transported(forward).get(k1 + 1).tx, sv.transported(forward).get(k1 + 1).ty, 1);
1076+
line = new Line3D(point, dir);
10571077
if(mv.measurements.get(k1 + 1).surface.doca[1] == -99) {
10581078
StateVec sVecPreviousFiltered = sv.filtered(true).get(k1 + 1);
10591079
double daf_weight = 1;
@@ -1067,7 +1087,7 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
10671087
double effectiveDoca = daf.get_EffectiveDoca();
10681088
double effectiveVar = daf.get_EffectiveVar();
10691089

1070-
double h = mv.hDoca(point, mv.measurements.get(k1 + 1).surface.wireLine[0]);
1090+
double h = mv.hDoca(line, mv.measurements.get(k1 + 1).surface.wireLine[0]);
10711091
double res = (effectiveDoca - h);
10721092
chi2 += res*res / effectiveVar;
10731093
ndfDAF += daf_weight;
@@ -1092,20 +1112,20 @@ private void calcFinalChisqDAF(int sector, boolean nofilter) {
10921112
double effectiveVar = daf.get_EffectiveVar();
10931113
int indexReferenceWire = daf.get_IndexReferenceWire();
10941114

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

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

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

0 commit comments

Comments
 (0)