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

Example 31 with RealMatrix

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

the class OrbitDeterminationTest method testW3B.

@Test
public // Orbit determination for range, azimuth elevation measurements
void testW3B() throws URISyntaxException, IllegalArgumentException, IOException, OrekitException, ParseException {
    // input in tutorial resources directory/output
    final String inputPath = OrbitDeterminationTest.class.getClassLoader().getResource("orbit-determination/W3B/od_test_W3.in").toURI().getPath();
    final File input = new File(inputPath);
    // configure Orekit data access
    Utils.setDataRoot("orbit-determination/W3B:potential/icgem-format");
    GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
    // orbit determination run.
    ResultOD odsatW3 = run(input, false);
    // test
    // definition of the accuracy for the test
    final double distanceAccuracy = 0.1;
    final double velocityAccuracy = 1e-4;
    final double angleAccuracy = 1e-5;
    // test on the convergence (with some margins)
    Assert.assertTrue(odsatW3.getNumberOfIteration() < 6);
    Assert.assertTrue(odsatW3.getNumberOfEvaluation() < 10);
    // test on the estimated position and velocity
    final Vector3D estimatedPos = odsatW3.getEstimatedPV().getPosition();
    final Vector3D estimatedVel = odsatW3.getEstimatedPV().getVelocity();
    final Vector3D refPos = new Vector3D(-40541446.255, -9905357.41, 206777.413);
    final Vector3D refVel = new Vector3D(759.0685, -1476.5156, 54.793);
    Assert.assertEquals(0.0, Vector3D.distance(refPos, estimatedPos), distanceAccuracy);
    Assert.assertEquals(0.0, Vector3D.distance(refVel, estimatedVel), velocityAccuracy);
    // test on propagator parameters
    final double dragCoef = -0.2154;
    Assert.assertEquals(dragCoef, odsatW3.propagatorParameters.getDrivers().get(0).getValue(), 1e-3);
    final Vector3D leakAcceleration0 = new Vector3D(odsatW3.propagatorParameters.getDrivers().get(1).getValue(), odsatW3.propagatorParameters.getDrivers().get(3).getValue(), odsatW3.propagatorParameters.getDrivers().get(5).getValue());
    // Assert.assertEquals(7.215e-6, leakAcceleration.getNorm(), 1.0e-8);
    Assert.assertEquals(8.002e-6, leakAcceleration0.getNorm(), 1.0e-8);
    final Vector3D leakAcceleration1 = new Vector3D(odsatW3.propagatorParameters.getDrivers().get(2).getValue(), odsatW3.propagatorParameters.getDrivers().get(4).getValue(), odsatW3.propagatorParameters.getDrivers().get(6).getValue());
    Assert.assertEquals(3.058e-10, leakAcceleration1.getNorm(), 1.0e-12);
    // test on measurements parameters
    final List<DelegatingDriver> list = new ArrayList<DelegatingDriver>();
    list.addAll(odsatW3.measurementsParameters.getDrivers());
    sortParametersChanges(list);
    // station CastleRock
    final double[] CastleAzElBias = { 0.062701342, -0.003613508 };
    final double CastleRangeBias = 11274.4677;
    Assert.assertEquals(CastleAzElBias[0], FastMath.toDegrees(list.get(0).getValue()), angleAccuracy);
    Assert.assertEquals(CastleAzElBias[1], FastMath.toDegrees(list.get(1).getValue()), angleAccuracy);
    Assert.assertEquals(CastleRangeBias, list.get(2).getValue(), distanceAccuracy);
    // station Fucino
    final double[] FucAzElBias = { -0.053526137, 0.075483886 };
    final double FucRangeBias = 13467.8256;
    Assert.assertEquals(FucAzElBias[0], FastMath.toDegrees(list.get(3).getValue()), angleAccuracy);
    Assert.assertEquals(FucAzElBias[1], FastMath.toDegrees(list.get(4).getValue()), angleAccuracy);
    Assert.assertEquals(FucRangeBias, list.get(5).getValue(), distanceAccuracy);
    // station Kumsan
    final double[] KumAzElBias = { -0.023574208, -0.054520756 };
    final double KumRangeBias = 13512.57594;
    Assert.assertEquals(KumAzElBias[0], FastMath.toDegrees(list.get(6).getValue()), angleAccuracy);
    Assert.assertEquals(KumAzElBias[1], FastMath.toDegrees(list.get(7).getValue()), angleAccuracy);
    Assert.assertEquals(KumRangeBias, list.get(8).getValue(), distanceAccuracy);
    // station Pretoria
    final double[] PreAzElBias = { 0.030201539, 0.009747877 };
    final double PreRangeBias = 13594.11889;
    Assert.assertEquals(PreAzElBias[0], FastMath.toDegrees(list.get(9).getValue()), angleAccuracy);
    Assert.assertEquals(PreAzElBias[1], FastMath.toDegrees(list.get(10).getValue()), angleAccuracy);
    Assert.assertEquals(PreRangeBias, list.get(11).getValue(), distanceAccuracy);
    // station Uralla
    final double[] UraAzElBias = { 0.167814449, -0.12305252 };
    final double UraRangeBias = 13450.26738;
    Assert.assertEquals(UraAzElBias[0], FastMath.toDegrees(list.get(12).getValue()), angleAccuracy);
    Assert.assertEquals(UraAzElBias[1], FastMath.toDegrees(list.get(13).getValue()), angleAccuracy);
    Assert.assertEquals(UraRangeBias, list.get(14).getValue(), distanceAccuracy);
    // test on statistic for the range residuals
    final long nbRange = 182;
    // statistics for the range residual (min, max, mean, std)
    final double[] RefStatRange = { -18.39149369, 12.54165259, -4.32E-05, 4.374712716 };
    Assert.assertEquals(nbRange, odsatW3.getRangeStat().getN());
    Assert.assertEquals(RefStatRange[0], odsatW3.getRangeStat().getMin(), distanceAccuracy);
    Assert.assertEquals(RefStatRange[1], odsatW3.getRangeStat().getMax(), distanceAccuracy);
    Assert.assertEquals(RefStatRange[2], odsatW3.getRangeStat().getMean(), distanceAccuracy);
    Assert.assertEquals(RefStatRange[3], odsatW3.getRangeStat().getStandardDeviation(), distanceAccuracy);
    // test on statistic for the azimuth residuals
    final long nbAzi = 339;
    // statistics for the azimuth residual (min, max, mean, std)
    final double[] RefStatAzi = { -0.043033616, 0.025297558, -1.39E-10, 0.010063041 };
    Assert.assertEquals(nbAzi, odsatW3.getAzimStat().getN());
    Assert.assertEquals(RefStatAzi[0], odsatW3.getAzimStat().getMin(), angleAccuracy);
    Assert.assertEquals(RefStatAzi[1], odsatW3.getAzimStat().getMax(), angleAccuracy);
    Assert.assertEquals(RefStatAzi[2], odsatW3.getAzimStat().getMean(), angleAccuracy);
    Assert.assertEquals(RefStatAzi[3], odsatW3.getAzimStat().getStandardDeviation(), angleAccuracy);
    // test on statistic for the elevation residuals
    final long nbEle = 339;
    final double[] RefStatEle = { -0.025061971, 0.056294405, -4.10E-11, 0.011604931 };
    Assert.assertEquals(nbEle, odsatW3.getElevStat().getN());
    Assert.assertEquals(RefStatEle[0], odsatW3.getElevStat().getMin(), angleAccuracy);
    Assert.assertEquals(RefStatEle[1], odsatW3.getElevStat().getMax(), angleAccuracy);
    Assert.assertEquals(RefStatEle[2], odsatW3.getElevStat().getMean(), angleAccuracy);
    Assert.assertEquals(RefStatEle[3], odsatW3.getElevStat().getStandardDeviation(), angleAccuracy);
    RealMatrix covariances = odsatW3.getCovariances();
    Assert.assertEquals(28, covariances.getRowDimension());
    Assert.assertEquals(28, covariances.getColumnDimension());
    // drag coefficient variance
    Assert.assertEquals(0.687998, covariances.getEntry(6, 6), 1.0e-5);
    // leak-X constant term variance
    Assert.assertEquals(2.0540e-12, covariances.getEntry(7, 7), 1.0e-16);
    // leak-Y constant term variance
    Assert.assertEquals(2.4930e-11, covariances.getEntry(9, 9), 1.0e-15);
    // leak-Z constant term variance
    Assert.assertEquals(7.6720e-11, covariances.getEntry(11, 11), 1.0e-15);
}

