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

Example 1 with RealMatrix

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

the class Roots method findRoots.

/**
	 * <p>
	 * Given a set of polynomial coefficients, compute the roots of the
	 * polynomial. Depending on the polynomial being considered the roots may
	 * contain complex number. When complex numbers are present they will come
	 * in pairs of complex conjugates.
	 * </p>
	 * 
	 * @param coefficients
	 *            coefficients of the polynomial.
	 * @return the roots of the polynomial
	 */
@Nonnull
public static IAST findRoots(double... coefficients) {
    int N = coefficients.length - 1;
    // Construct the companion matrix
    RealMatrix c = new Array2DRowRealMatrix(N, N);
    double a = coefficients[N];
    for (int i = 0; i < N; i++) {
        c.setEntry(i, N - 1, -coefficients[i] / a);
    }
    for (int i = 1; i < N; i++) {
        c.setEntry(i, i - 1, 1);
    }
    try {
        IAST roots = F.List();
        EigenDecomposition ed = new EigenDecomposition(c);
        double[] realValues = ed.getRealEigenvalues();
        double[] imagValues = ed.getImagEigenvalues();
        for (int i = 0; i < N; i++) {
            roots.append(F.chopExpr(F.complexNum(realValues[i], imagValues[i]), Config.DEFAULT_ROOTS_CHOP_DELTA));
        }
        return roots;
    } catch (Exception ime) {
        throw new WrappedException(ime);
    }
}

Example 2 with RealMatrix

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

the class BatchLSEstimatorTest method testKeplerPV.

/**
 * Perfect PV measurements with a perfect start
 * @throws OrekitException
 */
@Test
public void testKeplerPV() throws OrekitException {
    Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, 1.0e-6, 60.0, 1.0);
    // create perfect PV measurements
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new PVMeasurementCreator(), 0.0, 1.0, 300.0);
    // create orbit estimator
    final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(), propagatorBuilder);
    for (final ObservedMeasurement<?> measurement : measurements) {
        estimator.addMeasurement(measurement);
    }
    estimator.setParametersConvergenceThreshold(1.0e-2);
    estimator.setMaxIterations(10);
    estimator.setMaxEvaluations(20);
    EstimationTestUtils.checkFit(context, estimator, 1, 4, 0.0, 2.2e-8, 0.0, 1.1e-7, 0.0, 1.4e-8, 0.0, 6.3e-12);
    RealMatrix normalizedCovariances = estimator.getOptimum().getCovariances(1.0e-10);
    RealMatrix physicalCovariances = estimator.getPhysicalCovariances(1.0e-10);
    Assert.assertEquals(6, normalizedCovariances.getRowDimension());
    Assert.assertEquals(6, normalizedCovariances.getColumnDimension());
    Assert.assertEquals(6, physicalCovariances.getRowDimension());
    Assert.assertEquals(6, physicalCovariances.getColumnDimension());
    Assert.assertEquals(0.00258, physicalCovariances.getEntry(0, 0), 1.0e-5);
}

Example 3 with RealMatrix

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

the class GroundStationTest method testEstimateStationPosition.

