org.apache.commons.math3.linear.MatrixUtils Java Examples

The following examples show how to use org.apache.commons.math3.linear.MatrixUtils. You can vote up the ones you like or vote down the ones you don't like, and go to the original project or source file by following the links above each example. You may check out the related API usage on the sidebar.
Example #1
Source File: VectorialCovariance.java    From astor with GNU General Public License v2.0 6 votes vote down vote up
/**
 * Get the covariance matrix.
 * @return covariance matrix
 */
public RealMatrix getResult() {

    int dimension = sums.length;
    RealMatrix result = MatrixUtils.createRealMatrix(dimension, dimension);

    if (n > 1) {
        double c = 1.0 / (n * (isBiasCorrected ? (n - 1) : n));
        int k = 0;
        for (int i = 0; i < dimension; ++i) {
            for (int j = 0; j <= i; ++j) {
                double e = c * (n * productsSums[k++] - sums[i] * sums[j]);
                result.setEntry(i, j, e);
                result.setEntry(j, i, e);
            }
        }
    }

    return result;

}
 
Example #2
Source File: LinalgUtil.java    From MeteoInfo with GNU Lesser General Public License v3.0 6 votes vote down vote up
/**
 * Calculate inverse matrix
 *
 * @param a The matrix
 * @return Inverse matrix array
 */
public static Array inv(Array a) {
    double[][] aa = (double[][]) ArrayUtil.copyToNDJavaArray_Double(a);
    RealMatrix matrix = new Array2DRowRealMatrix(aa, false);
    RealMatrix invm = MatrixUtils.inverse(matrix);
    if (invm == null) {
        return null;
    }

    int m = invm.getRowDimension();
    int n = invm.getColumnDimension();
    Array r = Array.factory(DataType.DOUBLE, new int[]{m, n});
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < n; j++) {
            r.setDouble(i * n + j, invm.getEntry(i, j));
        }
    }

    return r;
}
 
Example #3
Source File: MultivariateTDistribution.java    From macrobase with Apache License 2.0 6 votes vote down vote up
public MultivariateTDistribution(RealVector mean, RealMatrix covarianceMatrix, double degreesOfFreedom) {
    this.mean = mean;
    if (mean.getDimension() > 1) {
        this.precisionMatrix = MatrixUtils.blockInverse(covarianceMatrix, (-1 + covarianceMatrix.getColumnDimension()) / 2);
    } else {
        this.precisionMatrix = MatrixUtils.createRealIdentityMatrix(1).scalarMultiply(1. / covarianceMatrix.getEntry(0, 0));
    }
    this.dof = degreesOfFreedom;

    this.D = mean.getDimension();

    double determinant = new LUDecomposition(covarianceMatrix).getDeterminant();

    this.multiplier = Math.exp(Gamma.logGamma(0.5 * (D + dof)) - Gamma.logGamma(0.5 * dof)) /
            Math.pow(Math.PI * dof, 0.5 * D) /
            Math.pow(determinant, 0.5);
}
 
Example #4
Source File: GLSMultipleLinearRegressionTest.java    From astor with GNU General Public License v2.0 6 votes vote down vote up
/**
 * Verifies that GLS with identity covariance matrix gives the same results
 * as OLS.
 */
@Test
public void testGLSOLSConsistency() {      
    RealMatrix identityCov = MatrixUtils.createRealIdentityMatrix(16);
    GLSMultipleLinearRegression glsModel = new GLSMultipleLinearRegression();
    OLSMultipleLinearRegression olsModel = new OLSMultipleLinearRegression();
    glsModel.newSampleData(longley, 16, 6);
    olsModel.newSampleData(longley, 16, 6);
    glsModel.newCovarianceData(identityCov.getData());
    double[] olsBeta = olsModel.calculateBeta().toArray();
    double[] glsBeta = glsModel.calculateBeta().toArray();
    // TODO:  Should have assertRelativelyEquals(double[], double[], eps) in TestUtils
    //        Should also add RealVector and RealMatrix versions
    for (int i = 0; i < olsBeta.length; i++) {
        TestUtils.assertRelativelyEquals(olsBeta[i], glsBeta[i], 10E-7);
    }
}
 
Example #5
Source File: AbstractLeastSquaresOptimizer.java    From astor with GNU General Public License v2.0 6 votes vote down vote up
/**
 * Computes the Jacobian matrix.
 *
 * @param params Model parameters at which to compute the Jacobian.
 * @return the weighted Jacobian: W<sup>1/2</sup> J.
 * @throws DimensionMismatchException if the Jacobian dimension does not
 * match problem dimension.
 * @since 3.1
 */
protected RealMatrix computeWeightedJacobian(double[] params) {
    ++jacobianEvaluations;

    final DerivativeStructure[] dsPoint = new DerivativeStructure[params.length];
    final int nC = params.length;
    for (int i = 0; i < nC; ++i) {
        dsPoint[i] = new DerivativeStructure(nC, 1, i, params[i]);
    }
    final DerivativeStructure[] dsValue = jF.value(dsPoint);
    final int nR = getTarget().length;
    if (dsValue.length != nR) {
        throw new DimensionMismatchException(dsValue.length, nR);
    }
    final double[][] jacobianData = new double[nR][nC];
    for (int i = 0; i < nR; ++i) {
        int[] orders = new int[nC];
        for (int j = 0; j < nC; ++j) {
            orders[j] = 1;
            jacobianData[i][j] = dsValue[i].getPartialDerivative(orders);
            orders[j] = 0;
        }
    }

    return weightMatrixSqrt.multiply(MatrixUtils.createRealMatrix(jacobianData));
}
 
