/** * Creates a linear solver using Cholesky decomposition */ public static LinearSolver<DenseMatrix64F> chol( int numRows ) { return symmPosDef(numRows); }
public OriginalDynamicallyConsistentNullspaceCalculator(FloatingInverseDynamicsJoint rootJoint, boolean computeSNsBar) { this.rootJoint = rootJoint; this.massMatrixCalculator = new CompositeRigidBodyMassMatrixCalculator(rootJoint.getSuccessor()); jointsInOrder = massMatrixCalculator.getJointsInOrder(); this.nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointsInOrder); massMatrixSolver = LinearSolverFactory.symmPosDef(nDegreesOfFreedom); lambdaSolver = LinearSolverFactory.symmPosDef(nDegreesOfFreedom); // size of matrix is only used to choose algorithm. nDegreesOfFreedom is an upper limit pseudoInverseSolver = LinearSolverFactory.pseudoInverse(true); this.computeSNsBar = computeSNsBar; }
public OriginalDynamicallyConsistentNullspaceCalculator(FloatingJointBasics rootJoint, boolean computeSNsBar) { this.rootJoint = rootJoint; this.massMatrixCalculator = new CompositeRigidBodyMassMatrixCalculator(rootJoint.getSuccessor()); jointsInOrder = massMatrixCalculator.getInput().getJointMatrixIndexProvider().getIndexedJointsInOrder().toArray(new JointBasics[0]); this.nDegreesOfFreedom = MultiBodySystemTools.computeDegreesOfFreedom(jointsInOrder); massMatrixSolver = LinearSolverFactory.symmPosDef(nDegreesOfFreedom); lambdaSolver = LinearSolverFactory.symmPosDef(nDegreesOfFreedom); // size of matrix is only used to choose algorithm. nDegreesOfFreedom is an upper limit pseudoInverseSolver = LinearSolverFactory.pseudoInverse(true); this.computeSNsBar = computeSNsBar; }
public DampedLeastSquaresSolver(int matrixSize, double alpha) { this.matrixSize = matrixSize; this.A = new DenseMatrix64F(matrixSize, matrixSize); this.tempMatrix1 = new DenseMatrix64F(matrixSize, matrixSize); this.tempMatrix2 = new DenseMatrix64F(matrixSize, 1); // this.linearSolver = LinearSolverFactory.linear(matrixSize); this.linearSolver = LinearSolverFactory.symmPosDef(matrixSize); this.linearSolverAlpha0 = new ConfigurableSolvePseudoInverseSVD(); this.alpha = alpha; }
public DampedLeastSquaresSolver(int matrixSize, double alpha) { this.A = new DenseMatrix64F(matrixSize, matrixSize); this.tempMatrix1 = new DenseMatrix64F(matrixSize, matrixSize); this.tempMatrix2 = new DenseMatrix64F(matrixSize, 1); // this.linearSolver = LinearSolverFactory.linear(matrixSize); this.linearSolver = LinearSolverFactory.symmPosDef(matrixSize); this.linearSolverAlpha0 = LinearSolverFactory.pseudoInverse(true); this.alpha = alpha; }
/** * 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; }
/** * 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; }
/** * 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 DampedQRNullspaceCalculator(int matrixSize, double alpha) { this.alpha = alpha; MathTools.checkIntervalContains(matrixSize, 1, Integer.MAX_VALUE); nullspaceProjector = new DenseMatrix64F(matrixSize, matrixSize); tempMatrixForProjectionInPlace = new DenseMatrix64F(matrixSize, matrixSize); linearSolver = LinearSolverFactory.symmPosDef(matrixSize); decomposer = DecompositionFactory.qr(matrixSize, matrixSize); nullspace = new DenseMatrix64F(matrixSize, matrixSize); Q = new DenseMatrix64F(matrixSize, matrixSize); R1 = new DenseMatrix64F(matrixSize, matrixSize); }
public YoKalmanFilter(String name, YoVariableRegistry parentRegistry) { registry = new YoVariableRegistry(name); nStates = new YoInteger("nStates", registry); nInputs = new YoInteger("nInputs", registry); nMeasurements = new YoInteger("nMeasurements", registry); // covariance matrices are symmetric positive semi-definite int matrixWidth = 0; // only used to decide which algorithm to use. we typically use small matrices, so this is fine solver = LinearSolverFactory.symmPosDef(matrixWidth); // wrap the solver so that it doesn't modify the input // solver = new LinearSolverSafe<DenseMatrix64F>(solver); // A little bit more performance can be gained by letting S be modified. In some // applications S should not be modified. updateCovarianceAndGain = new YoBoolean(name + "UpdateCovarianceAndGain", "Whether or not to update the state covariance matrix and the kalman gain K matrix each update", registry); updateCovarianceAndGain.set(true); parentRegistry.addChild(registry); }
public YoKalmanFilter(String name, YoVariableRegistry parentRegistry) { registry = new YoVariableRegistry(name); nStates = new IntegerYoVariable("nStates", registry); nInputs = new IntegerYoVariable("nInputs", registry); nMeasurements = new IntegerYoVariable("nMeasurements", registry); // covariance matrices are symmetric positive semi-definite int matrixWidth = 0; // only used to decide which algorithm to use. we typically use small matrices, so this is fine solver = LinearSolverFactory.symmPosDef(matrixWidth); // wrap the solver so that it doesn't modify the input // solver = new LinearSolverSafe<DenseMatrix64F>(solver); // A little bit more performance can be gained by letting S be modified. In some // applications S should not be modified. updateCovarianceAndGain = new BooleanYoVariable(name + "UpdateCovarianceAndGain", "Whether or not to update the state covariance matrix and the kalman gain K matrix each update", registry); updateCovarianceAndGain.set(true); parentRegistry.addChild(registry); }
LinearSolver<DenseMatrix64F> solver = LinearSolverFactory.symmPosDef(pg.numRows); DenseMatrix64F X = new DenseMatrix64F(sd.numRows,1); boolean setASuccess=solver.setA(pg);
aPrime = new DenseMatrix64F(SpatialAccelerationReadOnly.SIZE, 1); a = new DenseMatrix64F(SpatialAccelerationReadOnly.SIZE, 1); inverseSolver = nDoFs == 6 ? LinearSolverFactory.symmPosDef(6) : null; transformToParentJointFrame = new RigidBodyTransform(); getJoint().getMotionSubspace(S);