org.apache.commons.math3.linear.RealVector#subtract

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

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

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

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

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

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

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

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

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

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