示例#1
0
  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();
  }
示例#2
0
 private void computeCholDecompA() {
   Matrix covMat_m = new Matrix(covMat);
   CholeskyDecomposition decomposer = new CholeskyDecomposition(covMat_m);
   Matrix A_m = decomposer.getL();
   A = A_m.getArray();
 }