Examples with DelegatingDriver org.orekit.utils.ParameterDriversList.DelegatingDriver used on opensource projects

Example 1 with DelegatingDriver

use of org.orekit.utils.ParameterDriversList.DelegatingDriver in project Orekit by CS-SI.

the class KalmanOrbitDeterminationTest method testLageos2.

@Test
public // Orbit determination for Lageos2 based on SLR (range) measurements
void testLageos2() throws URISyntaxException, IllegalArgumentException, IOException, OrekitException, ParseException {
    // Print results on console
    final boolean print = false;
    // input in tutorial resources directory/output
    final String inputPath = KalmanOrbitDeterminationTest.class.getClassLoader().getResource("orbit-determination/Lageos2/od_test_Lageos2.in").toURI().getPath();
    final File input = new File(inputPath);
    // configure Orekit data acces
    Utils.setDataRoot("orbit-determination/Lageos2:potential/icgem-format");
    GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
    // Choice of an orbit type to use
    // Default for test is Cartesian
    final OrbitType orbitType = OrbitType.CARTESIAN;
    // Initial orbital Cartesian covariance matrix
    // These covariances are derived from the deltas between initial and reference orbits
    // So in a way they are "perfect"...
    // Cartesian covariance matrix initialization
    final RealMatrix cartesianOrbitalP = MatrixUtils.createRealDiagonalMatrix(new double[] { 1e4, 4e3, 1, 5e-3, 6e-5, 1e-4 });
    // Orbital Cartesian process noise matrix (Q)
    final RealMatrix cartesianOrbitalQ = MatrixUtils.createRealDiagonalMatrix(new double[] { 1.e-4, 1.e-4, 1.e-4, 1.e-10, 1.e-10, 1.e-10 });
    // Initial measurement covariance matrix and process noise matrix
    final RealMatrix measurementP = MatrixUtils.createRealDiagonalMatrix(new double[] { 1., 1., 1., 1. });
    final RealMatrix measurementQ = MatrixUtils.createRealDiagonalMatrix(new double[] { 1e-6, 1e-6, 1e-6, 1e-6 });
    // Kalman orbit determination run.
    ResultKalman kalmanLageos2 = run(input, orbitType, print, cartesianOrbitalP, cartesianOrbitalQ, null, null, measurementP, measurementQ);
    // Definition of the accuracy for the test
    final double distanceAccuracy = 0.86;
    final double velocityAccuracy = 4.12e-3;
    // Tests
    // Note: The reference initial orbit is the same as in the batch LS tests
    // -----
    // Number of measurements processed
    final int numberOfMeas = 258;
    Assert.assertEquals(numberOfMeas, kalmanLageos2.getNumberOfMeasurements());
    // Estimated position and velocity
    final Vector3D estimatedPos = kalmanLageos2.getEstimatedPV().getPosition();
    final Vector3D estimatedVel = kalmanLageos2.getEstimatedPV().getVelocity();
    // Reference position and velocity at initial date (same as in batch LS test)
    final Vector3D refPos0 = new Vector3D(-5532131.956902, 10025696.592156, -3578940.040009);
    final Vector3D refVel0 = new Vector3D(-3871.275109, -607.880985, 4280.972530);
    // Run the reference until Kalman last date
    final Orbit refOrbit = runReference(input, orbitType, refPos0, refVel0, null, kalmanLageos2.getEstimatedPV().getDate());
    final Vector3D refPos = refOrbit.getPVCoordinates().getPosition();
    final Vector3D refVel = refOrbit.getPVCoordinates().getVelocity();
    // Check distances
    final double dP = Vector3D.distance(refPos, estimatedPos);
    final double dV = Vector3D.distance(refVel, estimatedVel);
    Assert.assertEquals(0.0, dP, distanceAccuracy);
    Assert.assertEquals(0.0, dV, velocityAccuracy);
    // Print orbit deltas
    if (print) {
        System.out.println("Test performances:");
        System.out.format("\t%-30s\n", "ΔEstimated / Reference");
        System.out.format(Locale.US, "\t%-10s %20.6f\n", "ΔP [m]", dP);
        System.out.format(Locale.US, "\t%-10s %20.6f\n", "ΔV [m/s]", dV);
    }
    // Test on measurements parameters
    final List<DelegatingDriver> list = new ArrayList<DelegatingDriver>();
    list.addAll(kalmanLageos2.measurementsParameters.getDrivers());
    sortParametersChanges(list);
    // Batch LS values
    // final double[] stationOffSet = { 1.659203,  0.861250,  -0.885352 };
    // final double rangeBias = -0.286275;
    final double[] stationOffSet = { 0.298867, -0.137456, 0.013315 };
    final double rangeBias = 0.002390;
    Assert.assertEquals(stationOffSet[0], list.get(0).getValue(), distanceAccuracy);
    Assert.assertEquals(stationOffSet[1], list.get(1).getValue(), distanceAccuracy);
    Assert.assertEquals(stationOffSet[2], list.get(2).getValue(), distanceAccuracy);
    Assert.assertEquals(rangeBias, list.get(3).getValue(), distanceAccuracy);
    // test on statistic for the range residuals
    final long nbRange = 258;
    // Batch LS values
    // final double[] RefStatRange = { -2.431135, 2.218644, 0.038483, 0.982017 };
    final double[] RefStatRange = { -23.561314, 20.436464, 0.964164, 5.687187 };
    Assert.assertEquals(nbRange, kalmanLageos2.getRangeStat().getN());
    Assert.assertEquals(RefStatRange[0], kalmanLageos2.getRangeStat().getMin(), distanceAccuracy);
    Assert.assertEquals(RefStatRange[1], kalmanLageos2.getRangeStat().getMax(), distanceAccuracy);
    Assert.assertEquals(RefStatRange[2], kalmanLageos2.getRangeStat().getMean(), distanceAccuracy);
    Assert.assertEquals(RefStatRange[3], kalmanLageos2.getRangeStat().getStandardDeviation(), distanceAccuracy);
}

