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

Example 6 with DelegatingDriver

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

the class KalmanOrbitDeterminationTest method testW3B.

@Test
public // Orbit determination for range, azimuth elevation measurements
void testW3B() 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/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));
    // 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[] { FastMath.pow(2.4e4, 2), FastMath.pow(1.e5, 2), FastMath.pow(4.e4, 2), FastMath.pow(3.5, 2), FastMath.pow(2., 2), FastMath.pow(0.6, 2) });
    // 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 });
    // Propagation covariance and process noise matrices
    final RealMatrix propagationP = MatrixUtils.createRealDiagonalMatrix(new double[] { // Cd
    FastMath.pow(2., 2), // leak-X
    FastMath.pow(5.7e-6, 2), // leak-X
    FastMath.pow(1.1e-11, 2), // leak-Y
    FastMath.pow(7.68e-7, 2), // leak-Y
    FastMath.pow(1.26e-10, 2), // leak-Z
    FastMath.pow(5.56e-6, 2), // leak-Z
    FastMath.pow(2.79e-10, 2) });
    final RealMatrix propagationQ = MatrixUtils.createRealDiagonalMatrix(new double[] { // Cd
    FastMath.pow(1e-3, 2), // Leaks
    0., // Leaks
    0., // Leaks
    0., // Leaks
    0., // Leaks
    0., // Leaks
    0. });
    // Measurement covariance and process noise matrices
    // az/el bias sigma = 0.06deg
    // range bias sigma = 100m
    final double angularVariance = FastMath.pow(FastMath.toRadians(0.06), 2);
    final double rangeVariance = FastMath.pow(500., 2);
    final RealMatrix measurementP = MatrixUtils.createRealDiagonalMatrix(new double[] { angularVariance, angularVariance, rangeVariance, angularVariance, angularVariance, rangeVariance, angularVariance, angularVariance, rangeVariance, angularVariance, angularVariance, rangeVariance, angularVariance, angularVariance, rangeVariance });
    // Process noise sigma: 1e-6 for all
    final double measQ = FastMath.pow(1e-6, 2);
    final RealMatrix measurementQ = MatrixUtils.createRealIdentityMatrix(measurementP.getRowDimension()).scalarMultiply(measQ);
    // Kalman orbit determination run.
    ResultKalman kalmanW3B = run(input, orbitType, print, cartesianOrbitalP, cartesianOrbitalQ, propagationP, propagationQ, measurementP, measurementQ);
    // Tests
    // -----
    // Definition of the accuracy for the test
    final double distanceAccuracy = 0.1;
    // degrees
    final double angleAccuracy = 1e-5;
    // Number of measurements processed
    final int numberOfMeas = 521;
    Assert.assertEquals(numberOfMeas, kalmanW3B.getNumberOfMeasurements());
    // Test on propagator parameters
    // -----------------------------
    // Batch LS result
    // final double dragCoef  = -0.2154;
    final double dragCoef = 0.1931;
    Assert.assertEquals(dragCoef, kalmanW3B.propagatorParameters.getDrivers().get(0).getValue(), 1e-3);
    final Vector3D leakAcceleration0 = new Vector3D(kalmanW3B.propagatorParameters.getDrivers().get(1).getValue(), kalmanW3B.propagatorParameters.getDrivers().get(3).getValue(), kalmanW3B.propagatorParameters.getDrivers().get(5).getValue());
    // Batch LS results
    // Assert.assertEquals(8.002e-6, leakAcceleration0.getNorm(), 1.0e-8);
    Assert.assertEquals(5.994e-6, leakAcceleration0.getNorm(), 1.0e-8);
    final Vector3D leakAcceleration1 = new Vector3D(kalmanW3B.propagatorParameters.getDrivers().get(2).getValue(), kalmanW3B.propagatorParameters.getDrivers().get(4).getValue(), kalmanW3B.propagatorParameters.getDrivers().get(6).getValue());
    // Batch LS results
    // Assert.assertEquals(3.058e-10, leakAcceleration1.getNorm(), 1.0e-12);
    Assert.assertEquals(1.831e-10, leakAcceleration1.getNorm(), 1.0e-12);
    // Test on measurements parameters
    // -------------------------------
    final List<DelegatingDriver> list = new ArrayList<DelegatingDriver>();
    list.addAll(kalmanW3B.measurementsParameters.getDrivers());
    sortParametersChanges(list);
    // Station CastleRock
    // Batch LS results
    // final double[] CastleAzElBias  = { 0.062701342, -0.003613508 };
    // final double   CastleRangeBias = 11274.4677;
    final double[] CastleAzElBias = { 0.062635, -0.003672 };
    final double CastleRangeBias = 11289.3678;
    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
    // Batch LS results
    // final double[] FucAzElBias  = { -0.053526137, 0.075483886 };
    // final double   FucRangeBias = 13467.8256;
    final double[] FucAzElBias = { -0.053298, 0.075589 };
    final double FucRangeBias = 13482.0715;
    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
    // Batch LS results
    // final double[] KumAzElBias  = { -0.023574208, -0.054520756 };
    // final double   KumRangeBias = 13512.57594;
    final double[] KumAzElBias = { -0.022805, -0.055057 };
    final double KumRangeBias = 13502.7459;
    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
    // Batch LS results
    // final double[] PreAzElBias = { 0.030201539, 0.009747877 };
    // final double PreRangeBias = 13594.11889;
    final double[] PreAzElBias = { 0.030353, 0.009658 };
    final double PreRangeBias = 13609.2516;
    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
    // Batch LS results
    // final double[] UraAzElBias = { 0.167814449, -0.12305252 };
    // final double UraRangeBias = 13450.26738;
    final double[] UraAzElBias = { 0.167519, -0.122842 };
    final double UraRangeBias = 13441.7019;
    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 = { -12.981, 18.046, -1.133, 5.312 };
    Assert.assertEquals(nbRange, kalmanW3B.getRangeStat().getN());
    Assert.assertEquals(RefStatRange[0], kalmanW3B.getRangeStat().getMin(), distanceAccuracy);
    Assert.assertEquals(RefStatRange[1], kalmanW3B.getRangeStat().getMax(), distanceAccuracy);
    Assert.assertEquals(RefStatRange[2], kalmanW3B.getRangeStat().getMean(), distanceAccuracy);
    Assert.assertEquals(RefStatRange[3], kalmanW3B.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.041441, 0.023473, -0.004426, 0.009911 };
    Assert.assertEquals(nbAzi, kalmanW3B.getAzimStat().getN());
    Assert.assertEquals(RefStatAzi[0], kalmanW3B.getAzimStat().getMin(), angleAccuracy);
    Assert.assertEquals(RefStatAzi[1], kalmanW3B.getAzimStat().getMax(), angleAccuracy);
    Assert.assertEquals(RefStatAzi[2], kalmanW3B.getAzimStat().getMean(), angleAccuracy);
    Assert.assertEquals(RefStatAzi[3], kalmanW3B.getAzimStat().getStandardDeviation(), angleAccuracy);
    // test on statistic for the elevation residuals
    final long nbEle = 339;
    final double[] RefStatEle = { -0.025399, 0.043345, 0.001011, 0.010636 };
    Assert.assertEquals(nbEle, kalmanW3B.getElevStat().getN());
    Assert.assertEquals(RefStatEle[0], kalmanW3B.getElevStat().getMin(), angleAccuracy);
    Assert.assertEquals(RefStatEle[1], kalmanW3B.getElevStat().getMax(), angleAccuracy);
    Assert.assertEquals(RefStatEle[2], kalmanW3B.getElevStat().getMean(), angleAccuracy);
    Assert.assertEquals(RefStatEle[3], kalmanW3B.getElevStat().getStandardDeviation(), angleAccuracy);
    RealMatrix covariances = kalmanW3B.getCovariances();
    Assert.assertEquals(28, covariances.getRowDimension());
    Assert.assertEquals(28, covariances.getColumnDimension());
    // drag coefficient variance
    Assert.assertEquals(0.016349, covariances.getEntry(6, 6), 1.0e-5);
    // leak-X constant term variance
    Assert.assertEquals(2.047303E-13, covariances.getEntry(7, 7), 1.0e-16);
    // leak-Y constant term variance
    Assert.assertEquals(5.462497E-13, covariances.getEntry(9, 9), 1.0e-15);
    // leak-Z constant term variance
    Assert.assertEquals(1.717781E-11, covariances.getEntry(11, 11), 1.0e-15);
    // Test on orbital parameters
    // Done at the end to avoid changing the estimated propagation parameters
    // ----------------------------------------------------------------------
    // Estimated position and velocity
    final Vector3D estimatedPos = kalmanW3B.getEstimatedPV().getPosition();
    final Vector3D estimatedVel = kalmanW3B.getEstimatedPV().getVelocity();
    // Reference position and velocity at initial date (same as in batch LS test)
    final Vector3D refPos0 = new Vector3D(-40541446.255, -9905357.41, 206777.413);
    final Vector3D refVel0 = new Vector3D(759.0685, -1476.5156, 54.793);
    // Gather the selected propagation parameters and initialize them to the values found
    // with the batch LS method
    final ParameterDriversList refPropagationParameters = kalmanW3B.propagatorParameters;
    final double dragCoefRef = -0.215433133145843;
    final double[] leakXRef = { +5.69040439901955E-06, 1.09710906802403E-11 };
    final double[] leakYRef = { -7.66440256777678E-07, 1.25467464335066E-10 };
    final double[] leakZRef = { -5.574055079952E-06, 2.78703463746911E-10 };
    for (DelegatingDriver driver : refPropagationParameters.getDrivers()) {
        switch(driver.getName()) {
            case "drag coefficient":
                driver.setValue(dragCoefRef);
                break;
            case "leak-X[0]":
                driver.setValue(leakXRef[0]);
                break;
            case "leak-X[1]":
                driver.setValue(leakXRef[1]);
                break;
            case "leak-Y[0]":
                driver.setValue(leakYRef[0]);
                break;
            case "leak-Y[1]":
                driver.setValue(leakYRef[1]);
                break;
            case "leak-Z[0]":
                driver.setValue(leakZRef[0]);
                break;
            case "leak-Z[1]":
                driver.setValue(leakZRef[1]);
                break;
        }
    }
    // Run the reference until Kalman last date
    final Orbit refOrbit = runReference(input, orbitType, refPos0, refVel0, refPropagationParameters, kalmanW3B.getEstimatedPV().getDate());
    // Test on last orbit
    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);
    // FIXME: debug - Comparison with batch LS is bad
    final double debugDistanceAccuracy = 234.73;
    final double debugVelocityAccuracy = 0.086;
    Assert.assertEquals(0.0, Vector3D.distance(refPos, estimatedPos), debugDistanceAccuracy);
    Assert.assertEquals(0.0, Vector3D.distance(refVel, estimatedVel), debugVelocityAccuracy);
    // 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);
    }
}

