Java DecompositionSolver.getInverse方法代码示例

import org.apache.commons.math3.linear.DecompositionSolver; //导入方法依赖的package包/类
/**
 * Find the center of the ellipsoid.
 *
 * @param a the algebraic from of the polynomial.
 * @return a vector containing the center of the ellipsoid.
 */
private RealVector findCenter(RealMatrix a) {
    RealMatrix subA = a.getSubMatrix(0, 2, 0, 2);

    for (int q = 0; q < subA.getRowDimension(); q++) {
        for (int s = 0; s < subA.getColumnDimension(); s++) {
            subA.multiplyEntry(q, s, -1.0);
        }
    }

    RealVector subV = a.getRowVector(3).getSubVector(0, 3);

    // inv (dtd)
    DecompositionSolver solver = new SingularValueDecomposition(subA)
            .getSolver();
    RealMatrix subAi = solver.getInverse();

    return subAi.operate(subV);
}
 

import org.apache.commons.math3.linear.DecompositionSolver; //导入方法依赖的package包/类
/**
 * Find the center of the ellipsoid.
 * 
 * @param a
 *            the algebraic from of the polynomial.
 * @return a vector containing the center of the ellipsoid.
 */
private RealVector findCenter(RealMatrix a)
{
	RealMatrix subA = a.getSubMatrix(0, 2, 0, 2);

	for (int q = 0; q < subA.getRowDimension(); q++)
	{
		for (int s = 0; s < subA.getColumnDimension(); s++)
		{
			subA.multiplyEntry(q, s, -1.0);
		}
	}

	RealVector subV = a.getRowVector(3).getSubVector(0, 3);

	// inv (dtd)
	DecompositionSolver solver = new SingularValueDecomposition(subA)
			.getSolver();
	RealMatrix subAi = solver.getInverse();

	return subAi.operate(subV);
}
 

import org.apache.commons.math3.linear.DecompositionSolver; //导入方法依赖的package包/类
/**
 * pdf(x, x_hat) = exp(-0.5 * (x-x_hat) * inv(Σ) * (x-x_hat)T) / ( 2π^0.5d * det(Σ)^0.5)
 * 
 * @return value of probabilistic density function
 * @link https://en.wikipedia.org/wiki/Multivariate_normal_distribution#Density_function
 */
public static double pdf(@Nonnull final RealVector x, @Nonnull final RealVector x_hat,
        @Nonnull final RealMatrix sigma) {
    final int dim = x.getDimension();
    Preconditions.checkArgument(x_hat.getDimension() == dim, "|x| != |x_hat|, |x|=" + dim
            + ", |x_hat|=" + x_hat.getDimension());
    Preconditions.checkArgument(sigma.getRowDimension() == dim, "|x| != |sigma|, |x|=" + dim
            + ", |sigma|=" + sigma.getRowDimension());
    Preconditions.checkArgument(sigma.isSquare(), "Sigma is not square matrix");

    LUDecomposition LU = new LUDecomposition(sigma);
    final double detSigma = LU.getDeterminant();
    double denominator = Math.pow(2.d * Math.PI, 0.5d * dim) * Math.pow(detSigma, 0.5d);
    if (denominator == 0.d) { // avoid divide by zero
        return 0.d;
    }

    final RealMatrix invSigma;
    DecompositionSolver solver = LU.getSolver();
    if (solver.isNonSingular() == false) {
        SingularValueDecomposition svd = new SingularValueDecomposition(sigma);
        invSigma = svd.getSolver().getInverse(); // least square solution
    } else {
        invSigma = solver.getInverse();
    }
    //EigenDecomposition eigen = new EigenDecomposition(sigma);
    //double detSigma = eigen.getDeterminant();
    //RealMatrix invSigma = eigen.getSolver().getInverse();

    RealVector diff = x.subtract(x_hat);
    RealVector premultiplied = invSigma.preMultiply(diff);
    double sum = premultiplied.dotProduct(diff);
    double numerator = Math.exp(-0.5d * sum);

    return numerator / denominator;
}
 

import org.apache.commons.math3.linear.DecompositionSolver; //导入方法依赖的package包/类
@Nonnull
public static RealMatrix inverse(@Nonnull final RealMatrix m, final boolean exact)
        throws SingularMatrixException {
    LUDecomposition LU = new LUDecomposition(m);
    DecompositionSolver solver = LU.getSolver();
    final RealMatrix inv;
    if (exact || solver.isNonSingular()) {
        inv = solver.getInverse();
    } else {
        SingularValueDecomposition SVD = new SingularValueDecomposition(m);
        inv = SVD.getSolver().getInverse();
    }
    return inv;
}
 

import org.apache.commons.math3.linear.DecompositionSolver; //导入方法依赖的package包/类
/** {@inheritDoc} */
public RealMatrix getCovariances(double threshold) {
    // Set up the Jacobian.
    final RealMatrix j = this.getJacobian();

    // Compute transpose(J)J.
    final RealMatrix jTj = j.transpose().multiply(j);

    // Compute the covariances matrix.
    final DecompositionSolver solver
            = new QRDecomposition(jTj, threshold).getSolver();
    return solver.getInverse();
}
 

