/**
* Performs a matrix inversion operations that takes advantage of the special
* properties of a covariance matrix.
*
* @param cov A covariance matrix. Not modified.
* @param cov_inv The inverse of cov. Modified.
* @return true if it could invert the matrix false if it could not.
*/
public static bool invert(DMatrixRMaj cov, DMatrixRMaj cov_inv)
{
if (cov.numCols <= 4)
{
if (cov.numCols != cov.numRows)
{
throw new ArgumentException("Must be a square matrix.");
}
if (cov.numCols >= 2)
{
UnrolledInverseFromMinor_DDRM.inv(cov, cov_inv);
}
else
{
cov_inv.data[0] = 1.0 / cov_inv.data[0];
}
}
else
{
LinearSolverDense <DMatrixRMaj> solver = LinearSolverFactory_DDRM.symmPosDef(cov.numRows);
// wrap it to make sure the covariance is not modified.
solver = new LinearSolverSafe <DMatrixRMaj>(solver);
if (!solver.setA(cov))
{
return(false);
}
solver.invert(cov_inv);
}
return(true);
}
//@Override
public void configure(DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H)
{
this.F = F;
this.Q = Q;
this.H = H;
int dimenX = F.numCols;
int dimenZ = H.numRows;
a = new DMatrixRMaj(dimenX, 1);
b = new DMatrixRMaj(dimenX, dimenX);
y = new DMatrixRMaj(dimenZ, 1);
S = new DMatrixRMaj(dimenZ, dimenZ);
S_inv = new DMatrixRMaj(dimenZ, dimenZ);
c = new DMatrixRMaj(dimenZ, dimenX);
d = new DMatrixRMaj(dimenX, dimenZ);
K = new DMatrixRMaj(dimenX, dimenZ);
x = new DMatrixRMaj(dimenX, 1);
P = new DMatrixRMaj(dimenX, dimenX);
// covariance matrices are symmetric positive semi-definite
solver = LinearSolverFactory_DDRM.symmPosDef(dimenX);
}