Example 7 with DelegatingDriver

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

the class KalmanOrbitDeterminationTest method runReference.

/**
 * Use the physical models in the input file
 * Incorporate the initial reference values
 * And run the propagation until the last measurement to get the reference orbit at the same date
 * as the Kalman filter
 * @param input Input configuration file
 * @param orbitType Orbit type to use (calculation and display)
 * @param refPosition Initial reference position
 * @param refVelocity Initial reference velocity
 * @param refPropagationParameters Reference propagation parameters
 * @param kalmanFinalDate The final date of the Kalman filter
 * @return The reference orbit at the same date as the Kalman filter
 * @throws IOException Input file cannot be opened
 * @throws IllegalArgumentException Issue in key/value reading of input file
 * @throws OrekitException An Orekit exception... should be explicit
 * @throws ParseException Parsing of the input file or measurement file failed
 */
private Orbit runReference(final File input, final OrbitType orbitType, final Vector3D refPosition, final Vector3D refVelocity, final ParameterDriversList refPropagationParameters, final AbsoluteDate kalmanFinalDate) throws IOException, IllegalArgumentException, OrekitException, ParseException {
    // Read input parameters
    KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<ParameterKey>(ParameterKey.class);
    parser.parseInput(input.getAbsolutePath(), new FileInputStream(input));
    // Gravity field
    GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-5c.gfc", true));
    final NormalizedSphericalHarmonicsProvider gravityField = createGravityField(parser);
    // Orbit initial guess
    Orbit initialRefOrbit = new CartesianOrbit(new PVCoordinates(refPosition, refVelocity), parser.getInertialFrame(ParameterKey.INERTIAL_FRAME), parser.getDate(ParameterKey.ORBIT_DATE, TimeScalesFactory.getUTC()), gravityField.getMu());
    // Convert to desired orbit type
    initialRefOrbit = orbitType.convertType(initialRefOrbit);
    // IERS conventions
    final IERSConventions conventions;
    if (!parser.containsKey(ParameterKey.IERS_CONVENTIONS)) {
        conventions = IERSConventions.IERS_2010;
    } else {
        conventions = IERSConventions.valueOf("IERS_" + parser.getInt(ParameterKey.IERS_CONVENTIONS));
    }
    // Central body
    final OneAxisEllipsoid body = createBody(parser);
    // Propagator builder
    final NumericalPropagatorBuilder propagatorBuilder = createPropagatorBuilder(parser, conventions, gravityField, body, initialRefOrbit);
    // Force the selected propagation parameters to their reference values
    if (refPropagationParameters != null) {
        for (DelegatingDriver refDriver : refPropagationParameters.getDrivers()) {
            for (DelegatingDriver driver : propagatorBuilder.getPropagationParametersDrivers().getDrivers()) {
                if (driver.getName().equals(refDriver.getName())) {
                    driver.setValue(refDriver.getValue());
                }
            }
        }
    }
    // Build the reference propagator
    final NumericalPropagator propagator = propagatorBuilder.buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
    // Propagate until last date and return the orbit
    return propagator.propagate(kalmanFinalDate).getOrbit();
}

