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

Example 21 with RealMatrix

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

the class DoubleFormFactory method convertList.

public void convertList(final StringBuilder buf, final IAST list) {
    if (list instanceof ASTRealVector) {
        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('}');
        return;
    }
    if (list instanceof ASTRealMatrix) {
        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('}');
        return;
    }
    if (list.isEvalFlagOn(IAST.IS_MATRIX)) {
        if (!fEmpty) {
            newLine(buf);
        }
    }
    append(buf, "{");
    final int listSize = list.size();
    if (listSize > 1) {
        convertInternal(buf, list.arg1());
    }
    for (int i = 2; i < listSize; i++) {
        append(buf, ",");
        if (list.isEvalFlagOn(IAST.IS_MATRIX)) {
            newLine(buf);
            append(buf, ' ');
        }
        convertInternal(buf, list.get(i));
    }
    append(buf, "}");
}

Example 22 with RealMatrix

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

the class InterpolatingFunction method interpolate.

private HermiteInterpolator interpolate(IAST matrixAST) {
    HermiteInterpolator interpolator = (HermiteInterpolator) Config.EXPR_CACHE.getIfPresent(matrixAST);
    if (interpolator != null) {
        return interpolator;
    }
    RealMatrix matrix = matrixAST.toRealMatrix();
    if (matrix != null) {
        int rowDim = matrix.getRowDimension();
        int colDim = matrix.getColumnDimension();
        double[] x = new double[colDim - 1];
        double[][] data = matrix.getData();
        interpolator = new HermiteInterpolator();
        for (int i = 0; i < rowDim; i++) {
            System.arraycopy(data[i], 1, x, 0, colDim - 1);
            interpolator.addSamplePoint(data[i][0], x);
        }
        Config.EXPR_CACHE.put(matrixAST, interpolator);
        return interpolator;
    }
    return null;
}

Example 23 with RealMatrix

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

the class DOPComputer method compute.

/**
 * Compute the {@link DOP} at a given date for a set of GNSS spacecrafts.
 * <p>Four GNSS spacecraft at least are needed to compute the DOP.
 * If less than 4 propagators are provided, an exception will be thrown.
 * If less than 4 spacecrafts are visible at the date, all DOP values will be
 * set to {@link java.lang.Double#NaN NaN}.</p>
 *
 * @param date the computation date
 * @param gnss the propagators for GNSS spacecraft involved in the DOP computation
 * @return the {@link DOP} at the location
 * @throws OrekitException if something wrong occurs
 */
public DOP compute(final AbsoluteDate date, final List<Propagator> gnss) throws OrekitException {
    // Checks the number of provided propagators
    if (gnss.size() < DOP_MIN_PROPAGATORS) {
        throw new OrekitException(OrekitMessages.NOT_ENOUGH_GNSS_FOR_DOP, gnss.size(), DOP_MIN_PROPAGATORS);
    }
    // Initializes DOP values
    double gdop = Double.NaN;
    double pdop = Double.NaN;
    double hdop = Double.NaN;
    double vdop = Double.NaN;
    double tdop = Double.NaN;
    // Loop over the propagators of GNSS orbits
    final double[][] satDir = new double[gnss.size()][4];
    int satNb = 0;
    for (Propagator prop : gnss) {
        final Vector3D pos = prop.getPVCoordinates(date, frame).getPosition();
        final double elev = frame.getElevation(pos, frame, date);
        final double elMin = (elevationMask != null) ? elevationMask.getElevation(frame.getAzimuth(pos, frame, date)) : minElevation;
        // Only visible satellites are considered
        if (elev > elMin) {
            // Create the rows of the H matrix
            final Vector3D r = pos.normalize();
            satDir[satNb][0] = r.getX();
            satDir[satNb][1] = r.getY();
            satDir[satNb][2] = r.getZ();
            satDir[satNb][3] = -1.;
            satNb++;
        }
    }
    // DOP values are computed only if at least 4 SV are visible from the location
    if (satNb > 3) {
        // Construct matrix H
        final RealMatrix h = MatrixUtils.createRealMatrix(satNb, 4);
        for (int k = 0; k < satNb; k++) {
            h.setRow(k, satDir[k]);
        }
        // Compute the pseudo-inverse of H
        final RealMatrix hInv = MatrixUtils.inverse(h.transpose().multiply(h));
        final double sx2 = hInv.getEntry(0, 0);
        final double sy2 = hInv.getEntry(1, 1);
        final double sz2 = hInv.getEntry(2, 2);
        final double st2 = hInv.getEntry(3, 3);
        // Extract various DOP : GDOP, PDOP, HDOP, VDOP, TDOP
        gdop = FastMath.sqrt(hInv.getTrace());
        pdop = FastMath.sqrt(sx2 + sy2 + sz2);
        hdop = FastMath.sqrt(sx2 + sy2);
        vdop = FastMath.sqrt(sz2);
        tdop = FastMath.sqrt(st2);
    }
    // Return all the DOP values
    return new DOP(frame.getPoint(), date, satNb, gdop, pdop, hdop, vdop, tdop);
}

Example 24 with RealMatrix

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

the class KalmanEstimatorTest method testCartesianRangeRate.

/**
 * Perfect range rate measurements with a perfect start
 * Cartesian formalism
 * @throws OrekitException
 */
