Java Code Examples for org.apache.commons.math.linear.RealVector#getDimension()

/**
 * Predict the internal state estimation one time step ahead.
 *
 * @param u
 *            the control vector
 * @throws DimensionMismatchException
 *             if the dimension of the control vector does not fit
 */
public void predict(final RealVector u) {
    // sanity checks
    if (u != null &&
        u.getDimension() != controlMatrix.getColumnDimension()) {
        throw new DimensionMismatchException(u.getDimension(),
                                             controlMatrix.getColumnDimension());
    }

    // project the state estimation ahead (a priori state)
    // xHat(k)- = A * xHat(k-1) + B * u(k-1)
    stateEstimation = transitionMatrix.operate(stateEstimation);

    // add control input if it is available
    if (u != null) {
        stateEstimation = stateEstimation.add(controlMatrix.operate(u));
    }

    // project the error covariance ahead
    // P(k)- = A * P(k-1) * A' + Q
    errorCovariance = transitionMatrix.multiply(errorCovariance)
            .multiply(transitionMatrixT)
            .add(processModel.getProcessNoise());
}
 

/**
 * Predict the internal state estimation one time step ahead.
 *
 * @param u
 *            the control vector
 * @throws DimensionMismatchException
 *             if the dimension of the control vector does not fit
 */
public void predict(final RealVector u) {
    // sanity checks
    if (u != null &&
        u.getDimension() != controlMatrix.getColumnDimension()) {
        throw new DimensionMismatchException(u.getDimension(),
                                             controlMatrix.getColumnDimension());
    }

    // project the state estimation ahead (a priori state)
    // xHat(k)- = A * xHat(k-1) + B * u(k-1)
    stateEstimation = transitionMatrix.operate(stateEstimation);

    // add control input if it is available
    if (u != null) {
        stateEstimation = stateEstimation.add(controlMatrix.operate(u));
    }

    // project the error covariance ahead
    // P(k)- = A * P(k-1) * A' + Q
    errorCovariance = transitionMatrix.multiply(errorCovariance)
            .multiply(transitionMatrixT)
            .add(processModel.getProcessNoise());
}
 

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

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

@Test
public void testSparseVectors() throws IOException, ParseException
{
  RandomGenerator rg = new JDKRandomGenerator();
  rg.setSeed(0);
  RandomData rd = new RandomDataImpl(rg);
  int n = 20;
  List<RealVector> vectors = LSHTest.getVectors(rd, 1000, n);
  PigTest test = createPigTestFromString(sparseVectorTest);
  writeLinesToFile("input", getSparseLines(vectors));
  test.runScript();
  List<Tuple> neighbors = this.getLinesForAlias(test, "PTS");
  Assert.assertEquals(neighbors.size(), n);
  int idx = 0;
  for(Tuple t : neighbors)
  {
    Assert.assertTrue(t.get(0) instanceof DataBag);
    Assert.assertEquals(t.size(), 1);
    RealVector interpreted = DataTypeUtil.INSTANCE.convert(t, 3);
    RealVector original = vectors.get(idx);
    Assert.assertEquals(original.getDimension(), interpreted.getDimension());
    for(int i = 0;i < interpreted.getDimension();++i)
    {
      double originalField = original.getEntry(i);
      double interpretedField = interpreted.getEntry(i);
      Assert.assertTrue(Math.abs(originalField - interpretedField) < 1e-5);
    }

    idx++;
  }
}