Example 8 with DelegatingDriver

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

the class KalmanOrbitDeterminationTest method run.

/**
 * Function running the Kalman filter estimation.
 * @param input Input configuration file
 * @param orbitType Orbit type to use (calculation and display)
 * @param print Choose whether the results are printed on console or not
 * @param cartesianOrbitalP Orbital part of the initial covariance matrix in Cartesian formalism
 * @param cartesianOrbitalQ Orbital part of the process noise matrix in Cartesian formalism
 * @param propagationP Propagation part of the initial covariance matrix
 * @param propagationQ Propagation part of the process noise matrix
 * @param measurementP Measurement part of the initial covariance matrix
 * @param measurementQ Measurement part of the process noise matrix
 */
private ResultKalman run(final File input, final OrbitType orbitType, final boolean print, final RealMatrix cartesianOrbitalP, final RealMatrix cartesianOrbitalQ, final RealMatrix propagationP, final RealMatrix propagationQ, final RealMatrix measurementP, final RealMatrix measurementQ) throws IOException, IllegalArgumentException, OrekitException, ParseException {
    // Read input parameters
    KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<ParameterKey>(ParameterKey.class);
    parser.parseInput(input.getAbsolutePath(), new FileInputStream(input));
    // Log files
    final RangeLog rangeLog = new RangeLog();
    final RangeRateLog rangeRateLog = new RangeRateLog();
    final AzimuthLog azimuthLog = new AzimuthLog();
    final ElevationLog elevationLog = new ElevationLog();
    final PositionLog positionLog = new PositionLog();
    final VelocityLog velocityLog = new VelocityLog();
    // Gravity field
    GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-5c.gfc", true));
    final NormalizedSphericalHarmonicsProvider gravityField = createGravityField(parser);
    // Orbit initial guess
    Orbit initialGuess = createOrbit(parser, gravityField.getMu());
    // Convert to desired orbit type
    initialGuess = orbitType.convertType(initialGuess);
    // IERS conventions
    final IERSConventions conventions;
    if (!parser.containsKey(ParameterKey.IERS_CONVENTIONS)) {
        conventions = IERSConventions.IERS_2010;
    } else {
        conventions = IERSConventions.valueOf("IERS_" + parser.getInt(ParameterKey.IERS_CONVENTIONS));
    }
    // Central body
    final OneAxisEllipsoid body = createBody(parser);
    // Propagator builder
    final NumericalPropagatorBuilder propagatorBuilder = createPropagatorBuilder(parser, conventions, gravityField, body, initialGuess);
    // Measurements
    final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
    for (final String fileName : parser.getStringsList(ParameterKey.MEASUREMENTS_FILES, ',')) {
        measurements.addAll(readMeasurements(new File(input.getParentFile(), fileName), createStationsData(parser, body), createPVData(parser), createSatRangeBias(parser), createWeights(parser), createRangeOutliersManager(parser), createRangeRateOutliersManager(parser), createAzElOutliersManager(parser), createPVOutliersManager(parser)));
    }
    // Building the Kalman filter:
    // - Gather the estimated measurement parameters in a list
    // - Prepare the initial covariance matrix and the process noise matrix
    // - Build the Kalman filter
    // --------------------------------------------------------------------
    // Build the list of estimated measurements
    final ParameterDriversList estimatedMeasurementsParameters = new ParameterDriversList();
    for (ObservedMeasurement<?> measurement : measurements) {
        final List<ParameterDriver> drivers = measurement.getParametersDrivers();
        for (ParameterDriver driver : drivers) {
            if (driver.isSelected()) {
                // Add the driver
                estimatedMeasurementsParameters.add(driver);
            }
        }
    }
    // Sort the list lexicographically
    estimatedMeasurementsParameters.sort();
    // Orbital covariance matrix initialization
    // Jacobian of the orbital parameters w/r to Cartesian
    final double[][] dYdC = new double[6][6];
    initialGuess.getJacobianWrtCartesian(propagatorBuilder.getPositionAngle(), dYdC);
    final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
    RealMatrix orbitalP = Jac.multiply(cartesianOrbitalP.multiply(Jac.transpose()));
    // Orbital process noise matrix
    RealMatrix orbitalQ = Jac.multiply(cartesianOrbitalQ.multiply(Jac.transpose()));
    // Build the full covariance matrix and process noise matrix
    final int nbPropag = (propagationP != null) ? propagationP.getRowDimension() : 0;
    final int nbMeas = (measurementP != null) ? measurementP.getRowDimension() : 0;
    final RealMatrix initialP = MatrixUtils.createRealMatrix(6 + nbPropag + nbMeas, 6 + nbPropag + nbMeas);
    final RealMatrix Q = MatrixUtils.createRealMatrix(6 + nbPropag + nbMeas, 6 + nbPropag + nbMeas);
    // Orbital part
    initialP.setSubMatrix(orbitalP.getData(), 0, 0);
    Q.setSubMatrix(orbitalQ.getData(), 0, 0);
    // Propagation part
    if (propagationP != null) {
        initialP.setSubMatrix(propagationP.getData(), 6, 6);
        Q.setSubMatrix(propagationQ.getData(), 6, 6);
    }
    // Measurement part
    if (measurementP != null) {
        initialP.setSubMatrix(measurementP.getData(), 6 + nbPropag, 6 + nbPropag);
        Q.setSubMatrix(measurementQ.getData(), 6 + nbPropag, 6 + nbPropag);
    }
    // Build the Kalman
    KalmanEstimatorBuilder kalmanBuilder = new KalmanEstimatorBuilder();
    kalmanBuilder.builder(propagatorBuilder);
    kalmanBuilder.estimatedMeasurementsParameters(estimatedMeasurementsParameters);
    kalmanBuilder.initialCovarianceMatrix(initialP);
    kalmanBuilder.processNoiseMatrixProvider(new ConstantProcessNoise(Q));
    final KalmanEstimator kalman = kalmanBuilder.build();
    // Add an observer
    kalman.setObserver(new KalmanObserver() {

        /**
         * Date of the first measurement.
         */
        private AbsoluteDate t0;

        /**
         * {@inheritDoc}
         * @throws OrekitException
         */
        @Override
        @SuppressWarnings("unchecked")
        public void evaluationPerformed(final KalmanEstimation estimation) throws OrekitException {
            // Current measurement number, date and status
            final EstimatedMeasurement<?> estimatedMeasurement = estimation.getCorrectedMeasurement();
            final int currentNumber = estimation.getCurrentMeasurementNumber();
            final AbsoluteDate currentDate = estimatedMeasurement.getDate();
            final EstimatedMeasurement.Status currentStatus = estimatedMeasurement.getStatus();
            // Current estimated measurement
            final ObservedMeasurement<?> observedMeasurement = estimatedMeasurement.getObservedMeasurement();
            // Measurement type & Station name
            String measType = "";
            String stationName = "";
            // Register the measurement in the proper measurement logger
            if (observedMeasurement instanceof Range) {
                // Add the tuple (estimation, prediction) to the log
                rangeLog.add(currentNumber, (EstimatedMeasurement<Range>) estimatedMeasurement);
                // Measurement type & Station name
                measType = "RANGE";
                stationName = ((EstimatedMeasurement<Range>) estimatedMeasurement).getObservedMeasurement().getStation().getBaseFrame().getName();
            } else if (observedMeasurement instanceof RangeRate) {
                rangeRateLog.add(currentNumber, (EstimatedMeasurement<RangeRate>) estimatedMeasurement);
                measType = "RANGE_RATE";
                stationName = ((EstimatedMeasurement<RangeRate>) estimatedMeasurement).getObservedMeasurement().getStation().getBaseFrame().getName();
            } else if (observedMeasurement instanceof AngularAzEl) {
                azimuthLog.add(currentNumber, (EstimatedMeasurement<AngularAzEl>) estimatedMeasurement);
                elevationLog.add(currentNumber, (EstimatedMeasurement<AngularAzEl>) estimatedMeasurement);
                measType = "AZ_EL";
                stationName = ((EstimatedMeasurement<AngularAzEl>) estimatedMeasurement).getObservedMeasurement().getStation().getBaseFrame().getName();
            } else if (observedMeasurement instanceof PV) {
                positionLog.add(currentNumber, (EstimatedMeasurement<PV>) estimatedMeasurement);
                velocityLog.add(currentNumber, (EstimatedMeasurement<PV>) estimatedMeasurement);
                measType = "PV";
            }
            // Header
            if (print) {
                if (currentNumber == 1) {
                    // Set t0 to first measurement date
                    t0 = currentDate;
                    // Print header
                    final String formatHeader = "%-4s\t%-25s\t%15s\t%-10s\t%-10s\t%-20s\t%20s\t%20s";
                    String header = String.format(Locale.US, formatHeader, "Nb", "Epoch", "Dt[s]", "Status", "Type", "Station", "DP Corr", "DV Corr");
                    // Orbital drivers
                    for (DelegatingDriver driver : estimation.getEstimatedOrbitalParameters().getDrivers()) {
                        header += String.format(Locale.US, "\t%20s", driver.getName());
                        header += String.format(Locale.US, "\t%20s", "D" + driver.getName());
                    }
                    // Propagation drivers
                    for (DelegatingDriver driver : estimation.getEstimatedPropagationParameters().getDrivers()) {
                        header += String.format(Locale.US, "\t%20s", driver.getName());
                        header += String.format(Locale.US, "\t%20s", "D" + driver.getName());
                    }
                    // Measurements drivers
                    for (DelegatingDriver driver : estimation.getEstimatedMeasurementsParameters().getDrivers()) {
                        header += String.format(Locale.US, "\t%20s", driver.getName());
                        header += String.format(Locale.US, "\t%20s", "D" + driver.getName());
                    }
                    // Print header
                    System.out.println(header);
                }
                // Print current measurement info in terminal
                String line = "";
                // Line format
                final String lineFormat = "%4d\t%-25s\t%15.3f\t%-10s\t%-10s\t%-20s\t%20.9e\t%20.9e";
                // Orbital correction = DP & DV between predicted orbit and estimated orbit
                final Vector3D predictedP = estimation.getPredictedSpacecraftStates()[0].getPVCoordinates().getPosition();
                final Vector3D predictedV = estimation.getPredictedSpacecraftStates()[0].getPVCoordinates().getVelocity();
                final Vector3D estimatedP = estimation.getCorrectedSpacecraftStates()[0].getPVCoordinates().getPosition();
                final Vector3D estimatedV = estimation.getCorrectedSpacecraftStates()[0].getPVCoordinates().getVelocity();
                final double DPcorr = Vector3D.distance(predictedP, estimatedP);
                final double DVcorr = Vector3D.distance(predictedV, estimatedV);
                line = String.format(Locale.US, lineFormat, currentNumber, currentDate.toString(), currentDate.durationFrom(t0), currentStatus.toString(), measType, stationName, DPcorr, DVcorr);
                // Handle parameters printing (value and error)
                int jPar = 0;
                final RealMatrix Pest = estimation.getPhysicalEstimatedCovarianceMatrix();
                // Orbital drivers
                for (DelegatingDriver driver : estimation.getEstimatedOrbitalParameters().getDrivers()) {
                    line += String.format(Locale.US, "\t%20.9f", driver.getValue());
                    line += String.format(Locale.US, "\t%20.9e", FastMath.sqrt(Pest.getEntry(jPar, jPar)));
                    jPar++;
                }
                // Propagation drivers
                for (DelegatingDriver driver : estimation.getEstimatedPropagationParameters().getDrivers()) {
                    line += String.format(Locale.US, "\t%20.9f", driver.getValue());
                    line += String.format(Locale.US, "\t%20.9e", FastMath.sqrt(Pest.getEntry(jPar, jPar)));
                    jPar++;
                }
                // Measurements drivers
                for (DelegatingDriver driver : estimatedMeasurementsParameters.getDrivers()) {
                    line += String.format(Locale.US, "\t%20.9f", driver.getValue());
                    line += String.format(Locale.US, "\t%20.9e", FastMath.sqrt(Pest.getEntry(jPar, jPar)));
                    jPar++;
                }
                // Print the line
                System.out.println(line);
            }
        }
    });
    // Process the list measurements
    final Orbit estimated = kalman.processMeasurements(measurements).getInitialState().getOrbit();
    // Get the last estimated physical covariances
    final RealMatrix covarianceMatrix = kalman.getPhysicalEstimatedCovarianceMatrix();
    // Parameters and measurements.
    final ParameterDriversList propagationParameters = kalman.getPropagationParametersDrivers(true);
    final ParameterDriversList measurementsParameters = kalman.getEstimatedMeasurementsParameters();
    // Eventually, print parameter changes, statistics and covariances
    if (print) {
        // Display parameter change for non orbital drivers
        int length = 0;
        for (final ParameterDriver parameterDriver : propagationParameters.getDrivers()) {
            length = FastMath.max(length, parameterDriver.getName().length());
        }
        for (final ParameterDriver parameterDriver : measurementsParameters.getDrivers()) {
            length = FastMath.max(length, parameterDriver.getName().length());
        }
        if (propagationParameters.getNbParams() > 0) {
            displayParametersChanges(System.out, "Estimated propagator parameters changes: ", true, length, propagationParameters);
        }
        if (measurementsParameters.getNbParams() > 0) {
            displayParametersChanges(System.out, "Estimated measurements parameters changes: ", true, length, measurementsParameters);
        }
        // Measurements statistics summary
        System.out.println("");
        rangeLog.displaySummary(System.out);
        rangeRateLog.displaySummary(System.out);
        azimuthLog.displaySummary(System.out);
        elevationLog.displaySummary(System.out);
        positionLog.displaySummary(System.out);
        velocityLog.displaySummary(System.out);
        // Covariances and sigmas
        displayFinalCovariances(System.out, kalman);
    }
    // Instantiation of the results
    return new ResultKalman(propagationParameters, measurementsParameters, kalman.getCurrentMeasurementNumber(), estimated.getPVCoordinates(), rangeLog.createStatisticsSummary(), rangeRateLog.createStatisticsSummary(), azimuthLog.createStatisticsSummary(), elevationLog.createStatisticsSummary(), positionLog.createStatisticsSummary(), velocityLog.createStatisticsSummary(), covarianceMatrix);
}