Example #6
Source File: AbstractLeastSquaresOptimizer.java    From astor with GNU General Public License v2.0 6 votes vote down vote up
/**
 * Get the covariance matrix of the optimized parameters.
 * <br/>
 * Note that this operation involves the inversion of the
 * <code>J<sup>T</sup>J</code> matrix, where {@code J} is the
 * Jacobian matrix.
 * The {@code threshold} parameter is a way for the caller to specify
 * that the result of this computation should be considered meaningless,
 * and thus trigger an exception.
 *
 * @param threshold Singularity threshold.
 * @return the covariance matrix.
 * @throws org.apache.commons.math3.linear.SingularMatrixException
 * if the covariance matrix cannot be computed (singular problem).
 */
public double[][] getCovariances(double threshold) {
    // Set up the jacobian.
    updateJacobian();

    // Compute transpose(J)J, without building intermediate matrices.
    double[][] jTj = new double[cols][cols];
    for (int i = 0; i < cols; ++i) {
        for (int j = i; j < cols; ++j) {
            double sum = 0;
            for (int k = 0; k < rows; ++k) {
                sum += weightedResidualJacobian[k][i] * weightedResidualJacobian[k][j];
            }
            jTj[i][j] = sum;
            jTj[j][i] = sum;
        }
    }

    // Compute the covariances matrix.
    final DecompositionSolver solver
        = new QRDecomposition(MatrixUtils.createRealMatrix(jTj), threshold).getSolver();
    return solver.getInverse().getData();
}
 
Example #7
Source File: DecomposeSingularValuesIntegrationTest.java    From gatk-protected with BSD 3-Clause "New" or "Revised" License 6 votes vote down vote up
private void assertSVDValues(final File outputFileV, final File outputFileS, final File outputFileU) {
    try {
        final ReadCountCollection rcc = ReadCountCollectionUtils.parse(CONTROL_PCOV_FULL_FILE);
        final SVD svd = SVDFactory.createSVD(rcc.counts());
        final RealMatrix sDiag = new DiagonalMatrix(svd.getSingularValues());
        assertOutputFileValues(outputFileU, svd.getU());
        assertOutputFileValues(outputFileS, sDiag);
        assertOutputFileValues(outputFileV, svd.getV());
        assertUnitaryMatrix(svd.getV());
        assertUnitaryMatrix(svd.getU());
        Assert.assertTrue(MatrixUtils.isSymmetric(sDiag, 1e-32));

    } catch (final IOException ioe) {
        Assert.fail("Could not open test file: " + CONTROL_PCOV_FULL_FILE, ioe);
    }
}
 
Example #8
Source File: VectorialCovariance.java    From astor with GNU General Public License v2.0 6 votes vote down vote up
/**
 * Get the covariance matrix.
 * @return covariance matrix
 */
public RealMatrix getResult() {

    int dimension = sums.length;
    RealMatrix result = MatrixUtils.createRealMatrix(dimension, dimension);

    if (n > 1) {
        double c = 1.0 / (n * (isBiasCorrected ? (n - 1) : n));
        int k = 0;
        for (int i = 0; i < dimension; ++i) {
            for (int j = 0; j <= i; ++j) {
                double e = c * (n * productsSums[k++] - sums[i] * sums[j]);
                result.setEntry(i, j, e);
                result.setEntry(j, i, e);
            }
        }
    }

    return result;

}
 
Example #9
Source File: VectorialCovariance.java    From astor with GNU General Public License v2.0 6 votes vote down vote up
/**
 * Get the covariance matrix.
 * @return covariance matrix
 */
public RealMatrix getResult() {

    int dimension = sums.length;
    RealMatrix result = MatrixUtils.createRealMatrix(dimension, dimension);

    if (n > 1) {
        double c = 1.0 / (n * (isBiasCorrected ? (n - 1) : n));
        int k = 0;
        for (int i = 0; i < dimension; ++i) {
            for (int j = 0; j <= i; ++j) {
                double e = c * (n * productsSums[k++] - sums[i] * sums[j]);
                result.setEntry(i, j, e);
                result.setEntry(j, i, e);
            }
        }
    }

    return result;

}
 
