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

Example 16 with RealMatrix

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

the class Model method getErrorStateTransitionMatrix.

/**
 * Get the normalized error state transition matrix (STM) from previous point to current point.
 * The STM contains the partial derivatives of current state with respect to previous state.
 * The  STM is an MxM matrix where M is the size of the state vector.
 * M = nbOrb + nbPropag + nbMeas
 * @param predictedSpacecraftState current spacecraft state
 * @return the normalized error state transition matrix
 * @throws OrekitException if Jacobians cannot be computed
 */
private RealMatrix getErrorStateTransitionMatrix(final SpacecraftState predictedSpacecraftState) throws OrekitException {
    /* The state transition matrix is obtained as follows, with:
         *  - Y  : Current state vector
         *  - Y0 : Initial state vector
         *  - Pp : Current propagation parameter
         *  - Pp0: Initial propagation parameter
         *  - Mp : Current measurement parameter
         *  - Mp0: Initial measurement parameter
         *
         *       |        |         |         |   |        |        |   .    |
         *       | dY/dY0 | dY/dPp  | dY/dMp  |   | dY/dY0 | dY/dPp | ..0..  |
         *       |        |         |         |   |        |        |   .    |
         *       |--------|---------|---------|   |--------|--------|--------|
         *       |        |         |         |   |   .    | 1 0 0..|   .    |
         * STM = | dP/dY0 | dP/dPp0 | dP/dMp  | = | ..0..  | 0 1 0..| ..0..  |
         *       |        |         |         |   |   .    | 0 0 1..|   .    |
         *       |--------|---------|---------|   |--------|--------|--------|
         *       |        |         |         |   |   .    |   .    | 1 0 0..|
         *       | dM/dY0 | dM/dPp0 | dM/dMp0 |   | ..0..  | ..0..  | 0 1 0..|
         *       |        |         |         |   |   .    |   .    | 0 0 1..|
         */
    // Initialize to the proper size identity matrix
    final RealMatrix stm = MatrixUtils.createRealIdentityMatrix(nbOrbitalParameters + nbPropagationParameters + nbMeasurementsParameters);
    // Derivatives of the state vector with respect to initial state vector
    if (nbOrbitalParameters > 0) {
        final double[][] dYdY0 = new double[nbOrbitalParameters][nbOrbitalParameters];
        jacobiansMapper.getStateJacobian(predictedSpacecraftState, dYdY0);
        // Fill upper left corner (dY/dY0)
        stm.setSubMatrix(dYdY0, 0, 0);
    }
    // Derivatives of the state vector with respect to propagation parameters
    if (nbPropagationParameters > 0) {
        final double[][] dYdPp = new double[nbOrbitalParameters][nbPropagationParameters];
        jacobiansMapper.getParametersJacobian(predictedSpacecraftState, dYdPp);
        // Fill 1st row, 2nd column (dY/dPp)
        stm.setSubMatrix(dYdPp, 0, nbOrbitalParameters);
    }
    // Normalization of the STM
    // normalized(STM)ij = STMij*Sj/Si
    final double[] scales = getParametersScale();
    for (int i = 0; i < scales.length; i++) {
        for (int j = 0; j < scales.length; j++) {
            stm.setEntry(i, j, stm.getEntry(i, j) * scales[j] / scales[i]);
        }
    }
    // Return the error state transition matrix
    return stm;
}

Example 17 with RealMatrix

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

the class KalmanEstimator method decorate.

/**
 * Decorate an observed measurement.
 * <p>
 * The "physical" measurement noise matrix is the covariance matrix of the measurement.
 * Normalizing it consists in applying the following equation: Rn[i,j] =  R[i,j]/σ[i]/σ[j]
 * Thus the normalized measurement noise matrix is the matrix of the correlation coefficients
 * between the different components of the measurement.
 * </p>
 * @param observedMeasurement the measurement
 * @return decorated measurement
 */