Example 2 with DelegatingDriver

use of org.orekit.utils.ParameterDriversList.DelegatingDriver in project Orekit by CS-SI.

the class BatchLSEstimatorTest method testMultiSatWithParameters.

/**
 * A modified version of the previous test with a selection of propagation drivers to estimate
 *  One common (µ)
 *  Some specifics for each satellite (Cr and Ca)
 *
 * @throws OrekitException
 */
@Test
public void testMultiSatWithParameters() throws OrekitException {
    // Test: Set the propagator drivers to estimate for each satellite
    final boolean muEstimated = true;
    final boolean crEstimated1 = true;
    final boolean caEstimated1 = true;
    final boolean crEstimated2 = true;
    final boolean caEstimated2 = false;
    // Builder sat 1
    final Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    final NumericalPropagatorBuilder propagatorBuilder1 = context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, 1.0e-6, 60.0, 1.0, Force.POTENTIAL, Force.SOLAR_RADIATION_PRESSURE);
    // Adding selection of parameters
    String satName = "sat 1";
    for (DelegatingDriver driver : propagatorBuilder1.getPropagationParametersDrivers().getDrivers()) {
        if (driver.getName().equals("central attraction coefficient")) {
            driver.setSelected(muEstimated);
        }
        if (driver.getName().equals(RadiationSensitive.REFLECTION_COEFFICIENT)) {
            driver.setName(driver.getName() + " " + satName);
            driver.setSelected(crEstimated1);
        }
        if (driver.getName().equals(RadiationSensitive.ABSORPTION_COEFFICIENT)) {
            driver.setName(driver.getName() + " " + satName);
            driver.setSelected(caEstimated1);
        }
    }
    // Builder for sat 2
    final Context context2 = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    final NumericalPropagatorBuilder propagatorBuilder2 = context2.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, 1.0e-6, 60.0, 1.0, Force.POTENTIAL, Force.SOLAR_RADIATION_PRESSURE);
    // Adding selection of parameters
    satName = "sat 2";
    for (ParameterDriver driver : propagatorBuilder2.getPropagationParametersDrivers().getDrivers()) {
        if (driver.getName().equals("central attraction coefficient")) {
            driver.setSelected(muEstimated);
        }
        if (driver.getName().equals(RadiationSensitive.REFLECTION_COEFFICIENT)) {
            driver.setName(driver.getName() + " " + satName);
            driver.setSelected(crEstimated2);
        }
        if (driver.getName().equals(RadiationSensitive.ABSORPTION_COEFFICIENT)) {
            driver.setName(driver.getName() + " " + satName);
            driver.setSelected(caEstimated2);
        }
    }
    // Create perfect inter-satellites range measurements
    final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
    final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(), original.getPosition().add(new Vector3D(1000, 2000, 3000)), original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))), context.initialOrbit.getFrame(), context.initialOrbit.getMu());
    final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit, propagatorBuilder2);
    closePropagator.setEphemerisMode();
    closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
    final BoundedPropagator ephemeris = closePropagator.getGeneratedEphemeris();
    Propagator propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder1);
    final List<ObservedMeasurement<?>> r12 = EstimationTestUtils.createMeasurements(propagator1, new InterSatellitesRangeMeasurementCreator(ephemeris), 1.0, 3.0, 300.0);
    // create perfect range measurements for first satellite
    propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder1);
    final List<ObservedMeasurement<?>> r1 = EstimationTestUtils.createMeasurements(propagator1, new RangeMeasurementCreator(context), 1.0, 3.0, 300.0);
    // create orbit estimator
    final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(), propagatorBuilder1, propagatorBuilder2);
    for (final ObservedMeasurement<?> interSat : r12) {
        estimator.addMeasurement(interSat);
    }
    for (final ObservedMeasurement<?> range : r1) {
        estimator.addMeasurement(range);
    }
    estimator.setParametersConvergenceThreshold(1.0e-2);
    estimator.setMaxIterations(10);
    estimator.setMaxEvaluations(20);
    estimator.setObserver(new BatchLSObserver() {

        int lastIter = 0;

        int lastEval = 0;

        /**
         * {@inheritDoc}
         */
        @Override
        public void evaluationPerformed(int iterationsCount, int evaluationscount, Orbit[] orbits, ParameterDriversList estimatedOrbitalParameters, ParameterDriversList estimatedPropagatorParameters, ParameterDriversList estimatedMeasurementsParameters, EstimationsProvider evaluationsProvider, Evaluation lspEvaluation) throws OrekitException {
            if (iterationsCount == lastIter) {
                Assert.assertEquals(lastEval + 1, evaluationscount);
            } else {
                Assert.assertEquals(lastIter + 1, iterationsCount);
            }
            lastIter = iterationsCount;
            lastEval = evaluationscount;
            AbsoluteDate previous = AbsoluteDate.PAST_INFINITY;
            for (int i = 0; i < evaluationsProvider.getNumber(); ++i) {
                AbsoluteDate current = evaluationsProvider.getEstimatedMeasurement(i).getDate();
                Assert.assertTrue(current.compareTo(previous) >= 0);
                previous = current;
            }
        }
    });
    List<DelegatingDriver> parameters = estimator.getOrbitalParametersDrivers(true).getDrivers();
    ParameterDriver a0Driver = parameters.get(0);
    Assert.assertEquals("a[0]", a0Driver.getName());
    a0Driver.setValue(a0Driver.getValue() + 1.2);
    a0Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
    ParameterDriver a1Driver = parameters.get(6);
    Assert.assertEquals("a[1]", a1Driver.getName());
    a1Driver.setValue(a1Driver.getValue() - 5.4);
    a1Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
    final Orbit before = new KeplerianOrbit(parameters.get(6).getValue(), parameters.get(7).getValue(), parameters.get(8).getValue(), parameters.get(9).getValue(), parameters.get(10).getValue(), parameters.get(11).getValue(), PositionAngle.TRUE, closeOrbit.getFrame(), closeOrbit.getDate(), closeOrbit.getMu());
    Assert.assertEquals(4.7246, Vector3D.distance(closeOrbit.getPVCoordinates().getPosition(), before.getPVCoordinates().getPosition()), 1.0e-3);
    Assert.assertEquals(0.0010514, Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(), before.getPVCoordinates().getVelocity()), 1.0e-6);
    EstimationTestUtils.checkFit(context, estimator, 4, 5, 0.0, 6.0e-06, 0.0, 1.7e-05, 0.0, 4.4e-07, 0.0, 1.7e-10);
    final Orbit determined = new KeplerianOrbit(parameters.get(6).getValue(), parameters.get(7).getValue(), parameters.get(8).getValue(), parameters.get(9).getValue(), parameters.get(10).getValue(), parameters.get(11).getValue(), PositionAngle.TRUE, closeOrbit.getFrame(), closeOrbit.getDate(), closeOrbit.getMu());
    Assert.assertEquals(0.0, Vector3D.distance(closeOrbit.getPVCoordinates().getPosition(), determined.getPVCoordinates().getPosition()), 5.8e-6);
    Assert.assertEquals(0.0, Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(), determined.getPVCoordinates().getVelocity()), 3.5e-9);
    // got a default one
    for (final ParameterDriver driver : estimator.getOrbitalParametersDrivers(true).getDrivers()) {
        if (driver.getName().startsWith("a[")) {
            // user-specified reference date
            Assert.assertEquals(0, driver.getReferenceDate().durationFrom(AbsoluteDate.GALILEO_EPOCH), 1.0e-15);
        } else {
            // default reference date
            Assert.assertEquals(0, driver.getReferenceDate().durationFrom(propagatorBuilder1.getInitialOrbitDate()), 1.0e-15);
        }
    }
}