Example #10
Source File: CorrelatedRandomVectorGeneratorTest.java    From astor with GNU General Public License v2.0 5 votes vote down vote up
@Test
public void testMath226() {
    double[] mean = { 1, 1, 10, 1 };
    double[][] cov = {
            { 1, 3, 2, 6 },
            { 3, 13, 16, 2 },
            { 2, 16, 38, -1 },
            { 6, 2, -1, 197 }
    };
    RealMatrix covRM = MatrixUtils.createRealMatrix(cov);
    JDKRandomGenerator jg = new JDKRandomGenerator();
    jg.setSeed(5322145245211l);
    NormalizedRandomGenerator rg = new GaussianRandomGenerator(jg);
    CorrelatedRandomVectorGenerator sg =
        new CorrelatedRandomVectorGenerator(mean, covRM, 0.00001, rg);

    double[] min = new double[mean.length];
    Arrays.fill(min, Double.POSITIVE_INFINITY);
    double[] max = new double[mean.length];
    Arrays.fill(max, Double.NEGATIVE_INFINITY);
    for (int i = 0; i < 10; i++) {
        double[] generated = sg.nextVector();
        for (int j = 0; j < generated.length; ++j) {
            min[j] = FastMath.min(min[j], generated[j]);
            max[j] = FastMath.max(max[j], generated[j]);
        }
    }
    for (int j = 0; j < min.length; ++j) {
        Assert.assertTrue(max[j] - min[j] > 2.0);
    }

}
 
Example #11
Source File: OmsCurvaturesBivariate.java    From hortonmachine with GNU General Public License v3.0 5 votes vote down vote up
/**
 * Calculates the parameters of a bivariate quadratic equation.
 * 
 * @param elevationValues the window of points to use.
 * @return the parameters of the bivariate quadratic equation as [a, b, c, d, e, f]
 */
private static double[] calculateParameters( final double[][] elevationValues ) {
    int rows = elevationValues.length;
    int cols = elevationValues[0].length;
    int pointsNum = rows * cols;

    final double[][] xyMatrix = new double[pointsNum][6];
    final double[] valueArray = new double[pointsNum];

    // TODO check on resolution
    int index = 0;
    for( int y = 0; y < rows; y++ ) {
        for( int x = 0; x < cols; x++ ) {
            xyMatrix[index][0] = x * x; // x^2
            xyMatrix[index][1] = y * y; // y^2
            xyMatrix[index][2] = x * y; // xy
            xyMatrix[index][3] = x; // x
            xyMatrix[index][4] = y; // y
            xyMatrix[index][5] = 1;
            valueArray[index] = elevationValues[y][x];
            index++;
        }
    }

    RealMatrix A = MatrixUtils.createRealMatrix(xyMatrix);
    RealVector z = MatrixUtils.createRealVector(valueArray);

    DecompositionSolver solver = new RRQRDecomposition(A).getSolver();
    RealVector solution = solver.solve(z);

    // start values for a, b, c, d, e, f, all set to 0.0
    final double[] parameters = solution.toArray();
    return parameters;
}
 
Example #12
Source File: CorrelatedRandomVectorGeneratorTest.java    From astor with GNU General Public License v2.0 5 votes vote down vote up
public CorrelatedRandomVectorGeneratorTest() {
    mean = new double[] { 0.0, 1.0, -3.0, 2.3 };

    RealMatrix b = MatrixUtils.createRealMatrix(4, 3);
    int counter = 0;
    for (int i = 0; i < b.getRowDimension(); ++i) {
        for (int j = 0; j < b.getColumnDimension(); ++j) {
            b.setEntry(i, j, 1.0 + 0.1 * ++counter);
        }
    }
    RealMatrix bbt = b.multiply(b.transpose());
    covariance = MatrixUtils.createRealMatrix(mean.length, mean.length);
    for (int i = 0; i < covariance.getRowDimension(); ++i) {
        covariance.setEntry(i, i, bbt.getEntry(i, i));
        for (int j = 0; j < covariance.getColumnDimension(); ++j) {
            double s = bbt.getEntry(i, j);
            covariance.setEntry(i, j, s);
            covariance.setEntry(j, i, s);
        }
    }

    RandomGenerator rg = new JDKRandomGenerator();
    rg.setSeed(17399225432l);
    GaussianRandomGenerator rawGenerator = new GaussianRandomGenerator(rg);
    generator = new CorrelatedRandomVectorGenerator(mean,
                                                    covariance,
                                                    1.0e-12 * covariance.getNorm(),
                                                    rawGenerator);
}
 
Example #13
Source File: CorrelatedRandomVectorGeneratorTest.java    From astor with GNU General Public License v2.0 5 votes vote down vote up
@Test
public void testMath226() {
    double[] mean = { 1, 1, 10, 1 };
    double[][] cov = {
            { 1, 3, 2, 6 },
            { 3, 13, 16, 2 },
            { 2, 16, 38, -1 },
            { 6, 2, -1, 197 }
    };
    RealMatrix covRM = MatrixUtils.createRealMatrix(cov);
    JDKRandomGenerator jg = new JDKRandomGenerator();
    jg.setSeed(5322145245211l);
    NormalizedRandomGenerator rg = new GaussianRandomGenerator(jg);
    CorrelatedRandomVectorGenerator sg =
        new CorrelatedRandomVectorGenerator(mean, covRM, 0.00001, rg);

    double[] min = new double[mean.length];
    Arrays.fill(min, Double.POSITIVE_INFINITY);
    double[] max = new double[mean.length];
    Arrays.fill(max, Double.NEGATIVE_INFINITY);
    for (int i = 0; i < 10; i++) {
        double[] generated = sg.nextVector();
        for (int j = 0; j < generated.length; ++j) {
            min[j] = FastMath.min(min[j], generated[j]);
            max[j] = FastMath.max(max[j], generated[j]);
        }
    }
    for (int j = 0; j < min.length; ++j) {
        Assert.assertTrue(max[j] - min[j] > 2.0);
    }

}
 
