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

Example 26 with ParameterDriversList

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

the class Model method fetchEvaluatedMeasurement.

/**
 * Fetch a measurement that was evaluated during propagation.
 * @param index index of the measurement first component
 * @param evaluation measurement evaluation
 * @exception OrekitException if Jacobians cannot be computed
 */
void fetchEvaluatedMeasurement(final int index, final EstimatedMeasurement<?> evaluation) throws OrekitException {
    // States and observed measurement
    final SpacecraftState[] evaluationStates = evaluation.getStates();
    final ObservedMeasurement<?> observedMeasurement = evaluation.getObservedMeasurement();
    evaluations.put(observedMeasurement, evaluation);
    if (evaluation.getStatus() == EstimatedMeasurement.Status.REJECTED) {
        return;
    }
    // compute weighted residuals
    final double[] evaluated = evaluation.getEstimatedValue();
    final double[] observed = observedMeasurement.getObservedValue();
    final double[] sigma = observedMeasurement.getTheoreticalStandardDeviation();
    final double[] weight = evaluation.getObservedMeasurement().getBaseWeight();
    for (int i = 0; i < evaluated.length; ++i) {
        value.setEntry(index + i, weight[i] * (evaluated[i] - observed[i]) / sigma[i]);
    }
    for (int k = 0; k < evaluationStates.length; ++k) {
        final int p = observedMeasurement.getPropagatorsIndices().get(k);
        // partial derivatives of the current Cartesian coordinates with respect to current orbital state
        final double[][] aCY = new double[6][6];
        final Orbit currentOrbit = evaluationStates[k].getOrbit();
        currentOrbit.getJacobianWrtParameters(builders[p].getPositionAngle(), aCY);
        final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);
        // Jacobian of the measurement with respect to current orbital state
        final RealMatrix dMdC = new Array2DRowRealMatrix(evaluation.getStateDerivatives(k), false);
        final RealMatrix dMdY = dMdC.multiply(dCdY);
        // Jacobian of the measurement with respect to initial orbital state
        final double[][] aYY0 = new double[6][6];
        mappers[p].getStateJacobian(evaluationStates[k], aYY0);
        final RealMatrix dYdY0 = new Array2DRowRealMatrix(aYY0, false);
        final RealMatrix dMdY0 = dMdY.multiply(dYdY0);
        for (int i = 0; i < dMdY0.getRowDimension(); ++i) {
            int jOrb = orbitsStartColumns[p];
            for (int j = 0; j < dMdY0.getColumnDimension(); ++j) {
                final ParameterDriver driver = builders[p].getOrbitalParametersDrivers().getDrivers().get(j);
                if (driver.isSelected()) {
                    jacobian.setEntry(index + i, jOrb++, weight[i] * dMdY0.getEntry(i, j) / sigma[i] * driver.getScale());
                }
            }
        }
        // Jacobian of the measurement with respect to propagation parameters
        final ParameterDriversList selectedPropagationDrivers = getSelectedPropagationDriversForBuilder(p);
        final int nbParams = selectedPropagationDrivers.getNbParams();
        if (nbParams > 0) {
            final double[][] aYPp = new double[6][nbParams];
            mappers[p].getParametersJacobian(evaluationStates[k], 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 < nbParams; ++j) {
                    final ParameterDriver delegating = selectedPropagationDrivers.getDrivers().get(j);
                    jacobian.addToEntry(index + i, propagationParameterColumns.get(delegating.getName()), weight[i] * dMdPp.getEntry(i, j) / sigma[i] * delegating.getScale());
                }
            }
        }
    }
    // Jacobian of the measurement with respect to measurements parameters
    for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
        if (driver.isSelected()) {
            final double[] aMPm = evaluation.getParameterDerivatives(driver);
            for (int i = 0; i < aMPm.length; ++i) {
                jacobian.setEntry(index + i, measurementParameterColumns.get(driver.getName()), weight[i] * aMPm[i] / sigma[i] * driver.getScale());
            }
        }
    }
}

Example 27 with ParameterDriversList

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

the class BatchLSEstimator method getMeasurementsParametersDrivers.