@Test
public void testEstimateStationPosition() throws OrekitException, IOException, ClassNotFoundException {
    Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, 1.0e-6, 60.0, 0.001);
    // create perfect range measurements
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new RangeMeasurementCreator(context), 1.0, 3.0, 300.0);
    // move one station
    final RandomGenerator random = new Well19937a(0x4adbecfc743bda60l);
    final TopocentricFrame base = context.stations.get(0).getBaseFrame();
    final BodyShape parent = base.getParentShape();
    final Vector3D baseOrigin = parent.transform(base.getPoint());
    final Vector3D deltaTopo = new Vector3D(2 * random.nextDouble() - 1, 2 * random.nextDouble() - 1, 2 * random.nextDouble() - 1);
    final Transform topoToParent = base.getTransformTo(parent.getBodyFrame(), (AbsoluteDate) null);
    final Vector3D deltaParent = topoToParent.transformVector(deltaTopo);
    final String movedSuffix = "-moved";
    final GroundStation moved = new GroundStation(new TopocentricFrame(parent, parent.transform(baseOrigin.subtract(deltaParent), parent.getBodyFrame(), null), base.getName() + movedSuffix), context.ut1.getEOPHistory(), context.stations.get(0).getDisplacements());
    // create orbit estimator
    final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(), propagatorBuilder);
    for (final ObservedMeasurement<?> measurement : measurements) {
        final Range range = (Range) measurement;
        final String name = range.getStation().getBaseFrame().getName() + movedSuffix;
        if (moved.getBaseFrame().getName().equals(name)) {
            estimator.addMeasurement(new Range(moved, range.getDate(), range.getObservedValue()[0], range.getTheoreticalStandardDeviation()[0], range.getBaseWeight()[0]));
        } else {
            estimator.addMeasurement(range);
        }
    }
    estimator.setParametersConvergenceThreshold(1.0e-3);
    estimator.setMaxIterations(100);
    estimator.setMaxEvaluations(200);
    // we want to estimate station offsets
    moved.getEastOffsetDriver().setSelected(true);
    moved.getNorthOffsetDriver().setSelected(true);
    moved.getZenithOffsetDriver().setSelected(true);
    EstimationTestUtils.checkFit(context, estimator, 2, 3, 0.0, 5.6e-7, 0.0, 1.4e-6, 0.0, 4.8e-7, 0.0, 2.6e-10);
    Assert.assertEquals(deltaTopo.getX(), moved.getEastOffsetDriver().getValue(), 4.5e-7);
    Assert.assertEquals(deltaTopo.getY(), moved.getNorthOffsetDriver().getValue(), 6.2e-7);
    Assert.assertEquals(deltaTopo.getZ(), moved.getZenithOffsetDriver().getValue(), 2.6e-7);
    GeodeticPoint result = moved.getOffsetGeodeticPoint(null);
    GeodeticPoint reference = context.stations.get(0).getBaseFrame().getPoint();
    Assert.assertEquals(reference.getLatitude(), result.getLatitude(), 1.4e-14);
    Assert.assertEquals(reference.getLongitude(), result.getLongitude(), 2.9e-14);
    Assert.assertEquals(reference.getAltitude(), result.getAltitude(), 2.6e-7);
    RealMatrix normalizedCovariances = estimator.getOptimum().getCovariances(1.0e-10);
    RealMatrix physicalCovariances = estimator.getPhysicalCovariances(1.0e-10);
    Assert.assertEquals(9, normalizedCovariances.getRowDimension());
    Assert.assertEquals(9, normalizedCovariances.getColumnDimension());
    Assert.assertEquals(9, physicalCovariances.getRowDimension());
    Assert.assertEquals(9, physicalCovariances.getColumnDimension());
    Assert.assertEquals(0.55431, physicalCovariances.getEntry(6, 6), 1.0e-5);
    Assert.assertEquals(0.22694, physicalCovariances.getEntry(7, 7), 1.0e-5);
    Assert.assertEquals(0.13106, physicalCovariances.getEntry(8, 8), 1.0e-5);
    ByteArrayOutputStream bos = new ByteArrayOutputStream();
    ObjectOutputStream oos = new ObjectOutputStream(bos);
    oos.writeObject(moved.getEstimatedEarthFrame().getTransformProvider());
    Assert.assertTrue(bos.size() > 155000);
    Assert.assertTrue(bos.size() < 160000);
    ByteArrayInputStream bis = new ByteArrayInputStream(bos.toByteArray());
    ObjectInputStream ois = new ObjectInputStream(bis);
    EstimatedEarthFrameProvider deserialized = (EstimatedEarthFrameProvider) ois.readObject();
    Assert.assertEquals(moved.getPrimeMeridianOffsetDriver().getValue(), deserialized.getPrimeMeridianOffsetDriver().getValue(), 1.0e-15);
    Assert.assertEquals(moved.getPrimeMeridianDriftDriver().getValue(), deserialized.getPrimeMeridianDriftDriver().getValue(), 1.0e-15);
    Assert.assertEquals(moved.getPolarOffsetXDriver().getValue(), deserialized.getPolarOffsetXDriver().getValue(), 1.0e-15);
    Assert.assertEquals(moved.getPolarDriftXDriver().getValue(), deserialized.getPolarDriftXDriver().getValue(), 1.0e-15);
    Assert.assertEquals(moved.getPolarOffsetYDriver().getValue(), deserialized.getPolarOffsetYDriver().getValue(), 1.0e-15);
    Assert.assertEquals(moved.getPolarDriftYDriver().getValue(), deserialized.getPolarDriftYDriver().getValue(), 1.0e-15);
}

Example 4 with RealMatrix

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

the class KalmanEstimatorTest method testKeplerianRangeAzElAndRangeRate.

/**
 * Perfect Range, Azel and range rate measurements with a biased start
 *  Start: position/velocity biased with: [+1000,0,0] m and [0,0,0.01] m/s
 *  Keplerian formalism
 */
