public void copyTo(Matrix dest) {
for (int i = 0; i < this.row; i++) {
for (int j = 0; j < this.col; j++) {
dest.set(j, i, elements[i][j]);
}
}
}
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();
}
public void leftMultiply(Matrix other) {
// other * this
if (other.col != this.row) {
System.err.println("attempt to multiply incompatible matrices.");
System.exit(1);
} else {
tempCopy();
for (int i = 0; i < other.row; i++) {
for (int j = 0; j < tempElements[0].length; j++) {
// multiply other row by this col to get replacement elements[i][j]
double replacement = 0;
for (int k = 0; k < other.col; k++) {
replacement += other.get(k, i) * tempElements[k][j];
}
this.set(j, i, replacement);
}
}
}
}
public void rightMultiply(Matrix other) {
// this * other
if (this.col != other.row) {
System.err.println("attempt to multiply incompatible matrices.");
System.exit(1);
} else {
tempCopy();
for (int i = 0; i < tempElements.length; i++) {
for (int j = 0; j < other.col; j++) {
// multiply this row by other col to get replacement elements[i][j]
double replacement = 0;
for (int k = 0; k < tempElements[0].length; k++) {
replacement += tempElements[i][k] * other.get(j, k);
}
this.set(j, i, replacement);
}
}
}
}
private void computeCholDecompA() {
Matrix covMat_m = new Matrix(covMat);
CholeskyDecomposition decomposer = new CholeskyDecomposition(covMat_m);
Matrix A_m = decomposer.getL();
A = A_m.getArray();
}