/**
 * Get the measurements parameters supported by this estimator (including measurements and modifiers).
 * @param estimatedOnly if true, only estimated parameters are returned
 * @return measurements parameters supported by this estimator
 * @exception OrekitException if different parameters have the same name
 */
public ParameterDriversList getMeasurementsParametersDrivers(final boolean estimatedOnly) throws OrekitException {
    final ParameterDriversList parameters = new ParameterDriversList();
    for (final ObservedMeasurement<?> measurement : measurements) {
        for (final ParameterDriver driver : measurement.getParametersDrivers()) {
            if ((!estimatedOnly) || driver.isSelected()) {
                parameters.add(driver);
            }
        }
    }
    parameters.sort();
    return parameters;
}

Example 28 with ParameterDriversList

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

the class BatchLSEstimatorTest method testMultiSat.

@Test
public void testMultiSat() throws OrekitException {
    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);
    final NumericalPropagatorBuilder propagatorBuilder2 = context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, 1.0e-6, 60.0, 1.0);
    // 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;
            Assert.assertEquals(r12.size() + r1.size(), evaluationsProvider.getNumber());
            try {
                evaluationsProvider.getEstimatedMeasurement(-1);
                Assert.fail("an exception should have been thrown");
            } catch (OrekitException oe) {
                Assert.assertEquals(LocalizedCoreFormats.OUT_OF_RANGE_SIMPLE, oe.getSpecifier());
            }
            try {
                evaluationsProvider.getEstimatedMeasurement(r12.size() + r1.size());
                Assert.fail("an exception should have been thrown");
            } catch (OrekitException oe) {
                Assert.assertEquals(LocalizedCoreFormats.OUT_OF_RANGE_SIMPLE, oe.getSpecifier());
            }
            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, 2, 3, 0.0, 2.3e-06, 0.0, 6.6e-06, 0.0, 6.2e-07, 0.0, 2.8e-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()), 1.6e-6);
    Assert.assertEquals(0.0, Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(), determined.getPVCoordinates().getVelocity()), 1.6e-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 29 with ParameterDriversList

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

the class BatchLSEstimatorTest method testKeplerRangeWithOnBoardAntennaOffset.

/**
 * Perfect range measurements with a biased start and an on-board antenna range offset
 * @throws OrekitException
 */
@Test
public void testKeplerRangeWithOnBoardAntennaOffset() 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);
    propagatorBuilder.setAttitudeProvider(new LofOffset(propagatorBuilder.getFrame(), LOFType.LVLH));
    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);
    // create orbit estimator
    final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(), propagatorBuilder);
    final OnBoardAntennaRangeModifier obaModifier = new OnBoardAntennaRangeModifier(antennaPhaseCenter);
    for (final ObservedMeasurement<?> range : measurements) {
        ((Range) range).addModifier(obaModifier);
        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;
            Assert.assertEquals(measurements.size(), evaluationsProvider.getNumber());
            try {
                evaluationsProvider.getEstimatedMeasurement(-1);
                Assert.fail("an exception should have been thrown");
            } catch (OrekitException oe) {
                Assert.assertEquals(LocalizedCoreFormats.OUT_OF_RANGE_SIMPLE, oe.getSpecifier());
            }
            try {
                evaluationsProvider.getEstimatedMeasurement(measurements.size());
                Assert.fail("an exception should have been thrown");
            } catch (OrekitException oe) {
                Assert.assertEquals(LocalizedCoreFormats.OUT_OF_RANGE_SIMPLE, oe.getSpecifier());
            }
            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;
            }
        }
    });
    ParameterDriver aDriver = estimator.getOrbitalParametersDrivers(true).getDrivers().get(0);
    Assert.assertEquals("a", aDriver.getName());
    aDriver.setValue(aDriver.getValue() + 1.2);
    aDriver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
    EstimationTestUtils.checkFit(context, estimator, 2, 3, 0.0, 2.0e-5, 0.0, 5.2e-5, 0.0, 2.7e-5, 0.0, 1.1e-8);
    // got a default one
    for (final ParameterDriver driver : estimator.getOrbitalParametersDrivers(true).getDrivers()) {
        if ("a".equals(driver.getName())) {
            // 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(propagatorBuilder.getInitialOrbitDate()), 1.0e-15);
        }
    }
}