Example 32 with RealMatrix

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

the class HolmesFeatherstoneAttractionModelTest method accelerationDerivatives.

@Override
protected FieldVector3D<DerivativeStructure> accelerationDerivatives(final ForceModel forceModel, final AbsoluteDate date, final Frame frame, final FieldVector3D<DerivativeStructure> position, final FieldVector3D<DerivativeStructure> velocity, final FieldRotation<DerivativeStructure> rotation, final DerivativeStructure mass) throws OrekitException {
    try {
        java.lang.reflect.Field bodyFrameField = HolmesFeatherstoneAttractionModel.class.getDeclaredField("bodyFrame");
        bodyFrameField.setAccessible(true);
        Frame bodyFrame = (Frame) bodyFrameField.get(forceModel);
        // get the position in body frame
        final Transform fromBodyFrame = bodyFrame.getTransformTo(frame, date);
        final Transform toBodyFrame = fromBodyFrame.getInverse();
        final Vector3D positionBody = toBodyFrame.transformPosition(position.toVector3D());
        // compute gradient and Hessian
        final GradientHessian gh = gradientHessian((HolmesFeatherstoneAttractionModel) forceModel, date, positionBody);
        // gradient of the non-central part of the gravity field
        final double[] gInertial = fromBodyFrame.transformVector(new Vector3D(gh.getGradient())).toArray();
        // Hessian of the non-central part of the gravity field
        final RealMatrix hBody = new Array2DRowRealMatrix(gh.getHessian(), false);
        final RealMatrix rot = new Array2DRowRealMatrix(toBodyFrame.getRotation().getMatrix());
        final RealMatrix hInertial = rot.transpose().multiply(hBody).multiply(rot);
        // distribute all partial derivatives in a compact acceleration vector
        final double[] derivatives = new double[1 + mass.getFreeParameters()];
        final DerivativeStructure[] accDer = new DerivativeStructure[3];
        for (int i = 0; i < 3; ++i) {
            // first element is value of acceleration (i.e. gradient of field)
            derivatives[0] = gInertial[i];
            // next three elements are one row of the Jacobian of acceleration (i.e. Hessian of field)
            derivatives[1] = hInertial.getEntry(i, 0);
            derivatives[2] = hInertial.getEntry(i, 1);
            derivatives[3] = hInertial.getEntry(i, 2);
            // next elements (three or four depending on mass being used or not) are left as 0
            accDer[i] = mass.getFactory().build(derivatives);
        }
        return new FieldVector3D<>(accDer);
    } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
        return null;
    }
}

