Examples with QRDecomposition org.hipparchus.linear.QRDecomposition used on opensource projects

Example 1 with QRDecomposition

use of org.hipparchus.linear.QRDecomposition in project Orekit by CS-SI.

the class JacobiansMapper method setInitialJacobians.

/**
 * Set the Jacobian with respect to state into a one-dimensional additional state array.
 * <p>
 * This method converts the Jacobians to Cartesian parameters and put the converted data
 * in the one-dimensional {@code p} array.
 * </p>
 * @param state spacecraft state
 * @param dY1dY0 Jacobian of current state at time t₁
 * with respect to state at some previous time t₀
 * @param dY1dP Jacobian of current state at time t₁
 * with respect to parameters (may be null if there are no parameters)
 * @param p placeholder where to put the one-dimensional additional state
 * @see #getStateJacobian(SpacecraftState, double[][])
 */
void setInitialJacobians(final SpacecraftState state, final double[][] dY1dY0, final double[][] dY1dP, final double[] p) {
    // set up a converter between state parameters and Cartesian parameters
    final RealMatrix dY1dC1 = new Array2DRowRealMatrix(getdYdC(state), false);
    final DecompositionSolver solver = new QRDecomposition(dY1dC1).getSolver();
    // convert the provided state Jacobian to Cartesian parameters
    final RealMatrix dC1dY0 = solver.solve(new Array2DRowRealMatrix(dY1dY0, false));
    // map the converted state Jacobian to one-dimensional array
    int index = 0;
    for (int i = 0; i < STATE_DIMENSION; ++i) {
        for (int j = 0; j < STATE_DIMENSION; ++j) {
            p[index++] = dC1dY0.getEntry(i, j);
        }
    }
    if (parameters.getNbParams() != 0) {
        // convert the provided state Jacobian to Cartesian parameters
        final RealMatrix dC1dP = solver.solve(new Array2DRowRealMatrix(dY1dP, false));
        // map the converted parameters Jacobian to one-dimensional array
        for (int i = 0; i < STATE_DIMENSION; ++i) {
            for (int j = 0; j < parameters.getNbParams(); ++j) {
                p[index++] = dC1dP.getEntry(i, j);
            }
        }
    }
}

Example 2 with QRDecomposition

use of org.hipparchus.linear.QRDecomposition in project Orekit by CS-SI.

the class AngularCoordinates method inverseCrossProducts.

/**
 * Find a vector from two known cross products.
 * <p>
 * We want to find Ω such that: Ω ⨯ v₁ = c₁ and Ω ⨯ v₂ = c₂
 * </p>
 * <p>
 * The first equation (Ω ⨯ v₁ = c₁) will always be fulfilled exactly,
 * and the second one will be fulfilled if possible.
 * </p>
 * @param v1 vector forming the first known cross product
 * @param c1 know vector for cross product Ω ⨯ v₁
 * @param v2 vector forming the second known cross product
 * @param c2 know vector for cross product Ω ⨯ v₂
 * @param tolerance relative tolerance factor used to check singularities
 * @return vector Ω such that: Ω ⨯ v₁ = c₁ and Ω ⨯ v₂ = c₂
 * @exception MathIllegalArgumentException if vectors are inconsistent and
 * no solution can be found
 */
private static Vector3D inverseCrossProducts(final Vector3D v1, final Vector3D c1, final Vector3D v2, final Vector3D c2, final double tolerance) throws MathIllegalArgumentException {
    final double v12 = v1.getNormSq();
    final double v1n = FastMath.sqrt(v12);
    final double v22 = v2.getNormSq();
    final double v2n = FastMath.sqrt(v22);
    final double threshold = tolerance * FastMath.max(v1n, v2n);
    Vector3D omega;
    try {
        // create the over-determined linear system representing the two cross products
        final RealMatrix m = MatrixUtils.createRealMatrix(6, 3);
        m.setEntry(0, 1, v1.getZ());
        m.setEntry(0, 2, -v1.getY());
        m.setEntry(1, 0, -v1.getZ());
        m.setEntry(1, 2, v1.getX());
        m.setEntry(2, 0, v1.getY());
        m.setEntry(2, 1, -v1.getX());
        m.setEntry(3, 1, v2.getZ());
        m.setEntry(3, 2, -v2.getY());
        m.setEntry(4, 0, -v2.getZ());
        m.setEntry(4, 2, v2.getX());
        m.setEntry(5, 0, v2.getY());
        m.setEntry(5, 1, -v2.getX());
        final RealVector rhs = MatrixUtils.createRealVector(new double[] { c1.getX(), c1.getY(), c1.getZ(), c2.getX(), c2.getY(), c2.getZ() });
        // find the best solution we can
        final DecompositionSolver solver = new QRDecomposition(m, threshold).getSolver();
        final RealVector v = solver.solve(rhs);
        omega = new Vector3D(v.getEntry(0), v.getEntry(1), v.getEntry(2));
    } catch (MathIllegalArgumentException miae) {
        if (miae.getSpecifier() == LocalizedCoreFormats.SINGULAR_MATRIX) {
            // handle some special cases for which we can compute a solution
            final double c12 = c1.getNormSq();
            final double c1n = FastMath.sqrt(c12);
            final double c22 = c2.getNormSq();
            final double c2n = FastMath.sqrt(c22);
            if (c1n <= threshold && c2n <= threshold) {
                // simple special case, velocities are cancelled
                return Vector3D.ZERO;
            } else if (v1n <= threshold && c1n >= threshold) {
                // this is inconsistent, if v₁ is zero, c₁ must be 0 too
                throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, c1n, 0, true);
            } else if (v2n <= threshold && c2n >= threshold) {
                // this is inconsistent, if v₂ is zero, c₂ must be 0 too
                throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, c2n, 0, true);
            } else if (Vector3D.crossProduct(v1, v2).getNorm() <= threshold && v12 > threshold) {
                // simple special case, v₂ is redundant with v₁, we just ignore it
                // use the simplest Ω: orthogonal to both v₁ and c₁
                omega = new Vector3D(1.0 / v12, Vector3D.crossProduct(v1, c1));
            } else {
                throw miae;
            }
        } else {
            throw miae;
        }
    }
    // check results
    final double d1 = Vector3D.distance(Vector3D.crossProduct(omega, v1), c1);
    if (d1 > threshold) {
        throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, d1, 0, true);
    }
    final double d2 = Vector3D.distance(Vector3D.crossProduct(omega, v2), c2);
    if (d2 > threshold) {
        throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, d2, 0, true);
    }
    return omega;
}

Example 3 with QRDecomposition

use of org.hipparchus.linear.QRDecomposition in project Orekit by CS-SI.

the class Orbit method createInverseJacobian.

/**
 * Create an inverse Jacobian.
 * @param type type of the position angle to use
 * @return inverse Jacobian
 */
private double[][] createInverseJacobian(final PositionAngle type) {
    // get the direct Jacobian
    final double[][] directJacobian = new double[6][6];
    getJacobianWrtCartesian(type, directJacobian);
    // invert the direct Jacobian
    final RealMatrix matrix = MatrixUtils.createRealMatrix(directJacobian);
    final DecompositionSolver solver = new QRDecomposition(matrix).getSolver();
    return solver.getInverse().getData();
}