Example #14
Source File: AdamsNordsieckTransformer.java    From astor with GNU General Public License v2.0 5 votes vote down vote up
/** Simple constructor.
 * @param nSteps number of steps of the multistep method
 * (excluding the one being computed)
 */
private AdamsNordsieckTransformer(final int nSteps) {

    // compute exact coefficients
    FieldMatrix<BigFraction> bigP = buildP(nSteps);
    FieldDecompositionSolver<BigFraction> pSolver =
        new FieldLUDecomposition<BigFraction>(bigP).getSolver();

    BigFraction[] u = new BigFraction[nSteps];
    Arrays.fill(u, BigFraction.ONE);
    BigFraction[] bigC1 = pSolver
        .solve(new ArrayFieldVector<BigFraction>(u, false)).toArray();

    // update coefficients are computed by combining transform from
    // Nordsieck to multistep, then shifting rows to represent step advance
    // then applying inverse transform
    BigFraction[][] shiftedP = bigP.getData();
    for (int i = shiftedP.length - 1; i > 0; --i) {
        // shift rows
        shiftedP[i] = shiftedP[i - 1];
    }
    shiftedP[0] = new BigFraction[nSteps];
    Arrays.fill(shiftedP[0], BigFraction.ZERO);
    FieldMatrix<BigFraction> bigMSupdate =
        pSolver.solve(new Array2DRowFieldMatrix<BigFraction>(shiftedP, false));

    // convert coefficients to double
    update         = MatrixUtils.bigFractionMatrixToRealMatrix(bigMSupdate);
    c1             = new double[nSteps];
    for (int i = 0; i < nSteps; ++i) {
        c1[i] = bigC1[i].doubleValue();
    }

}
 
Example #15
Source File: BiDiagonalTransformerTest.java    From astor with GNU General Public License v2.0 5 votes vote down vote up
@Test
public void testMatricesValues() {
   BiDiagonalTransformer transformer =
        new BiDiagonalTransformer(MatrixUtils.createRealMatrix(testSquare));
   final double s17 = FastMath.sqrt(17.0);
    RealMatrix uRef = MatrixUtils.createRealMatrix(new double[][] {
            {  -8 / (5 * s17), 19 / (5 * s17) },
            { -19 / (5 * s17), -8 / (5 * s17) }
    });
    RealMatrix bRef = MatrixUtils.createRealMatrix(new double[][] {
            { -3 * s17 / 5, 32 * s17 / 85 },
            {      0.0,     -5 * s17 / 17 }
    });
    RealMatrix vRef = MatrixUtils.createRealMatrix(new double[][] {
            { 1.0,  0.0 },
            { 0.0, -1.0 }
    });

    // check values against known references
    RealMatrix u = transformer.getU();
    Assert.assertEquals(0, u.subtract(uRef).getNorm(), 1.0e-14);
    RealMatrix b = transformer.getB();
    Assert.assertEquals(0, b.subtract(bRef).getNorm(), 1.0e-14);
    RealMatrix v = transformer.getV();
    Assert.assertEquals(0, v.subtract(vRef).getNorm(), 1.0e-14);

    // check the same cached instance is returned the second time
    Assert.assertTrue(u == transformer.getU());
    Assert.assertTrue(b == transformer.getB());
    Assert.assertTrue(v == transformer.getV());

}
 
Example #16
Source File: TestInvertMatrices.java    From deeplearning4j with Apache License 2.0 5 votes vote down vote up
@Test
public void testInverse() {
    RealMatrix matrix = new Array2DRowRealMatrix(new double[][] {{1, 2}, {3, 4}});

    RealMatrix inverse = MatrixUtils.inverse(matrix);
    INDArray arr = InvertMatrix.invert(Nd4j.linspace(1, 4, 4).reshape(2, 2), false);
    for (int i = 0; i < inverse.getRowDimension(); i++) {
        for (int j = 0; j < inverse.getColumnDimension(); j++) {
            assertEquals(arr.getDouble(i, j), inverse.getEntry(i, j), 1e-1);
        }
    }
}
 
Example #17
Source File: RidgeRegression.java    From Surus with Apache License 2.0 5 votes vote down vote up
public void updateCoefficients(double l2penalty) {
       if (this.X_svd == null) {
       	this.X_svd = new SingularValueDecomposition(X);
       }
    RealMatrix V = this.X_svd.getV();
    double[] s = this.X_svd.getSingularValues();
    RealMatrix U = this.X_svd.getU();
    
    for (int i = 0; i < s.length; i++) {
    	s[i] = s[i] / (s[i]*s[i] + l2penalty);
    }
    RealMatrix S = MatrixUtils.createRealDiagonalMatrix(s);
    
    RealMatrix Z = V.multiply(S).multiply(U.transpose());
    
    this.coefficients = Z.operate(this.Y);
    
    this.fitted = this.X.operate(this.coefficients);
    double errorVariance = 0;
    for (int i = 0; i < residuals.length; i++) {
    	this.residuals[i] = this.Y[i] - this.fitted[i];
    	errorVariance += this.residuals[i] * this.residuals[i];
    }
    errorVariance = errorVariance / (X.getRowDimension() - X.getColumnDimension());
    
    RealMatrix errorVarianceMatrix = MatrixUtils.createRealIdentityMatrix(this.Y.length).scalarMultiply(errorVariance);
    RealMatrix coefficientsCovarianceMatrix = Z.multiply(errorVarianceMatrix).multiply(Z.transpose());
    this.standarderrors = getDiagonal(coefficientsCovarianceMatrix);
}
 