Example 33 with RealMatrix

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

the class KalmanEstimatorTest method testEquinoctialRightAscensionDeclination.

/**
 * Perfect right-ascension/declination measurements with a perfect start
 * Equinoctial formalism
 * @throws OrekitException
 */
@Test
public void testEquinoctialRightAscensionDeclination() throws OrekitException {
    // Create context
    Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    // Create initial orbit and propagator builder
    final OrbitType orbitType = OrbitType.EQUINOCTIAL;
    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 AngularRaDecMeasurementCreator(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, dYdC);
    final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
    // Keplerian 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 = 1.53e-5;
    final double expectedDeltaVel = 0.;
    final double velEps = 5.04e-9;
    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);
}

Example 34 with RealMatrix

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

the class IntegratedEphemerisTest method testPartialDerivativesIssue16.

@Test
public void testPartialDerivativesIssue16() throws OrekitException {
    final String eqName = "derivatives";
    numericalPropagator.setEphemerisMode();
    numericalPropagator.setOrbitType(OrbitType.CARTESIAN);
    final PartialDerivativesEquations derivatives = new PartialDerivativesEquations(eqName, numericalPropagator);
    final SpacecraftState initialState = derivatives.setInitialJacobians(new SpacecraftState(initialOrbit));
    final JacobiansMapper mapper = derivatives.getMapper();
    numericalPropagator.setInitialState(initialState);
    numericalPropagator.propagate(initialOrbit.getDate().shiftedBy(3600.0));
    BoundedPropagator ephemeris = numericalPropagator.getGeneratedEphemeris();
    ephemeris.setMasterMode(new OrekitStepHandler() {

        private final Array2DRowRealMatrix dYdY0 = new Array2DRowRealMatrix(6, 6);

        public void handleStep(OrekitStepInterpolator interpolator, boolean isLast) throws OrekitException {
            SpacecraftState state = interpolator.getCurrentState();
            Assert.assertEquals(mapper.getAdditionalStateDimension(), state.getAdditionalState(eqName).length);
            mapper.getStateJacobian(state, dYdY0.getDataRef());
            // no parameters, this is a no-op and should work
            mapper.getParametersJacobian(state, null);
            RealMatrix deltaId = dYdY0.subtract(MatrixUtils.createRealIdentityMatrix(6));
            Assert.assertTrue(deltaId.getNorm() > 100);
            Assert.assertTrue(deltaId.getNorm() < 3100);
        }
    });
    ephemeris.propagate(initialOrbit.getDate().shiftedBy(1800.0));
}