import org.apache.commons.math3.linear.DecompositionSolver; //导入方法依赖的package包/类
/**
 * Correct the current state estimate with an actual measurement.
 *
 * @param z the measurement vector
 * @throws DimensionMismatchException if the dimension of the
 * measurement vector does not fit
 * @throws org.apache.commons.math3.linear.SingularMatrixException
 * if the covariance matrix could not be inverted
 */
public void correct(final RealVector z) {
    // sanity checks
    MathUtils.checkNotNull(z);
    if (z.getDimension() != measurementMatrix.getRowDimension()) {
        throw new DimensionMismatchException(z.getDimension(),
                                             measurementMatrix.getRowDimension());
    }

    // S = H * P(k) - * H' + R
    RealMatrix s = measurementMatrix.multiply(errorCovariance)
        .multiply(measurementMatrixT)
        .add(measurementModel.getMeasurementNoise());

    // invert S
    // as the error covariance matrix is a symmetric positive
    // semi-definite matrix, we can use the cholesky decomposition
    DecompositionSolver solver = new CholeskyDecomposition(s).getSolver();
    RealMatrix invertedS = solver.getInverse();

    // Inn = z(k) - H * xHat(k)-
    RealVector innovation = z.subtract(measurementMatrix.operate(stateEstimation));

    // calculate gain matrix
    // K(k) = P(k)- * H' * (H * P(k)- * H' + R)^-1
    // K(k) = P(k)- * H' * S^-1
    RealMatrix kalmanGain = errorCovariance.multiply(measurementMatrixT).multiply(invertedS);

    // update estimate with measurement z(k)
    // xHat(k) = xHat(k)- + K * Inn
    stateEstimation = stateEstimation.add(kalmanGain.operate(innovation));

    // update covariance of prediction error
    // P(k) = (I - K * H) * P(k)-
    RealMatrix identity = MatrixUtils.createRealIdentityMatrix(kalmanGain.getRowDimension());
    errorCovariance = identity.subtract(kalmanGain.multiply(measurementMatrix)).multiply(errorCovariance);
}
 

import org.apache.commons.math3.linear.DecompositionSolver; //导入方法依赖的package包/类
/**
 * Function to compute matrix inverse via matrix decomposition.
 * 
 * @param in commons-math3 Array2DRowRealMatrix
 * @return matrix block
 * @throws DMLRuntimeException if DMLRuntimeException occurs
 */
private static MatrixBlock computeMatrixInverse(Array2DRowRealMatrix in) 
	throws DMLRuntimeException 
{	
	if ( !in.isSquare() )
		throw new DMLRuntimeException("Input to inv() must be square matrix -- given: a " + in.getRowDimension() + "x" + in.getColumnDimension() + " matrix.");
	
	QRDecomposition qrdecompose = new QRDecomposition(in);
	DecompositionSolver solver = qrdecompose.getSolver();
	RealMatrix inverseMatrix = solver.getInverse();

	return DataConverter.convertToMatrixBlock(inverseMatrix.getData());
}
 

import org.apache.commons.math3.linear.DecompositionSolver; //导入方法依赖的package包/类
/**
 * Correct the current state estimate with an actual measurement.
 *
 * @param z
 *            the measurement vector
 * @throws NullArgumentException
 *             if the measurement vector is {@code null}
 * @throws DimensionMismatchException
 *             if the dimension of the measurement vector does not fit
 * @throws SingularMatrixException
 *             if the covariance matrix could not be inverted
 */
public void correct(final RealVector z)
        throws NullArgumentException, DimensionMismatchException, SingularMatrixException {

    // sanity checks
    MathUtils.checkNotNull(z);
    if (z.getDimension() != measurementMatrix.getRowDimension()) {
        throw new DimensionMismatchException(z.getDimension(),
                                             measurementMatrix.getRowDimension());
    }

    // S = H * P(k) - * H' + R
    RealMatrix s = measurementMatrix.multiply(errorCovariance)
        .multiply(measurementMatrixT)
        .add(measurementModel.getMeasurementNoise());

    // invert S
    // as the error covariance matrix is a symmetric positive
    // semi-definite matrix, we can use the cholesky decomposition
    DecompositionSolver solver = new CholeskyDecomposition(s).getSolver();
    RealMatrix invertedS = solver.getInverse();

    // Inn = z(k) - H * xHat(k)-
    RealVector innovation = z.subtract(measurementMatrix.operate(stateEstimation));

    // calculate gain matrix
    // K(k) = P(k)- * H' * (H * P(k)- * H' + R)^-1
    // K(k) = P(k)- * H' * S^-1
    RealMatrix kalmanGain = errorCovariance.multiply(measurementMatrixT).multiply(invertedS);

    // update estimate with measurement z(k)
    // xHat(k) = xHat(k)- + K * Inn
    stateEstimation = stateEstimation.add(kalmanGain.operate(innovation));

    // update covariance of prediction error
    // P(k) = (I - K * H) * P(k)-
    RealMatrix identity = MatrixUtils.createRealIdentityMatrix(kalmanGain.getRowDimension());
    errorCovariance = identity.subtract(kalmanGain.multiply(measurementMatrix)).multiply(errorCovariance);
}