@Test
public void testKeplerianRangeAzElAndRangeRate() throws OrekitException {
    // Create context
    Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    // Create initial orbit and propagator builder
    final OrbitType orbitType = OrbitType.KEPLERIAN;
    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 measPropagatorBuilder = context.createBuilder(orbitType, positionAngle, perfectStart, minStep, maxStep, dP);
    // Create perfect range measurements
    final Propagator rangePropagator = EstimationTestUtils.createPropagator(context.initialOrbit, measPropagatorBuilder);
    final List<ObservedMeasurement<?>> rangeMeasurements = EstimationTestUtils.createMeasurements(rangePropagator, new RangeMeasurementCreator(context), 0.0, 4.0, 300.0);
    // Create perfect az/el measurements
    final Propagator angularPropagator = EstimationTestUtils.createPropagator(context.initialOrbit, measPropagatorBuilder);
    final List<ObservedMeasurement<?>> angularMeasurements = EstimationTestUtils.createMeasurements(angularPropagator, new AngularAzElMeasurementCreator(context), 0.0, 4.0, 500.0);
    // Create perfect range rate measurements
    final Propagator rangeRatePropagator = EstimationTestUtils.createPropagator(context.initialOrbit, measPropagatorBuilder);
    final List<ObservedMeasurement<?>> rangeRateMeasurements = EstimationTestUtils.createMeasurements(rangeRatePropagator, new RangeRateMeasurementCreator(context, false), 0.0, 4.0, 700.0);
    // Concatenate measurements
    final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
    measurements.addAll(rangeMeasurements);
    measurements.addAll(angularMeasurements);
    measurements.addAll(rangeRateMeasurements);
    measurements.sort(new ChronologicalComparator());
    // Reference propagator for estimation performances
    final NumericalPropagator referencePropagator = measPropagatorBuilder.buildPropagator(measPropagatorBuilder.getSelectedNormalizedParameters());
    // Reference position/velocity at last measurement date
    final Orbit refOrbit = referencePropagator.propagate(measurements.get(measurements.size() - 1).getDate()).getOrbit();
    // Biased propagator for the Kalman
    final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(orbitType, positionAngle, false, minStep, maxStep, dP);
    // Cartesian covariance matrix initialization
    // Initial sigmas: 1000m on position, 0.01m/s on velocity
    final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double[] { 1e6, 1e6, 1e6, 1e-4, 1e-4, 1e-4 });
    // 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, dYdC);
    final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
    // Orbital initial covariance matrix
    final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
    // Process noise matrix
    final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double[] { 1.e-4, 1.e-4, 1.e-4, 1.e-10, 1.e-10, 1.e-10 });
    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 = 2.91e-2;
    final double expectedDeltaVel = 0.;
    final double velEps = 5.52e-6;
    final double[] expectedSigmasPos = { 1.747570, 0.666879, 1.696182 };
    final double sigmaPosEps = 1e-6;
    final double[] expectedSigmasVel = { 5.413666e-4, 4.088359e-4, 4.315316e-4 };
    final double sigmaVelEps = 1e-10;
    EstimationTestUtils.checkKalmanFit(context, kalman, measurements, refOrbit, positionAngle, expectedDeltaPos, posEps, expectedDeltaVel, velEps, expectedSigmasPos, sigmaPosEps, expectedSigmasVel, sigmaVelEps);
}

Example 5 with RealMatrix

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

the class KalmanEstimatorTest method testKeplerianRangeWithOnBoardAntennaOffset.

/**
 * Perfect range measurements with a biased start and an on-board antenna range offset
 * Keplerian formalism
 * @throws OrekitException
 */
@Test
public void testKeplerianRangeWithOnBoardAntennaOffset() throws OrekitException {
    // Create context
    Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    // Create initial orbit and propagator builder
    final OrbitType orbitType = OrbitType.KEPLERIAN;
    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);
    propagatorBuilder.setAttitudeProvider(new LofOffset(propagatorBuilder.getFrame(), LOFType.LVLH));
    // Antenna phase center definition
    final Vector3D antennaPhaseCenter = new Vector3D(-1.2, 2.3, -0.7);
    // Create perfect range measurements with antenna offset
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new RangeMeasurementCreator(context, antennaPhaseCenter), 1.0, 3.0, 300.0);
    // Add antenna offset to the measurements
    final OnBoardAntennaRangeModifier obaModifier = new OnBoardAntennaRangeModifier(antennaPhaseCenter);
    for (final ObservedMeasurement<?> range : measurements) {
        ((Range) range).addModifier(obaModifier);
    }
    // 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();
    // Change semi-major axis of 1.2m as in the batch test
    ParameterDriver aDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
    aDriver.setValue(aDriver.getValue() + 1.2);
    aDriver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
    // Cartesian covariance matrix initialization
    // 100m on position / 1e-2m/s on velocity
    final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double[] { 10., 10., 10., 1e-3, 1e-3, 1e-3 });
    // Jacobian of the orbital parameters w/r to Cartesian
    final Orbit initialOrbit = OrbitType.KEPLERIAN.convertType(context.initialOrbit);
    final double[][] dYdC = new double[6][6];
    initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
    final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
    // Keplerian initial covariance matrix
    final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
    // Process noise matrix is set to 0 here
    RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
    // 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.57e-3;
    final double expectedDeltaVel = 0.;
    final double velEps = 7.29e-6;
    final double[] expectedSigmasPos = { 1.105194, 0.930785, 1.254579 };
    final double sigmaPosEps = 1e-6;
    final double[] expectedSigmasVel = { 6.193718e-4, 4.088774e-4, 3.299135e-4 };
    final double sigmaVelEps = 1e-10;
    EstimationTestUtils.checkKalmanFit(context, kalman, measurements, refOrbit, positionAngle, expectedDeltaPos, posEps, expectedDeltaVel, velEps, expectedSigmasPos, sigmaPosEps, expectedSigmasVel, sigmaVelEps);
}