private void computeCovMat() {
// array for inverse of Covriance matrix
// the matrix is arraged as (x,y,z,c*t,x',y',p,k)
double[][] invCovMat_a = new double[8][8];
for (int i = 0; i < 8; i++) {
for (int j = 0; j < 8; j++) {
invCovMat_a[i][j] = 0.;
}
}
invCovMat_a[0][0] =
1.0 / pow(sigmaX, 2)
+ pow(cos(alpha), 2) / pow(sigmaW, 2)
+ pow(sin(alpha), 2) / pow(sigmaL, 2);
invCovMat_a[0][2] = (1.0 / pow(sigmaL, 2) - 1.0 / pow(sigmaW, 2)) * sin(alpha) * cos(alpha);
invCovMat_a[2][0] = invCovMat_a[0][2];
invCovMat_a[0][3] = sin(alpha) / pow(sigmaL, 2);
invCovMat_a[3][0] = invCovMat_a[0][3];
invCovMat_a[1][1] = 1 / pow(sigmaY, 2);
invCovMat_a[2][2] =
1 / pow(sigmaZ, 2)
+ pow(cos(alpha), 2) / pow(sigmaL, 2)
+ pow(sin(alpha), 2) / pow(sigmaW, 2);
invCovMat_a[2][3] = -1.0 / pow(sigmaZ, 2) + cos(alpha) / pow(sigmaL, 2);
invCovMat_a[3][2] = invCovMat_a[2][3];
invCovMat_a[3][3] = 1.0 / pow(sigmaZ, 2) + 1.0 / pow(sigmaL, 2);
invCovMat_a[4][4] = 1.0 / pow(sigmaXp, 2);
invCovMat_a[5][5] = 1.0 / pow(sigmaYp, 2);
invCovMat_a[6][6] = 1.0 / pow(sigmaP, 2);
invCovMat_a[7][7] = 1.0 / pow(sigmaK, 2);
// transfer invCovMat_a array to matrix invCovMat
Matrix invCovMat = new Matrix(invCovMat_a);
// inverse invCovMat to CovMat
Matrix CovMat_m = invCovMat.inverse();
covMat = CovMat_m.getArray();
}
private void computeCholDecompA() {
Matrix covMat_m = new Matrix(covMat);
CholeskyDecomposition decomposer = new CholeskyDecomposition(covMat_m);
Matrix A_m = decomposer.getL();
A = A_m.getArray();
}