diff --git a/reconstruction/dc/src/main/java/org/jlab/rec/dc/cluster/ClusterCleanerUtilities.java b/reconstruction/dc/src/main/java/org/jlab/rec/dc/cluster/ClusterCleanerUtilities.java index 48643ca6e2..37348e8c25 100644 --- a/reconstruction/dc/src/main/java/org/jlab/rec/dc/cluster/ClusterCleanerUtilities.java +++ b/reconstruction/dc/src/main/java/org/jlab/rec/dc/cluster/ClusterCleanerUtilities.java @@ -76,34 +76,29 @@ public List ClusterSplitter(FittedCluster clus, int nextClsStartI /// determined. /// This is a preliminary pattern recognition method used to identify /// reconstructed hits belonging to the same track-segment. - int N_t = 180; + final int N_t = 180; // From this calculate the bin size in the theta accumulator array - double ThetaMin = 0.; - double ThetaMax = 2. * Math.PI; - double SizeThetaBin = (ThetaMax - ThetaMin) / ((double) N_t); + final double ThetaMin = 0.; + final double ThetaMax = 2. * Math.PI; + final double SizeThetaBin = (ThetaMax - ThetaMin) / ((double) N_t); // Define the dimension of the r accumulator array - int N_r = 130; + final int N_r = 130; // From this calculate the bin size in the theta accumulator array - double RMin = -130; - double RMax = 130; + final double RMin = -130; + final double RMax = 130; + final double dR = RMax - RMin; - int[][] R_Phi_Accumul; - R_Phi_Accumul = new int[N_r][N_t]; + final int[][] R_Phi_Accumul = new int[N_r][N_t]; // cache the cos and sin theta values [for performance improvement] - double[] cosTheta_RPhi_array; - double[] sinTheta_RPhi_array; + final double[] cosTheta_RPhi_array = new double[N_t]; + final double[] sinTheta_RPhi_array = new double[N_t]; // the values corresponding to the peaks in the array - double[] binrMaxR_Phi; - double[] bintMaxR_Phi; - binrMaxR_Phi = new double[N_r * N_t]; - bintMaxR_Phi = new double[N_r * N_t]; - - cosTheta_RPhi_array = new double[N_t]; - sinTheta_RPhi_array = new double[N_t]; + final double[] binrMaxR_Phi = new double[N_r * N_t]; + final double[] bintMaxR_Phi = new double[N_r * N_t]; for (int j_t = 0; j_t < N_t; j_t++) { // theta_j in the middle of the bin : @@ -125,7 +120,7 @@ public List ClusterSplitter(FittedCluster clus, int nextClsStartI // r_j corresponding to that theta_j: double r_j = rho * cosTheta_RPhi_array[j_t] + phi * sinTheta_RPhi_array[j_t]; // this value of r_j falls into the following bin in the r array: - int j_r = (int) Math.floor(N_r * (r_j - RMin) / (float) (RMax - RMin)); + int j_r = (int) Math.floor(N_r * (r_j - RMin) / (float) (dR)); // increase this accumulator cell: R_Phi_Accumul[j_r][j_t]++; @@ -179,11 +174,12 @@ public List ClusterSplitter(FittedCluster clus, int nextClsStartI // r_j corresponding to that theta_j: double r_j = rho * cosTheta_RPhi_array[j_t] + phi * sinTheta_RPhi_array[j_t]; // this value of r_j falls into the following bin in the r array: - int j_r = (int) Math.floor(N_r * (r_j - RMin) / (float) (RMax - RMin)); + int j_r = (int) Math.floor(N_r * (r_j - RMin) / (float) (dR)); // match bins: if (j_r == binrMaxR_Phi[p] && j_t == bintMaxR_Phi[p]) { newClus.add(clus.get(i)); // add this hit + break; // it cannot be in any other theta bin } } } @@ -242,16 +238,15 @@ public List ClusterSplitter(FittedCluster clus, int nextClsStartI } int splitclusId = 1; - if (!selectedClusList2.isEmpty()) { + if (selectedClusList2.isEmpty()) { + selectedClusList2.add(clus); // if the splitting fails, then return the original cluster + } + else { for (FittedCluster cl : selectedClusList2) { cl.set_Id(clus.get_Id() * 1000 + splitclusId); splitclusId++; } } - - if (selectedClusList2.isEmpty()) { - selectedClusList2.add(clus); // if the splitting fails, then return the original cluster - } return selectedClusList2; } @@ -328,11 +323,8 @@ public FittedCluster LRAmbiguityResolver(DataEvent event, FittedCluster fClus, int index = 0; for(FittedHit hit: fClus) { - if (hit.get_Doca() < 0.4 * hit.get_CellSize()) { hit.set_LeftRightAmb(0); - } - if (hit.get_LeftRightAmb() == 0) { index++; } } diff --git a/reconstruction/dc/src/main/java/org/jlab/rec/dc/track/fit/RungeKuttaDoca.java b/reconstruction/dc/src/main/java/org/jlab/rec/dc/track/fit/RungeKuttaDoca.java index e7c2a273c0..d912792d9f 100644 --- a/reconstruction/dc/src/main/java/org/jlab/rec/dc/track/fit/RungeKuttaDoca.java +++ b/reconstruction/dc/src/main/java/org/jlab/rec/dc/track/fit/RungeKuttaDoca.java @@ -1,8 +1,7 @@ package org.jlab.rec.dc.track.fit; -import java.util.ArrayList; -import org.jlab.jnp.matrix.*; import org.jlab.clas.swimtools.Swim; +import org.jlab.jnp.matrix.Matrix; /** * Swims a given state vector to a given Z position using Runge Kutta 4 transport. @@ -103,43 +102,46 @@ void RK4transport(int sector, double h, Swim swim, StateVecsDoca.CovMat covMat, fCov.covMat.set(computeCovMat(covMat.covMat, sF)); } + private void linStep(double h, RK4Vec next, RK4Vec init, RK4Vec prev) { + next.x = init.tx + h*prev.tx; + next.y = init.ty + h*prev.ty; + next.dxdtx0 = init.dtxdtx0 + h*prev.dtxdtx0; + next.dxdty0 = init.dtxdty0 + h*prev.dtxdty0; + next.dxdq0 = init.dtxdq0 + h*prev.dtxdq0; + next.dydtx0 = init.dtydtx0 + h*prev.dtydtx0; + next.dydty0 = init.dtydty0 + h*prev.dtydty0; + next.dydq0 = init.dtydq0 + h*prev.dtydq0; + } + + private void covStep(RK4Vec next, double qv) { + double Csq = Csq(next.x, next.y); + double C = C(Csq); + double Ax = Ax(C, next.x, next.y); + double Ay = Ay(C, next.x, next.y); + next.tx = qv * Ax(C, next.x, next.y); + next.ty = qv * Ay(C, next.x, next.y); + double dAx_dtx = dAx_dtx(C, Csq, Ax, next.x, next.y); + double dAx_dty = dAx_dty(C, Csq, Ax, next.x, next.y); + double dAy_dtx = dAy_dtx(C, Csq, Ay, next.x, next.y); + double dAy_dty = dAy_dty(C, Csq, Ay, next.x, next.y); + next.dtxdtx0 = dtx_dtx0(qv, dAx_dtx, dAx_dty, next.dxdtx0, next.dydtx0); + next.dtxdty0 = dtx_dty0(qv, dAx_dty, next.dxdty0, next.dydty0); + next.dtxdq0 = dtx_dq0( qv, Ax, dAx_dtx, dAx_dty, next.dxdq0, next.dydq0); + next.dtydtx0 = dty_dtx0(qv, dAy_dtx, dAy_dty, next.dxdtx0, next.dydtx0); + next.dtydty0 = dty_dty0(qv, dAy_dty, next.dxdty0, next.dydty0); + next.dtydq0 = dty_dq0( qv, Ay, dAy_dtx, dAy_dty, next.dxdq0, next.dydq0); + } + /** Perform one RK4 step, updating the covariance matrix. */ private RK4Vec RK4step(int sector, double z0, double h, Swim swim, RK4Vec sInit, RK4Vec sPrev, double qv) { + RK4Vec sNext = new RK4Vec(); - swim.Bfield(sector, sInit.x + h*sPrev.x, sInit.y + h*sPrev.y, z0 + h, _b); - // State. - sNext.x = sInit.tx + h*sPrev.tx; - sNext.y = sInit.ty + h*sPrev.ty; + swim.Bfield(sector, sInit.x + h*sPrev.x, sInit.y + h*sPrev.y, z0 + h, _b); - double Csq = Csq(sNext.x, sNext.y); - double C = C(Csq); - double Ax = Ax(C, sNext.x, sNext.y); - double Ay = Ay(C, sNext.x, sNext.y); - - sNext.tx = qv * Ax; - sNext.ty = qv * Ay; - - // Jacobian. - sNext.dxdtx0 = sInit.dtxdtx0 + h*sPrev.dtxdtx0; - sNext.dxdty0 = sInit.dtxdty0 + h*sPrev.dtxdty0; - sNext.dxdq0 = sInit.dtxdq0 + h*sPrev.dtxdq0; - sNext.dydtx0 = sInit.dtydtx0 + h*sPrev.dtydtx0; - sNext.dydty0 = sInit.dtydty0 + h*sPrev.dtydty0; - sNext.dydq0 = sInit.dtydq0 + h*sPrev.dtydq0; - - double dAx_dtx = dAx_dtx(C, Csq, Ax, sNext.x, sNext.y); - double dAx_dty = dAx_dty(C, Csq, Ax, sNext.x, sNext.y); - double dAy_dtx = dAy_dtx(C, Csq, Ay, sNext.x, sNext.y); - double dAy_dty = dAy_dty(C, Csq, Ay, sNext.x, sNext.y); - - sNext.dtxdtx0 = this.dtx_dtx0(qv, dAx_dtx, dAx_dty, sNext.dxdtx0, sNext.dydtx0); - sNext.dtxdty0 = this.dtx_dty0(qv, dAx_dty, sNext.dxdty0, sNext.dydty0); - sNext.dtxdq0 = this.dtx_dq0( qv, Ax, dAx_dtx, dAx_dty, sNext.dxdq0, sNext.dydq0); - sNext.dtydtx0 = this.dty_dtx0(qv, dAy_dtx, dAy_dty, sNext.dxdtx0, sNext.dydtx0); - sNext.dtydty0 = this.dty_dty0(qv, dAy_dty, sNext.dxdty0, sNext.dydty0); - sNext.dtydq0 = this.dty_dq0( qv, Ay, dAy_dtx, dAy_dty, sNext.dxdq0, sNext.dydq0); + linStep(h, sNext, sInit, sPrev); + covStep(sNext, qv); return sNext; } @@ -172,97 +174,63 @@ private RK4Vec computeFinalState(double h, RK4Vec s0, RK4Vec s1, RK4Vec s2, RK4V return sF; } - /** Compute the final covariance matrix. covMat = FCF^T. */ - private double[][] computeCovMat(Matrix C, RK4Vec sF) { - double[][] cNext = new double[5][5]; - cNext[0][0] = C.get(0,0)+C.get(2,0)*sF.dxdtx0+C.get(3,0)*sF.dxdty0+C.get(4,0)*sF.dxdq0 - + sF.dxdq0 *(C.get(0,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0) - + sF.dxdtx0*(C.get(0,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0) - + sF.dxdty0*(C.get(0,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0); - - cNext[0][1] = C.get(0,1)+C.get(2,1)*sF.dxdtx0+C.get(3,1)*sF.dxdty0+C.get(4,1)*sF.dxdq0 - + sF.dydq0 *(C.get(0,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0) - + sF.dydtx0*(C.get(0,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0) - + sF.dydty0*(C.get(0,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0); - - cNext[0][2] = sF.dtxdq0 *(C.get(0,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0) - + sF.dtxdtx0*(C.get(0,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0) - + sF.dtxdty0*(C.get(0,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0); - - cNext[0][3] = sF.dtydq0 *(C.get(0,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0) - + sF.dtydtx0*(C.get(0,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0) - + sF.dtydty0*(C.get(0,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0); - - cNext[0][4] = C.get(0,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0; - - cNext[1][0] = C.get(1,0)+C.get(2,0)*sF.dydtx0+C.get(3,0)*sF.dydty0+C.get(4,0)*sF.dydq0 - + sF.dxdq0 *(C.get(1,4)+C.get(2,4)*sF.dydtx0+C.get(3,4)*sF.dydty0+C.get(4,4)*sF.dydq0) - + sF.dxdtx0*(C.get(1,2)+C.get(2,2)*sF.dydtx0+C.get(3,2)*sF.dydty0+C.get(4,2)*sF.dydq0) - + sF.dxdty0*(C.get(1,3)+C.get(2,3)*sF.dydtx0+C.get(3,3)*sF.dydty0+C.get(4,3)*sF.dydq0); - - cNext[1][1] = C.get(1,1)+C.get(2,1)*sF.dydtx0+C.get(3,1)*sF.dydty0+C.get(4,1)*sF.dydq0 - + sF.dydq0 *(C.get(1,4)+C.get(2,4)*sF.dydtx0+C.get(3,4)*sF.dydty0+C.get(4,4)*sF.dydq0) - + sF.dydtx0*(C.get(1,2)+C.get(2,2)*sF.dydtx0+C.get(3,2)*sF.dydty0+C.get(4,2)*sF.dydq0) - + sF.dydty0*(C.get(1,3)+C.get(2,3)*sF.dydtx0+C.get(3,3)*sF.dydty0+C.get(4,3)*sF.dydq0); - - cNext[1][2] = sF.dtxdq0 *(C.get(1,4)+C.get(2,4)*sF.dydtx0+C.get(3,4)*sF.dydty0+C.get(4,4)*sF.dydq0) - + sF.dtxdtx0*(C.get(1,2)+C.get(2,2)*sF.dydtx0+C.get(3,2)*sF.dydty0+C.get(4,2)*sF.dydq0) - + sF.dtxdty0*(C.get(1,3)+C.get(2,3)*sF.dydtx0+C.get(3,3)*sF.dydty0+C.get(4,3)*sF.dydq0); - - cNext[1][3] = sF.dtydq0 *(C.get(1,4)+C.get(2,4)*sF.dydtx0+C.get(3,4)*sF.dydty0+C.get(4,4)*sF.dydq0) - + sF.dtydtx0*(C.get(1,2)+C.get(2,2)*sF.dydtx0+C.get(3,2)*sF.dydty0+C.get(4,2)*sF.dydq0) - + sF.dtydty0*(C.get(1,3)+C.get(2,3)*sF.dydtx0+C.get(3,3)*sF.dydty0+C.get(4,3)*sF.dydq0); - - cNext[1][4] = C.get(1,4)+C.get(2,4)*sF.dydtx0+C.get(3,4)*sF.dydty0+C.get(4,4)*sF.dydq0; - - cNext[2][0] = C.get(2,0)*sF.dtxdtx0+C.get(3,0)*sF.dtxdty0+C.get(4,0)*sF.dtxdq0 + private static void doit(int i, double[][] next, Matrix C, RK4Vec sF) { + next[i][0] = C.get(i,0)+C.get(2,0)*sF.dxdtx0+C.get(3,0)*sF.dxdty0+C.get(4,0)*sF.dxdq0 + + sF.dxdq0 *(C.get(i,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0) + + sF.dxdtx0*(C.get(i,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0) + + sF.dxdty0*(C.get(i,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0); + + next[i][1] = C.get(i,1)+C.get(2,1)*sF.dxdtx0+C.get(3,1)*sF.dxdty0+C.get(4,1)*sF.dxdq0 + + sF.dydq0 *(C.get(i,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0) + + sF.dydtx0*(C.get(i,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0) + + sF.dydty0*(C.get(i,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0); + + next[i][2] = sF.dtxdq0 *(C.get(i,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0) + + sF.dtxdtx0*(C.get(i,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0) + + sF.dtxdty0*(C.get(i,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0); + + next[i][3] = sF.dtydq0 *(C.get(i,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0) + + sF.dtydtx0*(C.get(i,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0) + + sF.dtydty0*(C.get(i,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0); + + next[i][4] = C.get(i,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0; + } + + private static void doit2(int i, double[][] next, Matrix C, RK4Vec sF) { + next[i][0] = C.get(2,0)*sF.dtxdtx0+C.get(3,0)*sF.dtxdty0+C.get(4,0)*sF.dtxdq0 + sF.dxdq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0) + sF.dxdtx0*(C.get(2,2)*sF.dtxdtx0+C.get(3,2)*sF.dtxdty0+C.get(4,2)*sF.dtxdq0) + sF.dxdty0*(C.get(2,3)*sF.dtxdtx0+C.get(3,3)*sF.dtxdty0+C.get(4,3)*sF.dtxdq0); - cNext[2][1] = C.get(2,1)*sF.dtxdtx0+C.get(3,1)*sF.dtxdty0+C.get(4,1)*sF.dtxdq0 + next[i][1] = C.get(2,1)*sF.dtxdtx0+C.get(3,1)*sF.dtxdty0+C.get(4,1)*sF.dtxdq0 + sF.dydq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0) + sF.dydtx0*(C.get(2,2)*sF.dtxdtx0+C.get(3,2)*sF.dtxdty0+C.get(4,2)*sF.dtxdq0) + sF.dydty0*(C.get(2,3)*sF.dtxdtx0+C.get(3,3)*sF.dtxdty0+C.get(4,3)*sF.dtxdq0); - cNext[2][2] = sF.dtxdq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0) + next[i][2] = sF.dtxdq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0) + sF.dtxdtx0*(C.get(2,2)*sF.dtxdtx0+C.get(3,2)*sF.dtxdty0+C.get(4,2)*sF.dtxdq0) + sF.dtxdty0*(C.get(2,3)*sF.dtxdtx0+C.get(3,3)*sF.dtxdty0+C.get(4,3)*sF.dtxdq0); - cNext[2][3] = sF.dtydq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0) + next[i][3] = sF.dtydq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0) + sF.dtydtx0*(C.get(2,2)*sF.dtxdtx0+C.get(3,2)*sF.dtxdty0+C.get(4,2)*sF.dtxdq0) + sF.dtydty0*(C.get(2,3)*sF.dtxdtx0+C.get(3,3)*sF.dtxdty0+C.get(4,3)*sF.dtxdq0); - cNext[2][4] = C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0; - - cNext[3][0] = C.get(2,0)*sF.dtydtx0+C.get(3,0)*sF.dtydty0+C.get(4,0)*sF.dtydq0 - + sF.dxdq0 *(C.get(2,4)*sF.dtydtx0+C.get(3,4)*sF.dtydty0+C.get(4,4)*sF.dtydq0) - + sF.dxdtx0*(C.get(2,2)*sF.dtydtx0+C.get(3,2)*sF.dtydty0+C.get(4,2)*sF.dtydq0) - + sF.dxdty0*(C.get(2,3)*sF.dtydtx0+C.get(3,3)*sF.dtydty0+C.get(4,3)*sF.dtydq0); - - cNext[3][1] = C.get(2,1)*sF.dtydtx0+C.get(3,1)*sF.dtydty0+C.get(4,1)*sF.dtydq0 - + sF.dydq0 *(C.get(2,4)*sF.dtydtx0+C.get(3,4)*sF.dtydty0+C.get(4,4)*sF.dtydq0) - + sF.dydtx0*(C.get(2,2)*sF.dtydtx0+C.get(3,2)*sF.dtydty0+C.get(4,2)*sF.dtydq0) - + sF.dydty0*(C.get(2,3)*sF.dtydtx0+C.get(3,3)*sF.dtydty0+C.get(4,3)*sF.dtydq0); - - cNext[3][2] = sF.dtxdq0 *(C.get(2,4)*sF.dtydtx0+C.get(3,4)*sF.dtydty0+C.get(4,4)*sF.dtydq0) - + sF.dtxdtx0*(C.get(2,2)*sF.dtydtx0+C.get(3,2)*sF.dtydty0+C.get(4,2)*sF.dtydq0) - + sF.dtxdty0*(C.get(2,3)*sF.dtydtx0+C.get(3,3)*sF.dtydty0+C.get(4,3)*sF.dtydq0); - - cNext[3][3] = sF.dtydq0 *(C.get(2,4)*sF.dtydtx0+C.get(3,4)*sF.dtydty0+C.get(4,4)*sF.dtydq0) - + sF.dtydtx0*(C.get(2,2)*sF.dtydtx0+C.get(3,2)*sF.dtydty0+C.get(4,2)*sF.dtydq0) - + sF.dtydty0*(C.get(2,3)*sF.dtydtx0+C.get(3,3)*sF.dtydty0+C.get(4,3)*sF.dtydq0); - - cNext[3][4] = C.get(2,4)*sF.dtydtx0+C.get(3,4)*sF.dtydty0+C.get(4,4)*sF.dtydq0; + next[i][4] = C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0; + } + + /** Compute the final covariance matrix. covMat = FCF^T. */ + private double[][] computeCovMat(Matrix C, RK4Vec sF) { + double[][] cNext = new double[5][5]; + + doit(0, cNext, C, sF); + doit(1, cNext, C, sF); + doit2(2, cNext, C, sF); + doit2(3, cNext, C, sF); cNext[4][0] = C.get(4,0)+C.get(4,2)*sF.dxdtx0+C.get(4,3)*sF.dxdty0+C.get(4,4)*sF.dxdq0; - cNext[4][1] = C.get(4,1)+C.get(4,2)*sF.dydtx0+C.get(4,3)*sF.dydty0+C.get(4,4)*sF.dydq0; - cNext[4][2] = C.get(4,2)*sF.dtxdtx0+C.get(4,3)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0; - cNext[4][3] = C.get(4,2)*sF.dtydtx0+C.get(4,3)*sF.dtydty0+C.get(4,4)*sF.dtydq0; - cNext[4][4] = C.get(4,4); return cNext; @@ -274,13 +242,13 @@ private double RK4(double k1, double k2, double k3, double k4, double h) { } // Auxiliary calculations. - private double C(double tx, double ty) { + private static double C(double tx, double ty) { return Math.sqrt(1 + tx*tx + ty*ty); } - private double C(double Csq) { + private static double C(double Csq) { return Math.sqrt(Csq); } - private double Csq(double tx, double ty) { + private static double Csq(double tx, double ty) { return 1 + tx*tx + ty*ty; } @@ -305,22 +273,22 @@ private double dAy_dty(double C, double C2, double Ay, double tx, double ty) { } // Total derivatives. - private double dtx_dtx0(double qv, double dAx_dtx, double dAx_dty, + private static double dtx_dtx0(double qv, double dAx_dtx, double dAx_dty, double dtx_dtx0, double dty_dtx0) { return qv * (dAx_dtx*dtx_dtx0 + dAx_dty*dty_dtx0); } - private double dtx_dty0(double qv, double dAx_dty, double dtx_dty0, double dty_dty0) { + private static double dtx_dty0(double qv, double dAx_dty, double dtx_dty0, double dty_dty0) { return qv * (dAx_dty*dtx_dty0 + dAx_dty*dty_dty0); } private double dtx_dq0(double qv, double Ax, double dAx_dtx, double dAx_dty, double dtx_dq0, double dty_dq0) { return v*Ax + qv * (dAx_dtx*dtx_dq0 + dAx_dty*dty_dq0); } - private double dty_dtx0(double qv, double dAy_dtx, double dAy_dty, + private static double dty_dtx0(double qv, double dAy_dtx, double dAy_dty, double dtx_dtx0, double dty_dtx0) { return qv * (dAy_dtx*dtx_dtx0 + dAy_dty*dty_dtx0); } - private double dty_dty0(double qv, double dAy_dty, double dtx_dty0, double dty_dty0) { + private static double dty_dty0(double qv, double dAy_dty, double dtx_dty0, double dty_dty0) { return qv * (dAy_dty*dtx_dty0 + dAy_dty*dty_dty0); } private double dty_dq0(double qv, double Ay, double dAy_dtx, double dAy_dty,