Example 35 with RealMatrix

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

the class JacobianPropagatorConverterTest method doTestDerivatives.

private void doTestDerivatives(double tolP, double tolV, String... names) throws OrekitException {
    // we use a fixed step integrator on purpose
    // as the test is based on external differentiation using finite differences,
    // an adaptive step size integrator would introduce *lots* of numerical noise
    NumericalPropagatorBuilder builder = new NumericalPropagatorBuilder(OrbitType.CARTESIAN.convertType(orbit), new LutherIntegratorBuilder(10.0), PositionAngle.TRUE, dP);
    builder.setMass(200.0);
    builder.addForceModel(drag);
    builder.addForceModel(gravity);
    // retrieve a state slightly different from the initial state,
    // using normalized values different from 0.0 for the sake of generality
    RandomGenerator random = new Well19937a(0xe67f19c1a678d037l);
    List<ParameterDriver> all = new ArrayList<ParameterDriver>();
    for (final ParameterDriver driver : builder.getOrbitalParametersDrivers().getDrivers()) {
        all.add(driver);
    }
    for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
        all.add(driver);
    }
    double[] normalized = new double[names.length];
    List<ParameterDriver> selected = new ArrayList<ParameterDriver>(names.length);
    int index = 0;
    for (final ParameterDriver driver : all) {
        boolean found = false;
        for (final String name : names) {
            if (name.equals(driver.getName())) {
                found = true;
                normalized[index++] = driver.getNormalizedValue() + (2 * random.nextDouble() - 1);
                selected.add(driver);
            }
        }
        driver.setSelected(found);
    }
    // create a one hour sample that starts 10 minutes after initial state
    // the 10 minutes offset implies even the first point is influenced by model parameters
    final List<SpacecraftState> sample = new ArrayList<SpacecraftState>();
    Propagator propagator = builder.buildPropagator(normalized);
    propagator.setMasterMode(60.0, new OrekitFixedStepHandler() {

        @Override
        public void handleStep(SpacecraftState currentState, boolean isLast) {
            sample.add(currentState);
        }
    });
    propagator.propagate(orbit.getDate().shiftedBy(600.0), orbit.getDate().shiftedBy(4200.0));
    JacobianPropagatorConverter fitter = new JacobianPropagatorConverter(builder, 1.0e-3, 5000);
    try {
        Method setSample = AbstractPropagatorConverter.class.getDeclaredMethod("setSample", List.class);
        setSample.setAccessible(true);
        setSample.invoke(fitter, sample);
    } catch (NoSuchMethodException | SecurityException | IllegalAccessException | IllegalArgumentException | InvocationTargetException e) {
        Assert.fail(e.getLocalizedMessage());
    }
    MultivariateVectorFunction f = fitter.getObjectiveFunction();
    Pair<RealVector, RealMatrix> p = fitter.getModel().value(new ArrayRealVector(normalized));
    // check derivatives
    // a h offset on normalized parameter represents a physical offset of h * scale
    RealMatrix m = p.getSecond();
    double h = 10.0;
    double[] shifted = normalized.clone();
    double maxErrorP = 0;
    double maxErrorV = 0;
    for (int j = 0; j < selected.size(); ++j) {
        shifted[j] = normalized[j] + 2.0 * h;
        double[] valueP2 = f.value(shifted);
        shifted[j] = normalized[j] + 1.0 * h;
        double[] valueP1 = f.value(shifted);
        shifted[j] = normalized[j] - 1.0 * h;
        double[] valueM1 = f.value(shifted);
        shifted[j] = normalized[j] - 2.0 * h;
        double[] valueM2 = f.value(shifted);
        shifted[j] = normalized[j];
        for (int i = 0; i < valueP2.length; ++i) {
            double d = (8 * (valueP1[i] - valueM1[i]) - (valueP2[i] - valueM2[i])) / (12 * h);
            if (i % 6 < 3) {
                // position
                maxErrorP = FastMath.max(maxErrorP, FastMath.abs(m.getEntry(i, j) - d));
            } else {
                // velocity
                maxErrorV = FastMath.max(maxErrorV, FastMath.abs(m.getEntry(i, j) - d));
            }
        }
    }
    Assert.assertEquals(0.0, maxErrorP, tolP);
    Assert.assertEquals(0.0, maxErrorV, tolV);
}