Java Code Examples for org.apache.commons.math3.linear.RealVector#subtract()

The following examples show how to use org.apache.commons.math3.linear.RealVector#subtract() . 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: StatsUtils.java From incubator-hivemall with Apache License 2.0

6 votes

/**
 * @param mu1 mean vector of the first normal distribution
 * @param sigma1 covariance matrix of the first normal distribution
 * @param mu2 mean vector of the second normal distribution
 * @param sigma2 covariance matrix of the second normal distribution
 * @return the Hellinger distance between two multivariate normal distributions
 * @link https://en.wikipedia.org/wiki/Hellinger_distance#Examples
 */
public static double hellingerDistance(@Nonnull final RealVector mu1,
        @Nonnull final RealMatrix sigma1, @Nonnull final RealVector mu2,
        @Nonnull final RealMatrix sigma2) {
    RealVector muSub = mu1.subtract(mu2);
    RealMatrix sigmaMean = sigma1.add(sigma2).scalarMultiply(0.5d);
    LUDecomposition LUsigmaMean = new LUDecomposition(sigmaMean);
    double denominator = Math.sqrt(LUsigmaMean.getDeterminant());
    if (denominator == 0.d) {
        return 1.d; // avoid divide by zero
    }
    RealMatrix sigmaMeanInv = LUsigmaMean.getSolver().getInverse(); // has inverse iff det != 0
    double sigma1Det = MatrixUtils.det(sigma1);
    double sigma2Det = MatrixUtils.det(sigma2);

    double numerator = Math.pow(sigma1Det, 0.25d) * Math.pow(sigma2Det, 0.25d)
            * Math.exp(-0.125d * sigmaMeanInv.preMultiply(muSub).dotProduct(muSub));
    return 1.d - numerator / denominator;
}

Example 2

Source File: KalmanFilter.java From astor with GNU General Public License v2.0

5 votes

/**
 * 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 3

Source File: StatsUtils.java From incubator-hivemall with Apache License 2.0

5 votes

/**
 * 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;
}

Example 4

Source File: MultivariateTDistribution.java From macrobase with Apache License 2.0

5 votes

public double density(RealVector vector) {
    if (dof == 0) {
        return 0;
    }
    RealVector _diff = vector.subtract(mean);
    double prob = 1. / dof * _diff.dotProduct(precisionMatrix.operate(_diff));
    return multiplier * Math.pow(1 + prob, -(dof + D) / 2);
}

Example 5

Source File: LeastSquaresFactory.java From astor with GNU General Public License v2.0

5 votes

/**
 * Create an {@link Evaluation} with no weights.
 *
 * @param values   the computed function values
 * @param jacobian the computed function Jacobian
 * @param target   the observed values
 * @param point    the abscissa
 */
private UnweightedEvaluation(final RealVector values,
                             final RealMatrix jacobian,
                             final RealVector target,
                             final RealVector point) {
    super(target.getDimension());
    this.jacobian = jacobian;
    this.point = point;
    this.residuals = target.subtract(values);
}

Example 6

Source File: KalmanFilter.java From astor with GNU General Public License v2.0

5 votes

/**
 * 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);
}

Example 7

Source File: KalmanFilter.java From astor with GNU General Public License v2.0

5 votes

/**
 * 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 8

Source File: KalmanFilter.java From astor with GNU General Public License v2.0

5 votes

/**
 * 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 9

Source File: GaussianDPMM.java From DPMM-Clustering with GNU General Public License v3.0

5 votes

/**
 * Updates the cluster's internal parameters based on the stored information.
 */
@Override
protected void updateClusterParameters() {
    int n = pointList.size();
    if(n<=0) {
        return;
    }

    int kappa_n = kappa0 + n;
    int nu = nu0 + n;

    RealVector mu = xi_sum.mapDivide(n);
    RealVector mu_mu_0 = mu.subtract(mu0);

    RealMatrix C = xi_square_sum.subtract( ( mu.outerProduct(mu) ).scalarMultiply(n) );

    RealMatrix psi = psi0.add( C.add( ( mu_mu_0.outerProduct(mu_mu_0) ).scalarMultiply(kappa0*n/(double)kappa_n) ));
    C = null;
    mu_mu_0 = null;

    mean = ( mu0.mapMultiply(kappa0) ).add( mu.mapMultiply(n) ).mapDivide(kappa_n);
    covariance = psi.scalarMultiply(  (kappa_n+1.0)/(kappa_n*(nu - dimensionality + 1.0))  );

    //clear cache
    cache_covariance_determinant=null;
    cache_covariance_inverse=null;

    meanError = calculateMeanError(psi, kappa_n, nu);
    meanDf = Math.max(0, nu-dimensionality+1);
}

Example 10

Source File: KalmanFilter.java From astor with GNU General Public License v2.0

4 votes

/**
 * 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());

    // 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

    // instead of calculating the inverse of S we can rearrange the formula,
    // and then solve the linear equation A x X = B with A = S', X = K' and B = (H * P)'

    // K(k) * S = P(k)- * H'
    // S' * K(k)' = H * P(k)-'
    RealMatrix kalmanGain = new CholeskyDecomposition(s).getSolver()
            .solve(measurementMatrix.multiply(errorCovariance.transpose()))
            .transpose();

    // 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 11