Example #18
Source File: SyntheticBeaconDataGenerator.java    From BLELocalization with MIT License 5 votes vote down vote up
public void fit(List<Location> locations){
	setLocations(locations);
	int n = locations.size();
	double[][] K = new double[n][n];

	for(int i=0; i<n; i++){
		Location loc1 = locations.get(i);
		double[] x1 = ModelAdaptUtils.locationToVec(loc1);
		for(int j=i; j<n; j++){
			Location loc2 = locations.get(j);
			double[] x2 = ModelAdaptUtils.locationToVec(loc2);
			double k =kernel.computeKernel(x1, x2);
			K[i][j] = k;
			K[j][i] = k;
		}
	}
	RealMatrix Kmat = MatrixUtils.createRealMatrix(K);
	RealMatrix lambdamat = MatrixUtils.createRealIdentityMatrix(n).scalarMultiply(sigma_n*sigma_n); //Tentative treatment

	RealMatrix Kymat = Kmat.add(lambdamat);

	CholeskyDecomposition chol = new CholeskyDecomposition(Kymat);
	RealMatrix Lmat = chol.getL();

	double[] normalRands = new double[n];
	for(int i=0; i<n; i++){
		normalRands[i] = rand.nextGaussian();
	}
	this.y = Lmat.operate(normalRands);

	RealMatrix invKymat = (new LUDecomposition(Kymat)).getSolver().getInverse();
	this.alphas = invKymat.operate(y);
}
 
Example #19
Source File: SimpleAverageUserState.java    From samantha with MIT License 5 votes vote down vote up
private RealVector getStateValue(ObjectNode state) {
    RealVector vec = MatrixUtils.createRealVector(new double[actionAttrs.size() + 1]);
    double cnt = state.get(modelName + "-state-cnt").asDouble();
    vec.setEntry(0, cnt);
    for (int i=0; i< actionAttrs.size(); i++) {
        vec.setEntry(i + 1, state.get("state-" + actionAttrs.get(i)).asDouble() * cnt);
    }
    return vec;
}
 
Example #20
Source File: SynchronizedVariableSpace.java    From samantha with MIT License 5 votes vote down vote up
final public void requestVectorVar(String name, int size, int dim, double initial,
                                   boolean randomize, boolean normalize) {
    List<RealVector> var = new ArrayList<>(size);
    for (int i=0; i<size; i++) {
        RealVector vec = MatrixUtils.createRealVector(new double[dim]);
        initializeVector(vec, initial, randomize, normalize);
        var.add(vec);
    }
    writeLock.lock();
    try {
        vectorVars.put(name, var);
    } finally {
        writeLock.unlock();
    }
}
 