Example 3 with DelegatingDriver

use of org.orekit.utils.ParameterDriversList.DelegatingDriver in project Orekit by CS-SI.

the class Model method getMeasurementMatrix.

/**
 * Get the normalized measurement matrix H.
 * H contains the partial derivatives of the measurement with respect to the state.
 * H is an NxM matrix where N is the size of the measurement vector and M the size of the state vector.
 * @param predictedSpacecraftState the spacecraft state associated with the measurement
 * @return the normalized measurement matrix H
 * @throws OrekitException if Jacobians cannot be computed
 */
private RealMatrix getMeasurementMatrix(final SpacecraftState predictedSpacecraftState) throws OrekitException {
    // Number of parameters
    final int nbOrb = estimatedOrbitalParameters.getNbParams();
    final int nbPropag = estimatedPropagationParameters.getNbParams();
    final int nbMeas = estimatedMeasurementsParameters.getNbParams();
    // Initialize measurement matrix H: N x M
    // N: Number of measurements in current measurement
    // M: State vector size
    final RealMatrix measurementMatrix = MatrixUtils.createRealMatrix(predictedMeasurement.getEstimatedValue().length, nbOrb + nbPropag + nbMeas);
    // Predicted orbit
    final Orbit predictedOrbit = predictedSpacecraftState.getOrbit();
    // Observed measurement characteristics
    final ObservedMeasurement<?> observedMeasurement = predictedMeasurement.getObservedMeasurement();
    final double[] sigma = observedMeasurement.getTheoreticalStandardDeviation();
    // Measurement matrix's columns related to orbital parameters
    // ----------------------------------------------------------
    // Partial derivatives of the current Cartesian coordinates with respect to current orbital state
    final double[][] aCY = new double[6][6];
    // dC/dY
    predictedOrbit.getJacobianWrtParameters(builder.getPositionAngle(), aCY);
    final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);
    // Jacobian of the measurement with respect to current Cartesian coordinates
    final RealMatrix dMdC = new Array2DRowRealMatrix(predictedMeasurement.getStateDerivatives(0), false);
    // Jacobian of the measurement with respect to current orbital state
    final RealMatrix dMdY = dMdC.multiply(dCdY);
    // Fill the normalized measurement matrix's columns related to estimated orbital parameters
    if (nbOrb > 0) {
        for (int i = 0; i < dMdY.getRowDimension(); ++i) {
            int jOrb = 0;
            for (int j = 0; j < dMdY.getColumnDimension(); ++j) {
                final DelegatingDriver delegating = builder.getOrbitalParametersDrivers().getDrivers().get(j);
                if (delegating.isSelected()) {
                    measurementMatrix.setEntry(i, jOrb++, dMdY.getEntry(i, j) / sigma[i] * delegating.getScale());
                }
            }
        }
    }
    // Jacobian of the measurement with respect to propagation parameters
    if (nbPropag > 0) {
        final double[][] aYPp = new double[6][nbPropag];
        jacobiansMapper.getParametersJacobian(predictedSpacecraftState, aYPp);
        final RealMatrix dYdPp = new Array2DRowRealMatrix(aYPp, false);
        final RealMatrix dMdPp = dMdY.multiply(dYdPp);
        for (int i = 0; i < dMdPp.getRowDimension(); ++i) {
            for (int j = 0; j < nbPropag; ++j) {
                final DelegatingDriver delegating = estimatedPropagationParameters.getDrivers().get(j);
                measurementMatrix.setEntry(i, nbOrb + j, dMdPp.getEntry(i, j) / sigma[i] * delegating.getScale());
            }
        }
    }
    // Jacobian of the measurement with respect to measurement parameters
    if (nbMeas > 0) {
        // Gather the measurement parameters linked to current measurement
        for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
            if (driver.isSelected()) {
                // Derivatives of current measurement w/r to selected measurement parameter
                final double[] aMPm = predictedMeasurement.getParameterDerivatives(driver);
                // Check that the measurement parameter is managed by the filter
                if (measurementParameterColumns.get(driver.getName()) != null) {
                    // Column of the driver in the measurement matrix
                    final int driverColumn = measurementParameterColumns.get(driver.getName());
                    // Fill the corresponding indexes of the measurement matrix
                    for (int i = 0; i < aMPm.length; ++i) {
                        measurementMatrix.setEntry(i, driverColumn, aMPm[i] / sigma[i] * driver.getScale());
                    }
                }
            }
        }
    }
    // Return the normalized measurement matrix
    return measurementMatrix;
}

