@Override public void invert(T A_inv) { alg.invert(A_inv); }
@Override public void invert(T A_inv) { alg.invert(A_inv); }
/** * Creates a block matrix the same size as A_inv, inverts the matrix and copies the results back * onto A_inv. * * @param A_inv Where the inverted matrix saved. Modified. */ @Override public void invert(DenseMatrix64F A_inv) { blockB.reshape(A_inv.numRows,A_inv.numCols,false); alg.invert(blockB); BlockMatrixOps.convert(blockB,A_inv); }
/** * Creates a block matrix the same size as A_inv, inverts the matrix and copies the results back * onto A_inv. * * @param A_inv Where the inverted matrix saved. Modified. */ @Override public void invert(DenseMatrix64F A_inv) { blockB.reshape(A_inv.numRows,A_inv.numCols,false); alg.invert(blockB); BlockMatrixOps.convert(blockB,A_inv); }
/** * Creates a block matrix the same size as A_inv, inverts the matrix and copies the results back * onto A_inv. * * @param A_inv Where the inverted matrix saved. Modified. */ @Override public void invert(DenseMatrix64F A_inv) { blockB.reshape(A_inv.numRows,A_inv.numCols,false); alg.invert(blockB); BlockMatrixOps.convert(blockB,A_inv); }
public void setA(DenseMatrix64F A) { this.A.set(A); solver.setA(A); solver.invert(APlus); }
public void setA(DenseMatrix64F A) { this.A.set(A); solver.setA(A); solver.invert(APlus); }
public void computeSelectionMatrix(GeometricJacobian jacobian, DenseMatrix64F selectionMatrix) { DenseMatrix64F jacobianMatrix = jacobian.getJacobianMatrix(); if (selectionMatrixSolver.modifiesA()) { throw new RuntimeException("Selection matrix solver changes A"); } if (!selectionMatrixSolver.setA(jacobianMatrix)) throw new IllegalArgumentException("Invert failed, maybe a bug?"); selectionMatrixSolver.invert(selectionMatrix); } }
public void packInverse(LinearSolver<DenseMatrix64F> solver, BlockDiagSquareMatrix matrixToPack) { for(int i=0;i<blockSizes.length;i++) { tmpMatrix[i].reshape(blockSizes[i] , blockSizes[i]); packBlock(tmpMatrix[i], i, 0, 0); solver.setA(tmpMatrix[i]); solver.invert(tmpMatrix[i]); matrixToPack.setBlock(tmpMatrix[i] ,i); } }
public void packInverse(LinearSolver<DenseMatrix64F> solver, BlockDiagSquareMatrix matrixToPack) { for(int i=0;i<blockSizes.length;i++) { tmpMatrix[i].reshape(blockSizes[i] , blockSizes[i]); packBlock(tmpMatrix[i], i, 0, 0); solver.setA(tmpMatrix[i]); solver.invert(tmpMatrix[i]); matrixToPack.setBlock(tmpMatrix[i] ,i); } }
/** * 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 boolean invert( final DenseMatrix64F cov , final DenseMatrix64F cov_inv ) { if( cov.numCols <= 4 ) { if( cov.numCols != cov.numRows ) { throw new IllegalArgumentException("Must be a square matrix."); } if( cov.numCols >= 2 ) UnrolledInverseFromMinor.inv(cov,cov_inv); else cov_inv.data[0] = 1.0/cov_inv.data[0]; } else { LinearSolver<DenseMatrix64F> solver = LinearSolverFactory.symmPosDef(cov.numRows); // wrap it to make sure the covariance is not modified. solver = new LinearSolverSafe<DenseMatrix64F>(solver); if( !solver.setA(cov) ) return false; solver.invert(cov_inv); } return true; }
public void packInverse(LinearSolver<DenseMatrix64F> solver, DenseMatrix64F matrixToPack) { matrixToPack.zero(); for(int i=0;i<blockSizes.length;i++) { tmpMatrix[i].reshape(blockSizes[i] , blockSizes[i]); packBlock(tmpMatrix[i], i, 0, 0); solver.setA(tmpMatrix[i]); solver.invert(tmpMatrix[i]); setBlock(tmpMatrix[i] ,i, matrixToPack); } }
public void packInverse(LinearSolver<DenseMatrix64F> solver, DenseMatrix64F matrixToPack) { matrixToPack.zero(); for(int i=0;i<blockSizes.length;i++) { tmpMatrix[i].reshape(blockSizes[i] , blockSizes[i]); packBlock(tmpMatrix[i], i, 0, 0); solver.setA(tmpMatrix[i]); solver.invert(tmpMatrix[i]); setBlock(tmpMatrix[i] ,i, matrixToPack); } }
@Override public void invert(DenseMatrix64F A_inv) { if (alpha == 0) { linearSolverAlpha0.setA(this.A); linearSolverAlpha0.invert(A_inv); } else { tempMatrix1.reshape(A.getNumRows(), A.getNumRows()); CommonOps.multTransB(A, A, tempMatrix1); SpecializedOps.addIdentity(tempMatrix1, tempMatrix1, alpha * alpha); linearSolver.setA(tempMatrix1); tempMatrix2.reshape(tempMatrix1.getNumRows(), tempMatrix1.getNumCols()); linearSolver.invert(tempMatrix2); CommonOps.multTransA(A, tempMatrix2, A_inv); } }
private void solveSystem(int numRows, int numCols) { LinearSolver<DenseMatrix64F> qrSolver = LinearSolverFactory.qr(numRows, numCols); QRDecomposition<DenseMatrix64F> decomposition = qrSolver.getDecomposition(); qrSolver.setA(X); y.setData(response); qrSolver.solve(this.y, this.b); DenseMatrix64F R = decomposition.getR(null, true); LinearSolver<DenseMatrix64F> linearSolver = LinearSolverFactory.linear(numCols); linearSolver.setA(R); DenseMatrix64F Rinverse = new DenseMatrix64F(numCols, numCols); linearSolver.invert(Rinverse); // stores solver's solution inside of Rinverse. CommonOps.multOuter(Rinverse, this.XtXInv); }
private void computeQInverseAndAQInverse() { int numberOfVariables = quadraticCostQMatrix.getNumRows(); int numberOfEqualityConstraints = linearEqualityConstraintsAMatrix.getNumRows(); QInverse.reshape(numberOfVariables, numberOfVariables); solver.setA(quadraticCostQMatrix); solver.invert(QInverse); AQInverse.reshape(numberOfEqualityConstraints, numberOfVariables); QInverseATranspose.reshape(numberOfVariables, numberOfEqualityConstraints); AQInverseATranspose.reshape(numberOfEqualityConstraints, numberOfEqualityConstraints); if (numberOfEqualityConstraints > 0) { CommonOps.mult(linearEqualityConstraintsAMatrix, QInverse, AQInverse); CommonOps.multTransB(QInverse, linearEqualityConstraintsAMatrix, QInverseATranspose); CommonOps.multTransB(AQInverse, linearEqualityConstraintsAMatrix, AQInverseATranspose); } }
private void computeQInverseAndAQInverse() { int numberOfVariables = quadraticCostQMatrix.getNumRows(); int numberOfEqualityConstraints = linearEqualityConstraintsAMatrix.getNumRows(); ATranspose.reshape(linearEqualityConstraintsAMatrix.getNumCols(), linearEqualityConstraintsAMatrix.getNumRows()); CommonOps.transpose(linearEqualityConstraintsAMatrix, ATranspose); QInverse.reshape(numberOfVariables, numberOfVariables); solver.setA(quadraticCostQMatrix); solver.invert(QInverse); AQInverse.reshape(numberOfEqualityConstraints, numberOfVariables); QInverseATranspose.reshape(numberOfVariables, numberOfEqualityConstraints); AQInverseATranspose.reshape(numberOfEqualityConstraints, numberOfEqualityConstraints); if (numberOfEqualityConstraints > 0) { CommonOps.mult(linearEqualityConstraintsAMatrix, QInverse, AQInverse); CommonOps.mult(QInverse, ATranspose, QInverseATranspose); CommonOps.mult(AQInverse, ATranspose, AQInverseATranspose); } }
private void updateKalmanGainMatrixK() { int nMeasurements = this.nMeasurements.getIntegerValue(); int nStates = this.nStates.getIntegerValue(); c.reshape(nMeasurements, nStates); S.reshape(nMeasurements, nMeasurements); S_inv.reshape(nMeasurements, nMeasurements); d.reshape(nStates, nMeasurements); K.reshape(nStates, nMeasurements); // S = H P H' + R MatrixMatrixMult.mult_small(H, P, c); MatrixMatrixMult.multTransB(c, H, S); addEquals(S, R); // K = PH'S^(-1) if (!solver.setA(S)) throw new SingularMatrixException(); solver.invert(S_inv); MatrixMatrixMult.multTransA_small(H, S_inv, d); MatrixMatrixMult.mult_small(P, d, K); }
private void updateKalmanGainMatrixK() { int nMeasurements = this.nMeasurements.getIntegerValue(); int nStates = this.nStates.getIntegerValue(); c.reshape(nMeasurements, nStates); S.reshape(nMeasurements, nMeasurements); S_inv.reshape(nMeasurements, nMeasurements); d.reshape(nStates, nMeasurements); K.reshape(nStates, nMeasurements); // S = H P H' + R MatrixMatrixMult.mult_small(H, P, c); MatrixMatrixMult.multTransB(c, H, S); addEquals(S, R); // K = PH'S^(-1) if (!solver.setA(S)) throw new SingularMatrixException(); solver.invert(S_inv); MatrixMatrixMult.multTransA_small(H, S_inv, d); MatrixMatrixMult.mult_small(P, d, K); }
public void setConstraint(DenseMatrix64F j, DenseMatrix64F p) { this.j.set(j); this.p.set(p); if (j.getNumRows() > 0) { // J^{+} jPlus.reshape(j.getNumCols(), j.getNumRows()); solver.setA(j); solver.invert(jPlus); // J^{+} * p jPlusp.reshape(jPlus.getNumRows(), p.getNumCols()); CommonOps.mult(jPlus, p, jPlusp); // Q q.reshape(jPlus.getNumRows(), j.getNumCols()); CommonOps.setIdentity(q); CommonOps.multAdd(-1.0, jPlus, j, q); } else { jPlus.reshape(j.getNumCols(), 0); q.reshape(j.getNumCols(), j.getNumCols()); CommonOps.setIdentity(q); jPlusp.reshape(j.getNumCols(), 1); jPlusp.zero(); } }