Example #21
Source File: KalmanFilter.java    From astor with GNU General Public License v2.0 5 votes vote down vote up
/**
 * 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);
}
 
Example #22
Source File: EvaluationTest.java    From astor with GNU General Public License v2.0 5 votes vote down vote up
@Test
public void testComputeValueAndJacobian() {
    //setup
    final RealVector point = new ArrayRealVector(new double[]{1, 2});
    Evaluation evaluation = new LeastSquaresBuilder()
            .weight(new DiagonalMatrix(new double[]{16, 4}))
            .model(new MultivariateJacobianFunction() {
                public Pair<RealVector, RealMatrix> value(RealVector actualPoint) {
                    //verify correct values passed in
                    Assert.assertArrayEquals(
                            point.toArray(), actualPoint.toArray(), Precision.EPSILON);
                    //return values
                    return new Pair<RealVector, RealMatrix>(
                            new ArrayRealVector(new double[]{3, 4}),
                            MatrixUtils.createRealMatrix(new double[][]{{5, 6}, {7, 8}})
                    );
                }
            })
            .target(new double[2])
            .build()
            .evaluate(point);

    //action
    RealVector residuals = evaluation.getResiduals();
    RealMatrix jacobian = evaluation.getJacobian();

    //verify
    Assert.assertArrayEquals(evaluation.getPoint().toArray(), point.toArray(), 0);
    Assert.assertArrayEquals(new double[]{-12, -8}, residuals.toArray(), Precision.EPSILON);
    TestUtils.assertEquals(
            "jacobian",
            jacobian,
            MatrixUtils.createRealMatrix(new double[][]{{20, 24},{14, 16}}),
            Precision.EPSILON);
}
 
Example #23
Source File: BiDiagonalTransformerTest.java    From astor with GNU General Public License v2.0 5 votes vote down vote up
@Test
public void testMatricesValues() {
   BiDiagonalTransformer transformer =
        new BiDiagonalTransformer(MatrixUtils.createRealMatrix(testSquare));
   final double s17 = FastMath.sqrt(17.0);
    RealMatrix uRef = MatrixUtils.createRealMatrix(new double[][] {
            {  -8 / (5 * s17), 19 / (5 * s17) },
            { -19 / (5 * s17), -8 / (5 * s17) }
    });
    RealMatrix bRef = MatrixUtils.createRealMatrix(new double[][] {
            { -3 * s17 / 5, 32 * s17 / 85 },
            {      0.0,     -5 * s17 / 17 }
    });
    RealMatrix vRef = MatrixUtils.createRealMatrix(new double[][] {
            { 1.0,  0.0 },
            { 0.0, -1.0 }
    });

    // check values against known references
    RealMatrix u = transformer.getU();
    Assert.assertEquals(0, u.subtract(uRef).getNorm(), 1.0e-14);
    RealMatrix b = transformer.getB();
    Assert.assertEquals(0, b.subtract(bRef).getNorm(), 1.0e-14);
    RealMatrix v = transformer.getV();
    Assert.assertEquals(0, v.subtract(vRef).getNorm(), 1.0e-14);

    // check the same cached instance is returned the second time
    Assert.assertTrue(u == transformer.getU());
    Assert.assertTrue(b == transformer.getB());
    Assert.assertTrue(v == transformer.getV());

}
 
Example #24
Source File: BiDiagonalTransformerTest.java    From astor with GNU General Public License v2.0 5 votes vote down vote up
@Test
public void testMatricesValues() {
   BiDiagonalTransformer transformer =
        new BiDiagonalTransformer(MatrixUtils.createRealMatrix(testSquare));
   final double s17 = FastMath.sqrt(17.0);
    RealMatrix uRef = MatrixUtils.createRealMatrix(new double[][] {
            {  -8 / (5 * s17), 19 / (5 * s17) },
            { -19 / (5 * s17), -8 / (5 * s17) }
    });
    RealMatrix bRef = MatrixUtils.createRealMatrix(new double[][] {
            { -3 * s17 / 5, 32 * s17 / 85 },
            {      0.0,     -5 * s17 / 17 }
    });
    RealMatrix vRef = MatrixUtils.createRealMatrix(new double[][] {
            { 1.0,  0.0 },
            { 0.0, -1.0 }
    });

    // check values against known references
    RealMatrix u = transformer.getU();
    Assert.assertEquals(0, u.subtract(uRef).getNorm(), 1.0e-14);
    RealMatrix b = transformer.getB();
    Assert.assertEquals(0, b.subtract(bRef).getNorm(), 1.0e-14);
    RealMatrix v = transformer.getV();
    Assert.assertEquals(0, v.subtract(vRef).getNorm(), 1.0e-14);

    // check the same cached instance is returned the second time
    Assert.assertTrue(u == transformer.getU());
    Assert.assertTrue(b == transformer.getB());
    Assert.assertTrue(v == transformer.getV());

}
 
Example #25
Source File: WeightedIntDiGraphTest.java    From pacaya with Apache License 2.0 5 votes vote down vote up
@Test
public void testSumWalks() {
    RealMatrix m = new Array2DRowRealMatrix(new double[][] {
        { 0.3, 0.0, 0.3 },
        { 0.1, 0.6, 0.7 },
        { 0.0, 0.4, 0.2 },
    });
    RealMatrix sum = MatrixUtils.createRealIdentityMatrix(3);
    for (int i = 0; i < 2000; i++) {
        sum = sum.add(m.power(i + 1));
    }
    RealVector ones = new ArrayRealVector(3, 1.0);
    // this is how I'm computing the reference (different from the methods implementation)
    double expectedSumWalks = sum.preMultiply(ones).dotProduct(ones);
    RealVector s = new ArrayRealVector(new double[] {1, 2, 3});
    RealVector t = new ArrayRealVector(new double[] {5, 7, 4});
    assertEquals(127.49999999999903, expectedSumWalks, tol);
    assertEquals(127.49999999999903, WeightedIntDiGraph.sumWalks(m, null, null), tol);
    assertEquals(127.49999999999903, WeightedIntDiGraph.sumWalks(m, null, ones), tol);
    assertEquals(127.49999999999903, WeightedIntDiGraph.sumWalks(m, ones, null), tol);

    assertTrue(TestUtils.checkThrows(() -> {
        WeightedIntDiGraph.sumWalks(
            new Array2DRowRealMatrix(
                    new double[][] {
                        { 0.3, 0.0, 0.3, 0 },
                        { 0.1, 0.6, 0.7, 0 },
                        { 0.0, 0.4, 0.2, 0 }}),
            null,
            null);
        }, InputMismatchException.class));                
    assertEquals(699.9999999999948, WeightedIntDiGraph.sumWalks(m, null, t), tol);
    assertEquals(274.99999999999795, WeightedIntDiGraph.sumWalks(m, s, null), tol);
    assertEquals(1509.9999999999886, WeightedIntDiGraph.sumWalks(m, s, t), tol);

}
 
Example #26
Source File: MatrixStackTest.java    From NOVA-Core with GNU Lesser General Public License v3.0 5 votes vote down vote up
@Test
public void testTransforms() {
	ms.translate(Vector3DUtil.ONE);
	ms.scale(Vector3DUtil.ONE.scalarMultiply(2));
	ms.pushMatrix();
	ms.rotate(Vector3D.PLUS_J, Math.PI / 2);
	assertThat(ms.apply(Vector3D.PLUS_K)).isAlmostEqualTo(new Vector3D(-1, 1, 1));

	ms.popMatrix();
	ms.transform(MatrixUtils.createRealMatrix(new Rotation(Vector3D.PLUS_J, Math.PI / 2).getMatrix()));
	assertThat(ms.apply(Vector3D.PLUS_K)).isAlmostEqualTo(new Vector3D(-1, 1, 1));

	assertThat(ms.apply(Vector3DUtil.ONE)).isAlmostEqualTo(ms.apply(Vector3DUtil.ONE));

}
 
Example #27
Source File: MatrixStack.java    From NOVA-Core with GNU Lesser General Public License v3.0 5 votes vote down vote up
/**
 * Rotates the current matrix
 *
 * @param rotation The rotation to aply
 * @return The rorated matrix
 */