Example 9 with DelegatingDriver

use of org.orekit.utils.ParameterDriversList.DelegatingDriver 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 10 with DelegatingDriver

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

the class OrbitDeterminationTest method testLageos2.

@Test
public // Orbit determination for Lageos2 based on SLR (range) measurements
void testLageos2() throws URISyntaxException, IllegalArgumentException, IOException, OrekitException, ParseException {
    // input in tutorial resources directory/output
    final String inputPath = OrbitDeterminationTest.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));
    // orbit determination run.
    ResultOD odLageos2 = run(input, false);
    // test
    // definition of the accuracy for the test
    final double distanceAccuracy = 0.1;
    final double velocityAccuracy = 1e-4;
    // test on the convergence
    final int numberOfIte = 4;
    final int numberOfEval = 4;
    Assert.assertEquals(numberOfIte, odLageos2.getNumberOfIteration());
    Assert.assertEquals(numberOfEval, odLageos2.getNumberOfEvaluation());
    // test on the estimated position and velocity
    final Vector3D estimatedPos = odLageos2.getEstimatedPV().getPosition();
    final Vector3D estimatedVel = odLageos2.getEstimatedPV().getVelocity();
    // final Vector3D refPos = new Vector3D(-5532124.989973327, 10025700.01763335, -3578940.840115321);
    // final Vector3D refVel = new Vector3D(-3871.2736402553, -607.8775965705, 4280.9744110925);
    final Vector3D refPos = new Vector3D(-5532131.956902, 10025696.592156, -3578940.040009);
    final Vector3D refVel = new Vector3D(-3871.275109, -607.880985, 4280.972530);
    Assert.assertEquals(0.0, Vector3D.distance(refPos, estimatedPos), distanceAccuracy);
    Assert.assertEquals(0.0, Vector3D.distance(refVel, estimatedVel), velocityAccuracy);
    // test on measurements parameters
    final List<DelegatingDriver> list = new ArrayList<DelegatingDriver>();
    list.addAll(odLageos2.measurementsParameters.getDrivers());
    sortParametersChanges(list);
    // final double[] stationOffSet = { -1.351682,  -2.180542,  -5.278784 };
    // final double rangeBias = -7.923393;
    final double[] stationOffSet = { 1.659203, 0.861250, -0.885352 };
    final double rangeBias = -0.286275;
    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;
    // final double[] RefStatRange = { -2.795816, 6.171529, 0.310848, 1.657809 };
    final double[] RefStatRange = { -2.431135, 2.218644, 0.038483, 0.982017 };
    Assert.assertEquals(nbRange, odLageos2.getRangeStat().getN());
    Assert.assertEquals(RefStatRange[0], odLageos2.getRangeStat().getMin(), distanceAccuracy);
    Assert.assertEquals(RefStatRange[1], odLageos2.getRangeStat().getMax(), distanceAccuracy);
    Assert.assertEquals(RefStatRange[2], odLageos2.getRangeStat().getMean(), distanceAccuracy);
    Assert.assertEquals(RefStatRange[3], odLageos2.getRangeStat().getStandardDeviation(), distanceAccuracy);
}