Examples with RealVector org.apache.commons.math3.linear.RealVector used on opensource projects

Example 41 with RealVector

use of org.apache.commons.math3.linear.RealVector in project imagingbook-common by imagingbook.

the class Matrix method solve.

// use general method, i.e. double[][] inverse(double[][] A)
// @Deprecated
// public static double[][] inverse3x3(final double[][] A) {
// double[][] B = duplicate(A);
// final double det = determinant3x3(B);
// if (Math.abs(det) < Arithmetic.EPSILON_DOUBLE)
// return null;
// else {
// final double a00 = B[0][0];
// final double a01 = B[0][1];
// final double a02 = B[0][2];
// final double a10 = B[1][0];
// final double a11 = B[1][1];
// final double a12 = B[1][2];
// final double a20 = B[2][0];
// final double a21 = B[2][1];
// final double a22 = B[2][2];
// B[0][0] = (a11 * a22 - a12 * a21) / det;
// B[0][1] = (a02 * a21 - a01 * a22) / det;
// B[0][2] = (a01 * a12 - a02 * a11) / det;
// 
// B[1][0] = (a12 * a20 - a10 * a22) / det;
// B[1][1] = (a00 * a22 - a02 * a20) / det;
// B[1][2] = (a02 * a10 - a00 * a12) / det;
// 
// B[2][0] = (a10 * a21 - a11 * a20) / det;
// B[2][1] = (a01 * a20 - a00 * a21) / det;
// B[2][2] = (a00 * a11 - a01 * a10) / det;
// return B;
// }
// }
// numerically stable? should be replaced by standard inversion
// @Deprecated
// public static float[][] inverse3x3(final float[][] A) {
// final float[][] B = duplicate(A);
// final double det = determinant3x3(B);
// // IJ.log("   determinant = " + det);
// if (Math.abs(det) < Arithmetic.EPSILON_DOUBLE)
// return null;
// else {
// final double a00 = B[0][0];
// final double a01 = B[0][1];
// final double a02 = B[0][2];
// final double a10 = B[1][0];
// final double a11 = B[1][1];
// final double a12 = B[1][2];
// final double a20 = B[2][0];
// final double a21 = B[2][1];
// final double a22 = B[2][2];
// B[0][0] = (float) ((a11 * a22 - a12 * a21) / det);
// B[0][1] = (float) ((a02 * a21 - a01 * a22) / det);
// B[0][2] = (float) ((a01 * a12 - a02 * a11) / det);
// 
// B[1][0] = (float) ((a12 * a20 - a10 * a22) / det);
// B[1][1] = (float) ((a00 * a22 - a02 * a20) / det);
// B[1][2] = (float) ((a02 * a10 - a00 * a12) / det);
// 
// B[2][0] = (float) ((a10 * a21 - a11 * a20) / det);
// B[2][1] = (float) ((a01 * a20 - a00 * a21) / det);
// B[2][2] = (float) ((a00 * a11 - a01 * a10) / det);
// return B;
// }
// }
// ------------------------------------------------------------------------
// Finds the EXACT solution x for A.x = b
public static double[] solve(final double[][] A, double[] b) {
    RealMatrix AA = MatrixUtils.createRealMatrix(A);
    RealVector bb = MatrixUtils.createRealVector(b);
    DecompositionSolver solver = new LUDecomposition(AA).getSolver();
    double[] x = null;
    try {
        x = solver.solve(bb).toArray();
    } catch (SingularMatrixException e) {
    }
    return x;
}

Example 42 with RealVector

use of org.apache.commons.math3.linear.RealVector in project FSensor by KalebKE.

the class RotationKalmanFilter method correct.

/**
 * 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 43 with RealVector

use of org.apache.commons.math3.linear.RealVector in project FSensor by KalebKE.

the class FitPoints method solveSystem.

/**
 * Solve the polynomial expression Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz +
 * 2Gx + 2Hy + 2Iz from the provided points.
 *
 * @param points the points that will be fit to the polynomial expression.
 * @return the solution vector to the polynomial expression.
 */