public MatrixStack rotate(Rotation rotation) {
	RealMatrix rotMat = MatrixUtils.createRealMatrix(4, 4);
	rotMat.setSubMatrix(rotation.getMatrix(), 0, 0);
	rotMat.setEntry(3, 3, 1);
	current = current.preMultiply(rotMat);
	return this;
}
 
Example #28
Source File: GaussNewtonOptimizer.java    From astor with GNU General Public License v2.0 5 votes vote down vote up
/**
 * Compute the normal matrix, J<sup>T</sup>J.
 *
 * @param jacobian  the m by n jacobian matrix, J. Input.
 * @param residuals the m by 1 residual vector, r. Input.
 * @return  the n by n normal matrix and  the n by 1 J<sup>Tr vector.
 */
private static Pair<RealMatrix, RealVector> computeNormalMatrix(final RealMatrix jacobian,
                                                                final RealVector residuals) {
    //since the normal matrix is symmetric, we only need to compute half of it.
    final int nR = jacobian.getRowDimension();
    final int nC = jacobian.getColumnDimension();
    //allocate space for return values
    final RealMatrix normal = MatrixUtils.createRealMatrix(nC, nC);
    final RealVector jTr = new ArrayRealVector(nC);
    //for each measurement
    for (int i = 0; i < nR; ++i) {
        //compute JTr for measurement i
        for (int j = 0; j < nC; j++) {
            jTr.setEntry(j, jTr.getEntry(j) +
                    residuals.getEntry(i) * jacobian.getEntry(i, j));
        }

        // add the the contribution to the normal matrix for measurement i
        for (int k = 0; k < nC; ++k) {
            //only compute the upper triangular part
            for (int l = k; l < nC; ++l) {
                normal.setEntry(k, l, normal.getEntry(k, l) +
                        jacobian.getEntry(i, k) * jacobian.getEntry(i, l));
            }
        }
    }
    //copy the upper triangular part to the lower triangular part.
    for (int i = 0; i < nC; i++) {
        for (int j = 0; j < i; j++) {
            normal.setEntry(i, j, normal.getEntry(j, i));
        }
    }
    return new Pair<RealMatrix, RealVector>(normal, jTr);
}
 
Example #29
Source File: Arja_00181_s.java    From coming with MIT License 4 votes vote down vote up
/**
 * Initialization of the dynamic search parameters
 *
 * @param guess Initial guess for the arguments of the fitness function.
 */
private void initializeCMA(double[] guess) {
    if (lambda <= 0) {
        lambda = 4 + (int) (3. * Math.log(dimension));
    }
    // initialize sigma
    double[][] sigmaArray = new double[guess.length][1];
    for (int i = 0; i < guess.length; i++) {
        final double range =  (boundaries == null) ? 1.0 : boundaries[1][i] - boundaries[0][i];
        sigmaArray[i][0]   = ((inputSigma == null) ? 0.3 : inputSigma[i]) / range;
    }
    RealMatrix insigma = new Array2DRowRealMatrix(sigmaArray, false);
    sigma = max(insigma); // overall standard deviation

    // initialize termination criteria
    stopTolUpX = 1e3 * max(insigma);
    stopTolX = 1e-11 * max(insigma);
    stopTolFun = 1e-12;
    stopTolHistFun = 1e-13;

    // initialize selection strategy parameters
    mu = lambda / 2; // number of parents/points for recombination
    logMu2 = Math.log(mu + 0.5);
    weights = log(sequence(1, mu, 1)).scalarMultiply(-1.).scalarAdd(logMu2);
    double sumw = 0;
    double sumwq = 0;
    for (int i = 0; i < mu; i++) {
        double w = weights.getEntry(i, 0);
        sumw += w;
        sumwq += w * w;
    }
    weights = weights.scalarMultiply(1. / sumw);
    mueff = sumw * sumw / sumwq; // variance-effectiveness of sum w_i x_i

    // initialize dynamic strategy parameters and constants
    cc = (4. + mueff / dimension) /
            (dimension + 4. + 2. * mueff / dimension);
    cs = (mueff + 2.) / (dimension + mueff + 3.);
    damps = (1. + 2. * Math.max(0, Math.sqrt((mueff - 1.) /
            (dimension + 1.)) - 1.)) *
            Math.max(0.3, 1. - dimension /
                    (1e-6 + Math.min(maxIterations, getMaxEvaluations() /
                            lambda))) + cs; // minor increment
    ccov1 = 2. / ((dimension + 1.3) * (dimension + 1.3) + mueff);
    ccovmu = Math.min(1 - ccov1, 2. * (mueff - 2. + 1. / mueff) /
            ((dimension + 2.) * (dimension + 2.) + mueff));
    ccov1Sep = Math.min(1, ccov1 * (dimension + 1.5) / 3.);
    ccovmuSep = Math.min(1 - ccov1, ccovmu * (dimension + 1.5) / 3.);
    chiN = Math.sqrt(dimension) *
            (1. - 1. / (4. * dimension) + 1 / (21. * dimension * dimension));
    // intialize CMA internal values - updated each generation
    xmean = MatrixUtils.createColumnRealMatrix(guess); // objective
                                                       // variables
    diagD = insigma.scalarMultiply(1. / sigma);
    diagC = square(diagD);
    pc = zeros(dimension, 1); // evolution paths for C and sigma
    ps = zeros(dimension, 1); // B defines the coordinate system
    normps = ps.getFrobeniusNorm();

    B = eye(dimension, dimension);
    D = ones(dimension, 1); // diagonal D defines the scaling
    BD = times(B, repmat(diagD.transpose(), dimension, 1));
    C = B.multiply(diag(square(D)).multiply(B.transpose())); // covariance
    historySize = 10 + (int) (3. * 10. * dimension / lambda);
    fitnessHistory = new double[historySize]; // history of fitness values
    for (int i = 0; i < historySize; i++) {
        fitnessHistory[i] = Double.MAX_VALUE;
    }
}
 