private MeasurementDecorator decorate(final ObservedMeasurement<?> observedMeasurement) {
    // Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
    // of the measurement on its non-diagonal elements.
    // Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement
    // Normalizing it leaves us with the matrix of the correlation coefficients
    final RealMatrix covariance;
    if (observedMeasurement instanceof PV) {
        // For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
        final PV pv = (PV) observedMeasurement;
        covariance = MatrixUtils.createRealMatrix(pv.getCorrelationCoefficientsMatrix());
    } else {
        // For other measurements we do not have a covariance matrix.
        // Thus the correlation coefficients matrix is an identity matrix.
        covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
    }
    return new MeasurementDecorator(observedMeasurement, covariance, referenceDate);
}

Example 18 with RealMatrix

use of org.hipparchus.linear.RealMatrix 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 19 with RealMatrix

use of org.hipparchus.linear.RealMatrix in project symja_android_library by axkr.

the class AbstractMatrix1Matrix method numericEval.

@Override
public IExpr numericEval(final IAST ast, final EvalEngine engine) {
    RealMatrix matrix;
    boolean togetherMode = engine.isTogetherMode();
    try {
        engine.setTogetherMode(true);
        int[] dims = checkMatrixDimensions(ast.arg1());
        if (dims != null) {
            if (engine.isArbitraryMode()) {
                FieldMatrix<IExpr> fieldMatrix = Convert.list2Matrix(ast.arg1());
                if (fieldMatrix == null) {
                    return F.NIL;
                }
                Predicate<IExpr> zeroChecker = AbstractMatrix1Expr.optionZeroTest(ast, 2, engine);
                fieldMatrix = matrixEval(fieldMatrix, zeroChecker);
                return Convert.matrix2List(fieldMatrix);
            }
            matrix = ast.arg1().toRealMatrix();
            if (matrix != null) {
                matrix = realMatrixEval(matrix);
                if (matrix != null) {
                    return Convert.realMatrix2List(matrix);
                }
            }
        }
        return F.NIL;
    } catch (LimitException le) {
        throw le;
    } catch (final RuntimeException rex) {
        LOGGER.log(engine.getLogLevel(), ast.topHead(), rex);
        return F.NIL;
    } finally {
        engine.setTogetherMode(togetherMode);
    }
// return evaluate(ast, engine);
}

Example 20 with RealMatrix

use of org.hipparchus.linear.RealMatrix in project symja_android_library by axkr.

the class OutputFormFactory method convertList.

public void convertList(final Appendable buf, final IAST list, boolean isMatrix) throws IOException {
    if (list instanceof ASTRealVector) {
        try {
            RealVector vector = ((ASTRealVector) list).getRealVector();
            buf.append('{');
            int size = vector.getDimension();
            for (int i = 0; i < size; i++) {
                convertDouble(buf, vector.getEntry(i));
                if (i < size - 1) {
                    buf.append(",");
                }
            }
            buf.append('}');
        } catch (IOException e) {
            LOGGER.debug("OutputFormFactory.convertList() failed", e);
        }
        return;
    }
    if (list instanceof ASTRealMatrix) {
        try {
            RealMatrix matrix = ((ASTRealMatrix) list).getRealMatrix();
            buf.append('{');
            int rows = matrix.getRowDimension();
            int cols = matrix.getColumnDimension();
            for (int i = 0; i < rows; i++) {
                if (i != 0) {
                    buf.append(" ");
                }
                buf.append("{");
                for (int j = 0; j < cols; j++) {
                    convertDouble(buf, matrix.getEntry(i, j));
                    if (j < cols - 1) {
                        buf.append(",");
                    }
                }
                buf.append('}');
                if (i < rows - 1) {
                    buf.append(",");
                    buf.append('\n');
                }
            }
            buf.append('}');
        } catch (IOException e) {
            LOGGER.debug("OutputFormFactory.convertList() failed", e);
        }
        return;
    }
    if (list.isEvalFlagOn(IAST.IS_MATRIX) || isMatrix) {
        if (!fEmpty) {
            newLine(buf);
        }
    }
    append(buf, "{");
    final int listSize = list.size();
    if (listSize > 1) {
        convert(buf, list.arg1(), Integer.MIN_VALUE, false);
    }
    for (int i = 2; i < listSize; i++) {
        append(buf, ",");
        if (list.isEvalFlagOn(IAST.IS_MATRIX) || isMatrix) {
            newLine(buf);
            append(buf, ' ');
        }
        convert(buf, list.get(i), Integer.MIN_VALUE, false);
    }
    append(buf, "}");
}