Java RealVector.subtract方法代码示例

import org.apache.commons.math.linear.RealVector; //导入方法依赖的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.math.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 CholeskyDecompositionImpl(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.math.linear.RealVector; //导入方法依赖的package包/类
/**
 * Calculates the ZID vector. (Compare the python code).
 * @return the ZID vector
 */
protected RealVector calculateZID() {
	double[] bSubArray = new double[numberOfCoefficients-1];
	double[] aSubArray = new double[numberOfCoefficients-1];

	System.arraycopy(bCoefficients, 1, bSubArray, 0, bSubArray.length);
	System.arraycopy(aCoefficients, 1, aSubArray, 0, aSubArray.length);

	RealVector bVector = new ArrayRealVector(bSubArray);
	RealVector aVector = new ArrayRealVector(aSubArray);

	RealVector zid = bVector.subtract(aVector.mapMultiply(bCoefficients[0]));
	return zid;
}
 

import org.apache.commons.math.linear.RealVector; //导入方法依赖的package包/类
/**
 * Build a constraint involving two linear equations.
 * <p>
 * A linear constraint with two linear equation has one of the forms:
 * <ul>
 *   <li>l<sub>1</sub>x<sub>1</sub> + ... l<sub>n</sub>x<sub>n</sub> + l<sub>cst</sub> =
 *       r<sub>1</sub>x<sub>1</sub> + ... r<sub>n</sub>x<sub>n</sub> + r<sub>cst</sub></li>
 *   <li>l<sub>1</sub>x<sub>1</sub> + ... l<sub>n</sub>x<sub>n</sub> + l<sub>cst</sub> &lt;=
 *       r<sub>1</sub>x<sub>1</sub> + ... r<sub>n</sub>x<sub>n</sub> + r<sub>cst</sub></li>
 *   <li>l<sub>1</sub>x<sub>1</sub> + ... l<sub>n</sub>x<sub>n</sub> + l<sub>cst</sub> >=
 *       r<sub>1</sub>x<sub>1</sub> + ... r<sub>n</sub>x<sub>n</sub> + r<sub>cst</sub></li>
 * </ul>
 * </p>
 * @param lhsCoefficients The coefficients of the linear expression on the left hand side of the constraint
 * @param lhsConstant The constant term of the linear expression on the left hand side of the constraint
 * @param relationship The type of (in)equality used in the constraint
 * @param rhsCoefficients The coefficients of the linear expression on the right hand side of the constraint
 * @param rhsConstant The constant term of the linear expression on the right hand side of the constraint
 */
public LinearConstraint(final RealVector lhsCoefficients, final double lhsConstant,
                        final Relationship relationship,
                        final RealVector rhsCoefficients, final double rhsConstant) {
    this.coefficients = lhsCoefficients.subtract(rhsCoefficients);
    this.relationship = relationship;
    this.value        = rhsConstant - lhsConstant;
}