Example #30
Source File: Cardumen_00163_t.java    From coming with MIT License 4 votes vote down vote up
/**
 * Initialization of the dynamic search parameters
 *
 * @param guess Initial guess for the arguments of the fitness function.
 */
private void initializeCMA(double[] guess) {
    if (lambda <= 0) {
        lambda = 4 + (int) (3. * Math.log(dimension));
    }
    // initialize sigma
    double[][] sigmaArray = new double[guess.length][1];
    for (int i = 0; i < guess.length; i++) {
        final double range =  (boundaries == null) ? 1.0 : boundaries[1][i] - boundaries[0][i];
        sigmaArray[i][0]   = ((inputSigma == null) ? 0.3 : inputSigma[i]) / range;
    }
    RealMatrix insigma = new Array2DRowRealMatrix(sigmaArray, false);
    sigma = max(insigma); // overall standard deviation

    // initialize termination criteria
    stopTolUpX = 1e3 * max(insigma);
    stopTolX = 1e-11 * max(insigma);
    stopTolFun = 1e-12;
    stopTolHistFun = 1e-13;

    // initialize selection strategy parameters
    mu = lambda / 2; // number of parents/points for recombination
    logMu2 = Math.log(mu + 0.5);
    weights = log(sequence(1, mu, 1)).scalarMultiply(-1.).scalarAdd(logMu2);
    double sumw = 0;
    double sumwq = 0;
    for (int i = 0; i < mu; i++) {
        double w = weights.getEntry(i, 0);
        sumw += w;
        sumwq += w * w;
    }
    weights = weights.scalarMultiply(1. / sumw);
    mueff = sumw * sumw / sumwq; // variance-effectiveness of sum w_i x_i

    // initialize dynamic strategy parameters and constants
    cc = (4. + mueff / dimension) /
            (dimension + 4. + 2. * mueff / dimension);
    cs = (mueff + 2.) / (dimension + mueff + 3.);
    damps = (1. + 2. * Math.max(0, Math.sqrt((mueff - 1.) /
            (dimension + 1.)) - 1.)) *
            Math.max(0.3, 1. - dimension /
                    (1e-6 + Math.min(maxIterations, getMaxEvaluations() /
                            lambda))) + cs; // minor increment
    ccov1 = 2. / ((dimension + 1.3) * (dimension + 1.3) + mueff);
    ccovmu = Math.min(1 - ccov1, 2. * (mueff - 2. + 1. / mueff) /
            ((dimension + 2.) * (dimension + 2.) + mueff));
    ccov1Sep = Math.min(1, ccov1 * (dimension + 1.5) / 3.);
    ccovmuSep = Math.min(1 - ccov1, ccovmu * (dimension + 1.5) / 3.);
    chiN = Math.sqrt(dimension) *
            (1. - 1. / (4. * dimension) + 1 / (21. * dimension * dimension));
    // intialize CMA internal values - updated each generation
    xmean = MatrixUtils.createColumnRealMatrix(guess); // objective
                                                       // variables
    diagD = insigma.scalarMultiply(1. / sigma);
    diagC = square(diagD);
    pc = zeros(dimension, 1); // evolution paths for C and sigma
    ps = zeros(dimension, 1); // B defines the coordinate system
    normps = ps.getFrobeniusNorm();

    B = eye(dimension, dimension);
    D = ones(dimension, 1); // diagonal D defines the scaling
    BD = times(B, repmat(diagD.transpose(), dimension, 1));
    C = B.multiply(diag(square(D)).multiply(B.transpose())); // covariance
    historySize = 10 + (int) (3. * 10. * dimension / lambda);
    fitnessHistory = new double[historySize]; // history of fitness values
    for (int i = 0; i < historySize; i++) {
        fitnessHistory[i] = Double.MAX_VALUE;
    }
}