Example 4 with DelegatingDriver

use of org.orekit.utils.ParameterDriversList.DelegatingDriver in project Orekit by CS-SI.

the class Model method updateParameters.

/**
 * Update the estimated parameters after the correction phase of the filter.
 * The min/max allowed values are handled by the parameter themselves.
 * @throws OrekitException if setting the normalized values failed
 */
private void updateParameters() throws OrekitException {
    final RealVector correctedState = correctedEstimate.getState();
    int i = 0;
    for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
        // let the parameter handle min/max clipping
        driver.setNormalizedValue(correctedState.getEntry(i));
        correctedState.setEntry(i++, driver.getNormalizedValue());
    }
    for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
        // let the parameter handle min/max clipping
        driver.setNormalizedValue(correctedState.getEntry(i));
        correctedState.setEntry(i++, driver.getNormalizedValue());
    }
    for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
        // let the parameter handle min/max clipping
        driver.setNormalizedValue(correctedState.getEntry(i));
        correctedState.setEntry(i++, driver.getNormalizedValue());
    }
}

Example 5 with DelegatingDriver

use of org.orekit.utils.ParameterDriversList.DelegatingDriver in project Orekit by CS-SI.

the class TLEConverterTest method checkFit.

protected void checkFit(final TLE tle, final double duration, final double stepSize, final double threshold, final boolean positionOnly, final boolean withBStar, final double expectedRMS) throws OrekitException {
    Propagator p = TLEPropagator.selectExtrapolator(tle);
    List<SpacecraftState> sample = new ArrayList<SpacecraftState>();
    for (double dt = 0; dt < duration; dt += stepSize) {
        sample.add(p.propagate(tle.getDate().shiftedBy(dt)));
    }
    TLEPropagatorBuilder builder = new TLEPropagatorBuilder(tle, PositionAngle.TRUE, 1.0);
    List<DelegatingDriver> drivers = builder.getPropagationParametersDrivers().getDrivers();
    // there should *not* be any drivers for central attraction coefficient (see issue #313)
    Assert.assertEquals(1, drivers.size());
    Assert.assertEquals("BSTAR", drivers.get(0).getName());
    FiniteDifferencePropagatorConverter fitter = new FiniteDifferencePropagatorConverter(builder, threshold, 1000);
    if (withBStar) {
        fitter.convert(sample, positionOnly, TLEPropagatorBuilder.B_STAR);
    } else {
        fitter.convert(sample, positionOnly);
    }
    TLEPropagator prop = (TLEPropagator) fitter.getAdaptedPropagator();
    TLE fitted = prop.getTLE();
    Assert.assertEquals(expectedRMS, fitter.getRMS(), 0.001 * expectedRMS);
    Assert.assertEquals(tle.getSatelliteNumber(), fitted.getSatelliteNumber());
    Assert.assertEquals(tle.getClassification(), fitted.getClassification());
    Assert.assertEquals(tle.getLaunchYear(), fitted.getLaunchYear());
    Assert.assertEquals(tle.getLaunchNumber(), fitted.getLaunchNumber());
    Assert.assertEquals(tle.getLaunchPiece(), fitted.getLaunchPiece());
    Assert.assertEquals(tle.getElementNumber(), fitted.getElementNumber());
    Assert.assertEquals(tle.getRevolutionNumberAtEpoch(), fitted.getRevolutionNumberAtEpoch());
    final double eps = 1.0e-5;
    Assert.assertEquals(tle.getMeanMotion(), fitted.getMeanMotion(), eps * tle.getMeanMotion());
    Assert.assertEquals(tle.getE(), fitted.getE(), eps * tle.getE());
    Assert.assertEquals(tle.getI(), fitted.getI(), eps * tle.getI());
    Assert.assertEquals(tle.getPerigeeArgument(), fitted.getPerigeeArgument(), eps * tle.getPerigeeArgument());
    Assert.assertEquals(tle.getRaan(), fitted.getRaan(), eps * tle.getRaan());
    Assert.assertEquals(tle.getMeanAnomaly(), fitted.getMeanAnomaly(), eps * tle.getMeanAnomaly());
    if (withBStar) {
        Assert.assertEquals(tle.getBStar(), fitted.getBStar(), eps * tle.getBStar());
    }
}