@Test
public void testCartesianRangeRate() throws OrekitException {
    // Create context
    Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    // Create initial orbit and propagator builder
    final OrbitType orbitType = OrbitType.CARTESIAN;
    final PositionAngle positionAngle = PositionAngle.TRUE;
    final boolean perfectStart = true;
    final double minStep = 1.e-6;
    final double maxStep = 60.;
    final double dP = 1.;
    final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(orbitType, positionAngle, perfectStart, minStep, maxStep, dP);
    // Create perfect range measurements
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new RangeRateMeasurementCreator(context, false), 1.0, 3.0, 300.0);
    // Reference propagator for estimation performances
    final NumericalPropagator referencePropagator = propagatorBuilder.buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
    // Reference position/velocity at last measurement date
    final Orbit refOrbit = referencePropagator.propagate(measurements.get(measurements.size() - 1).getDate()).getOrbit();
    // Cartesian covariance matrix initialization
    // 100m on position / 1e-2m/s on velocity
    final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double[] { 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10 });
    // Jacobian of the orbital parameters w/r to Cartesian
    final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
    final double[][] dYdC = new double[6][6];
    initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
    final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
    // Initial covariance matrix
    final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
    // Process noise matrix
    final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double[] { 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12 });
    final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
    // Build the Kalman filter
    final KalmanEstimatorBuilder kalmanBuilder = new KalmanEstimatorBuilder();
    kalmanBuilder.builder(propagatorBuilder);
    kalmanBuilder.estimatedMeasurementsParameters(new ParameterDriversList());
    kalmanBuilder.initialCovarianceMatrix(initialP);
    kalmanBuilder.processNoiseMatrixProvider(new ConstantProcessNoise(Q));
    final KalmanEstimator kalman = kalmanBuilder.build();
    // Filter the measurements and check the results
    final double expectedDeltaPos = 0.;
    final double posEps = 9.50e-4;
    final double expectedDeltaVel = 0.;
    final double velEps = 3.49e-7;
    final double[] expectedSigmasPos = { 0.324398, 1.347031, 1.743310 };
    final double sigmaPosEps = 1e-6;
    final double[] expectedSigmasVel = { 2.856883e-4, 5.765844e-4, 5.056186e-4 };
    final double sigmaVelEps = 1e-10;
    EstimationTestUtils.checkKalmanFit(context, kalman, measurements, refOrbit, positionAngle, expectedDeltaPos, posEps, expectedDeltaVel, velEps, expectedSigmasPos, sigmaPosEps, expectedSigmasVel, sigmaVelEps);
}

Example 25 with RealMatrix

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

the class KalmanEstimatorTest method testCircularAzimuthElevation.

/**
 * Perfect azimuth/elevation measurements with a perfect start
 * Circular formalism
 * @throws OrekitException
 */
@Test
public void testCircularAzimuthElevation() throws OrekitException {
    // Create context
    Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    // Create initial orbit and propagator builder
    final OrbitType orbitType = OrbitType.CIRCULAR;
    final PositionAngle positionAngle = PositionAngle.TRUE;
    final boolean perfectStart = true;
    final double minStep = 1.e-6;
    final double maxStep = 60.;
    final double dP = 1.;
    final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(orbitType, positionAngle, perfectStart, minStep, maxStep, dP);
    // Create perfect range measurements
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new AngularAzElMeasurementCreator(context), 1.0, 4.0, 60.0);
    // Reference propagator for estimation performances
    final NumericalPropagator referencePropagator = propagatorBuilder.buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
    // Reference position/velocity at last measurement date
    final Orbit refOrbit = referencePropagator.propagate(measurements.get(measurements.size() - 1).getDate()).getOrbit();
    // Cartesian covariance matrix initialization
    final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double[] { 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10 });
    // Jacobian of the orbital parameters w/r to Cartesian
    final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
    final double[][] dYdC = new double[6][6];
    initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
    final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
    // Initial covariance matrix
    final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
    // Process noise matrix
    final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double[] { 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12 });
    final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
    // Build the Kalman filter
    final KalmanEstimatorBuilder kalmanBuilder = new KalmanEstimatorBuilder();
    kalmanBuilder.builder(propagatorBuilder);
    kalmanBuilder.estimatedMeasurementsParameters(new ParameterDriversList());
    kalmanBuilder.initialCovarianceMatrix(initialP);
    kalmanBuilder.processNoiseMatrixProvider(new ConstantProcessNoise(Q));
    final KalmanEstimator kalman = kalmanBuilder.build();
    // Filter the measurements and check the results
    final double expectedDeltaPos = 0.;
    final double posEps = 4.78e-7;
    final double expectedDeltaVel = 0.;
    final double velEps = 1.54e-10;
    final double[] expectedSigmasPos = { 0.356902, 1.297507, 1.798551 };
    final double sigmaPosEps = 1e-6;
    final double[] expectedSigmasVel = { 2.468745e-4, 5.810027e-4, 3.887394e-4 };
    final double sigmaVelEps = 1e-10;
    EstimationTestUtils.checkKalmanFit(context, kalman, measurements, refOrbit, positionAngle, expectedDeltaPos, posEps, expectedDeltaVel, velEps, expectedSigmasPos, sigmaPosEps, expectedSigmasVel, sigmaVelEps);
}