//@Override
public void update(DMatrixRMaj z, DMatrixRMaj R)
{
// y = z - H x
CommonOps_DDRM.mult(H, x, y);
CommonOps_DDRM.subtract(z, y, y);
// S = H P H' + R
CommonOps_DDRM.mult(H, P, c);
CommonOps_DDRM.multTransB(c, H, S);
CommonOps_DDRM.addEquals(S, R);
// K = PH'S^(-1)
if (!solver.setA(S))
{
throw new InvalidOperationException("Invert failed");
}
solver.invert(S_inv);
CommonOps_DDRM.multTransA(H, S_inv, d);
CommonOps_DDRM.mult(P, d, K);
// x = x + Ky
CommonOps_DDRM.mult(K, y, a);
CommonOps_DDRM.addEquals(x, a);
// P = (I-kH)P = P - (KH)P = P-K(HP)
CommonOps_DDRM.mult(H, P, c);
CommonOps_DDRM.mult(K, c, b);
CommonOps_DDRM.subtractEquals(P, b);
}
public void invert(DMatrixRMaj A_inv)
{
blockB.reshape(A_inv.numRows, A_inv.numCols, false);
alg.invert(blockB);
MatrixOps_DDRB.convert(blockB, A_inv);
}
/**
* <p>
* Performs a matrix inversion operation on the specified matrix and stores the results
* in the same matrix.<br>
* <br>
* a = a<sup>-1</sup>
* </p>
*
* <p>
* If the algorithm could not invert the matrix then false is returned. If it returns true
* that just means the algorithm finished. The results could still be bad
* because the matrix is singular or nearly singular.
* </p>
*
* @param A The matrix that is to be inverted. Results are stored here. Modified.
* @return true if it could invert the matrix false if it could not.
*/
public static bool invert(CMatrixRMaj A)
{
LinearSolverDense <CMatrixRMaj> solver = LinearSolverFactory_CDRM.lu(A.numRows);
if (solver.setA(A))
{
solver.invert(A);
}
else
{
return(false);
}
return(true);
}
/**
* <p>
* Performs a matrix inversion operation that does not modify the original
* and stores the results in another matrix. The two matrices must have the
* same dimension.<br>
* <br>
* b = a<sup>-1</sup>
* </p>
*
* <p>
* If the algorithm could not invert the matrix then false is returned. If it returns true
* that just means the algorithm finished. The results could still be bad
* because the matrix is singular or nearly singular.
* </p>
*
* <p>
* For medium to large matrices there might be a slight performance boost to using
* {@link LinearSolverFactory_CDRM} instead.
* </p>
*
* @param input The matrix that is to be inverted. Not modified.
* @param output Where the inverse matrix is stored. Modified.
* @return true if it could invert the matrix false if it could not.
*/
public static bool invert(CMatrixRMaj input, CMatrixRMaj output)
{
LinearSolverDense <CMatrixRMaj> solver = LinearSolverFactory_CDRM.lu(input.numRows);
if (solver.modifiesA())
{
input = (CMatrixRMaj)input.copy();
}
if (!solver.setA(input))
{
return(false);
}
solver.invert(output);
return(true);
}
public virtual void invert(T A_inv)
{
alg.invert(A_inv);
}
public void invert(T A_inv)
{
alg.invert(A_inv);
}