private RealVector solveSystem(ArrayList<ThreeSpacePoint> points) {
    // determine the number of points
    int numPoints = points.size();
    // the design matrix
    // size: numPoints x 9
    RealMatrix d = new Array2DRowRealMatrix(numPoints, 9);
    // Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz
    for (int i = 0; i < d.getRowDimension(); i++) {
        double xx = Math.pow(points.get(i).x, 2);
        double yy = Math.pow(points.get(i).y, 2);
        double zz = Math.pow(points.get(i).z, 2);
        double xy = 2 * (points.get(i).x * points.get(i).y);
        double xz = 2 * (points.get(i).x * points.get(i).z);
        double yz = 2 * (points.get(i).y * points.get(i).z);
        double x = 2 * points.get(i).x;
        double y = 2 * points.get(i).y;
        double z = 2 * points.get(i).z;
        d.setEntry(i, 0, xx);
        d.setEntry(i, 1, yy);
        d.setEntry(i, 2, zz);
        d.setEntry(i, 3, xy);
        d.setEntry(i, 4, xz);
        d.setEntry(i, 5, yz);
        d.setEntry(i, 6, x);
        d.setEntry(i, 7, y);
        d.setEntry(i, 8, z);
    }
    // solve the normal system of equations
    // v = (( d' * d )^-1) * ( d' * ones.mapAddToSelf(1));
    // Multiply: d' * d
    RealMatrix dtd = d.transpose().multiply(d);
    // Create a vector of ones.
    RealVector ones = new ArrayRealVector(numPoints);
    ones.mapAddToSelf(1);
    // Multiply: d' * ones.mapAddToSelf(1)
    RealVector dtOnes = d.transpose().operate(ones);
    // Find ( d' * d )^-1
    DecompositionSolver solver = new SingularValueDecomposition(dtd).getSolver();
    RealMatrix dtdi = solver.getInverse();
    // v = (( d' * d )^-1) * ( d' * ones.mapAddToSelf(1));
    RealVector v = dtdi.operate(dtOnes);
    return v;
}

Example 44 with RealVector

use of org.apache.commons.math3.linear.RealVector in project narchy by automenta.

the class HordeAgent method getAtp1.

public A getAtp1(RealVector o_tp1, A a, double r_tp1) {
    /*if (step.isEpisodeStarting())
            x_t = null;*/
    RealVector x_tp1 = projector.project(o_tp1);
    A a_tp1 = control.step(x_t, a, x_tp1, r_tp1);
    horde.update(o_tp1, x_t, a, x_tp1);
    x_t = x_tp1;
    return a_tp1;
}

Example 45 with RealVector

use of org.apache.commons.math3.linear.RealVector in project narchy by automenta.

the class HordeSchedulerTest method testScheduler.

@Test
public void testScheduler() {
    RealVector o_tp1 = new ArrayRealVector(1);
    FakeDemon d1 = new FakeDemon(), d2 = new FakeDemon();
    final List<FakeDemon> demons = Lists.newArrayList(d1, d2);
    final FakeFunction[] beforeFunctions = { new FakeFunction(d1), new FakeFunction(d2) };
    final FakeFunction[] afterFunctions = { new FakeFunction(d1), new FakeFunction(d2) };
    Horde horde = new Horde(new HordeScheduler(3));
    horde.beforeFunctions().addAll(Lists.newArrayList(beforeFunctions));
    horde.demons().addAll(demons);
    horde.afterFunctions().addAll(Lists.newArrayList(afterFunctions));
    final RealVector x0 = new ArrayRealVector(1), x1 = new ArrayRealVector(1);
    final Integer a0 = 0;
    checkFunction(beforeFunctions, null, null, null, null, false);
    checkFunction(afterFunctions, null, null, null, null, false);
    horde.update(o_tp1, x0, a0, x1);
    checkFunction(beforeFunctions, x0, a0, o_tp1, x1, false);
    checkFunction(afterFunctions, x0, a0, o_tp1, x1, true);
    checkDemon(d1, x0, a0, x1);
    checkDemon(d2, x0, a0, x1);
}