Examples with RangeRate org.orekit.estimation.measurements.RangeRate used on opensource projects

Example 6 with RangeRate

use of org.orekit.estimation.measurements.RangeRate in project Orekit by CS-SI.

the class OrbitDetermination method createStationsData.

/**
 * Set up stations.
 * @param parser input file parser
 * @param body central body
 * @return name to station data map
 * @exception OrekitException if some frame transforms cannot be computed
 * @throws NoSuchElementException if input parameters are missing
 */
private Map<String, StationData> createStationsData(final KeyValueFileParser<ParameterKey> parser, final OneAxisEllipsoid body) throws OrekitException, NoSuchElementException {
    final Map<String, StationData> stations = new HashMap<String, StationData>();
    final String[] stationNames = parser.getStringArray(ParameterKey.GROUND_STATION_NAME);
    final double[] stationLatitudes = parser.getAngleArray(ParameterKey.GROUND_STATION_LATITUDE);
    final double[] stationLongitudes = parser.getAngleArray(ParameterKey.GROUND_STATION_LONGITUDE);
    final double[] stationAltitudes = parser.getDoubleArray(ParameterKey.GROUND_STATION_ALTITUDE);
    final boolean[] stationPositionEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_POSITION_ESTIMATED);
    final double[] stationRangeSigma = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_SIGMA);
    final double[] stationRangeBias = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS);
    final double[] stationRangeBiasMin = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS_MIN);
    final double[] stationRangeBiasMax = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS_MAX);
    final boolean[] stationRangeBiasEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_BIAS_ESTIMATED);
    final double[] stationRangeRateSigma = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_SIGMA);
    final double[] stationRangeRateBias = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS);
    final double[] stationRangeRateBiasMin = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_MIN);
    final double[] stationRangeRateBiasMax = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_MAX);
    final boolean[] stationRangeRateBiasEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_ESTIMATED);
    final double[] stationAzimuthSigma = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_SIGMA);
    final double[] stationAzimuthBias = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS);
    final double[] stationAzimuthBiasMin = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS_MIN);
    final double[] stationAzimuthBiasMax = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS_MAX);
    final double[] stationElevationSigma = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_SIGMA);
    final double[] stationElevationBias = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS);
    final double[] stationElevationBiasMin = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS_MIN);
    final double[] stationElevationBiasMax = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS_MAX);
    final boolean[] stationAzElBiasesEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_AZ_EL_BIASES_ESTIMATED);
    final boolean[] stationElevationRefraction = parser.getBooleanArray(ParameterKey.GROUND_STATION_ELEVATION_REFRACTION_CORRECTION);
    final boolean[] stationRangeTropospheric = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_TROPOSPHERIC_CORRECTION);
    for (int i = 0; i < stationNames.length; ++i) {
        // the station itself
        final GeodeticPoint position = new GeodeticPoint(stationLatitudes[i], stationLongitudes[i], stationAltitudes[i]);
        final TopocentricFrame topo = new TopocentricFrame(body, position, stationNames[i]);
        final GroundStation station = new GroundStation(topo);
        station.getEastOffsetDriver().setSelected(stationPositionEstimated[i]);
        station.getNorthOffsetDriver().setSelected(stationPositionEstimated[i]);
        station.getZenithOffsetDriver().setSelected(stationPositionEstimated[i]);
        // range
        final double rangeSigma = stationRangeSigma[i];
        final Bias<Range> rangeBias;
        if (FastMath.abs(stationRangeBias[i]) >= Precision.SAFE_MIN || stationRangeBiasEstimated[i]) {
            rangeBias = new Bias<Range>(new String[] { stationNames[i] + "/range bias" }, new double[] { stationRangeBias[i] }, new double[] { rangeSigma }, new double[] { stationRangeBiasMin[i] }, new double[] { stationRangeBiasMax[i] });
            rangeBias.getParametersDrivers().get(0).setSelected(stationRangeBiasEstimated[i]);
        } else {
            // bias fixed to zero, we don't need to create a modifier for this
            rangeBias = null;
        }
        // range rate
        final double rangeRateSigma = stationRangeRateSigma[i];
        final Bias<RangeRate> rangeRateBias;
        if (FastMath.abs(stationRangeRateBias[i]) >= Precision.SAFE_MIN || stationRangeRateBiasEstimated[i]) {
            rangeRateBias = new Bias<RangeRate>(new String[] { stationNames[i] + "/range rate bias" }, new double[] { stationRangeRateBias[i] }, new double[] { rangeRateSigma }, new double[] { stationRangeRateBiasMin[i] }, new double[] { stationRangeRateBiasMax[i] });
            rangeRateBias.getParametersDrivers().get(0).setSelected(stationRangeRateBiasEstimated[i]);
        } else {
            // bias fixed to zero, we don't need to create a modifier for this
            rangeRateBias = null;
        }
        // angular biases
        final double[] azELSigma = new double[] { stationAzimuthSigma[i], stationElevationSigma[i] };
        final Bias<AngularAzEl> azELBias;
        if (FastMath.abs(stationAzimuthBias[i]) >= Precision.SAFE_MIN || FastMath.abs(stationElevationBias[i]) >= Precision.SAFE_MIN || stationAzElBiasesEstimated[i]) {
            azELBias = new Bias<AngularAzEl>(new String[] { stationNames[i] + "/az bias", stationNames[i] + "/el bias" }, new double[] { stationAzimuthBias[i], stationElevationBias[i] }, azELSigma, new double[] { stationAzimuthBiasMin[i], stationElevationBiasMin[i] }, new double[] { stationAzimuthBiasMax[i], stationElevationBiasMax[i] });
            azELBias.getParametersDrivers().get(0).setSelected(stationAzElBiasesEstimated[i]);
            azELBias.getParametersDrivers().get(1).setSelected(stationAzElBiasesEstimated[i]);
        } else {
            // bias fixed to zero, we don't need to create a modifier for this
            azELBias = null;
        }
        // Refraction correction
        final AngularRadioRefractionModifier refractionCorrection;
        if (stationElevationRefraction[i]) {
            final double altitude = station.getBaseFrame().getPoint().getAltitude();
            final AtmosphericRefractionModel refractionModel = new EarthITU453AtmosphereRefraction(altitude);
            refractionCorrection = new AngularRadioRefractionModifier(refractionModel);
        } else {
            refractionCorrection = null;
        }
        // Tropospheric correction
        final RangeTroposphericDelayModifier rangeTroposphericCorrection;
        if (stationRangeTropospheric[i]) {
            final SaastamoinenModel troposphericModel = SaastamoinenModel.getStandardModel();
            rangeTroposphericCorrection = new RangeTroposphericDelayModifier(troposphericModel);
        } else {
            rangeTroposphericCorrection = null;
        }
        stations.put(stationNames[i], new StationData(station, rangeSigma, rangeBias, rangeRateSigma, rangeRateBias, azELSigma, azELBias, refractionCorrection, rangeTroposphericCorrection));
    }
    return stations;
}

Example 7 with RangeRate

use of org.orekit.estimation.measurements.RangeRate in project Orekit by CS-SI.

the class OrbitDetermination method run.

private void run(final File input, final File home) throws IOException, IllegalArgumentException, OrekitException, ParseException {
    // read input parameters
    KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<ParameterKey>(ParameterKey.class);
    try (final FileInputStream fis = new FileInputStream(input)) {
        parser.parseInput(input.getAbsolutePath(), fis);
    }
    // log file
    final String baseName;
    final PrintStream logStream;
    if (parser.containsKey(ParameterKey.OUTPUT_BASE_NAME) && parser.getString(ParameterKey.OUTPUT_BASE_NAME).length() > 0) {
        baseName = parser.getString(ParameterKey.OUTPUT_BASE_NAME);
        logStream = new PrintStream(new File(home, baseName + "-log.out"), "UTF-8");
    } else {
        baseName = null;
        logStream = null;
    }
    final RangeLog rangeLog = new RangeLog(home, baseName);
    final RangeRateLog rangeRateLog = new RangeRateLog(home, baseName);
    final AzimuthLog azimuthLog = new AzimuthLog(home, baseName);
    final ElevationLog elevationLog = new ElevationLog(home, baseName);
    final PositionLog positionLog = new PositionLog(home, baseName);
    final VelocityLog velocityLog = new VelocityLog(home, baseName);
    try {
        // gravity field
        final NormalizedSphericalHarmonicsProvider gravityField = createGravityField(parser);
        // Orbit initial guess
        final Orbit initialGuess = createOrbit(parser, gravityField.getMu());
        // 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);
        // estimator
        final BatchLSEstimator estimator = createEstimator(parser, propagatorBuilder);
        // 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)));
        }
        for (ObservedMeasurement<?> measurement : measurements) {
            estimator.addMeasurement(measurement);
        }
        // estimate orbit
        estimator.setObserver(new BatchLSObserver() {

            private PVCoordinates previousPV;

            {
                previousPV = initialGuess.getPVCoordinates();
                final String header = "iteration evaluations      ΔP(m)        ΔV(m/s)           RMS          nb Range    nb Range-rate  nb Angular     nb PV%n";
                System.out.format(Locale.US, header);
                if (logStream != null) {
                    logStream.format(Locale.US, header);
                }
            }

            /**
             * {@inheritDoc}
             */
            @Override
            public void evaluationPerformed(final int iterationsCount, final int evaluationsCount, final Orbit[] orbits, final ParameterDriversList estimatedOrbitalParameters, final ParameterDriversList estimatedPropagatorParameters, final ParameterDriversList estimatedMeasurementsParameters, final EstimationsProvider evaluationsProvider, final LeastSquaresProblem.Evaluation lspEvaluation) {
                PVCoordinates currentPV = orbits[0].getPVCoordinates();
                final String format0 = "    %2d         %2d                                 %16.12f     %s       %s     %s     %s%n";
                final String format = "    %2d         %2d      %13.6f %12.9f %16.12f     %s       %s     %s     %s%n";
                final EvaluationCounter<Range> rangeCounter = new EvaluationCounter<Range>();
                final EvaluationCounter<RangeRate> rangeRateCounter = new EvaluationCounter<RangeRate>();
                final EvaluationCounter<AngularAzEl> angularCounter = new EvaluationCounter<AngularAzEl>();
                final EvaluationCounter<PV> pvCounter = new EvaluationCounter<PV>();
                for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimator.getLastEstimations().entrySet()) {
                    if (entry.getKey() instanceof Range) {
                        @SuppressWarnings("unchecked") EstimatedMeasurement<Range> evaluation = (EstimatedMeasurement<Range>) entry.getValue();
                        rangeCounter.add(evaluation);
                    } else if (entry.getKey() instanceof RangeRate) {
                        @SuppressWarnings("unchecked") EstimatedMeasurement<RangeRate> evaluation = (EstimatedMeasurement<RangeRate>) entry.getValue();
                        rangeRateCounter.add(evaluation);
                    } else if (entry.getKey() instanceof AngularAzEl) {
                        @SuppressWarnings("unchecked") EstimatedMeasurement<AngularAzEl> evaluation = (EstimatedMeasurement<AngularAzEl>) entry.getValue();
                        angularCounter.add(evaluation);
                    } else if (entry.getKey() instanceof PV) {
                        @SuppressWarnings("unchecked") EstimatedMeasurement<PV> evaluation = (EstimatedMeasurement<PV>) entry.getValue();
                        pvCounter.add(evaluation);
                    }
                }
                if (evaluationsCount == 1) {
                    System.out.format(Locale.US, format0, iterationsCount, evaluationsCount, lspEvaluation.getRMS(), rangeCounter.format(8), rangeRateCounter.format(8), angularCounter.format(8), pvCounter.format(8));
                    if (logStream != null) {
                        logStream.format(Locale.US, format0, iterationsCount, evaluationsCount, lspEvaluation.getRMS(), rangeCounter.format(8), rangeRateCounter.format(8), angularCounter.format(8), pvCounter.format(8));
                    }
                } else {
                    System.out.format(Locale.US, format, iterationsCount, evaluationsCount, Vector3D.distance(previousPV.getPosition(), currentPV.getPosition()), Vector3D.distance(previousPV.getVelocity(), currentPV.getVelocity()), lspEvaluation.getRMS(), rangeCounter.format(8), rangeRateCounter.format(8), angularCounter.format(8), pvCounter.format(8));
                    if (logStream != null) {
                        logStream.format(Locale.US, format, iterationsCount, evaluationsCount, Vector3D.distance(previousPV.getPosition(), currentPV.getPosition()), Vector3D.distance(previousPV.getVelocity(), currentPV.getVelocity()), lspEvaluation.getRMS(), rangeCounter.format(8), rangeRateCounter.format(8), angularCounter.format(8), pvCounter.format(8));
                    }
                }
                previousPV = currentPV;
            }
        });
        Orbit estimated = estimator.estimate()[0].getInitialState().getOrbit();
        // compute some statistics
        for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimator.getLastEstimations().entrySet()) {
            if (entry.getKey() instanceof Range) {
                @SuppressWarnings("unchecked") EstimatedMeasurement<Range> evaluation = (EstimatedMeasurement<Range>) entry.getValue();
                rangeLog.add(evaluation);
            } else if (entry.getKey() instanceof RangeRate) {
                @SuppressWarnings("unchecked") EstimatedMeasurement<RangeRate> evaluation = (EstimatedMeasurement<RangeRate>) entry.getValue();
                rangeRateLog.add(evaluation);
            } else if (entry.getKey() instanceof AngularAzEl) {
                @SuppressWarnings("unchecked") EstimatedMeasurement<AngularAzEl> evaluation = (EstimatedMeasurement<AngularAzEl>) entry.getValue();
                azimuthLog.add(evaluation);
                elevationLog.add(evaluation);
            } else if (entry.getKey() instanceof PV) {
                @SuppressWarnings("unchecked") EstimatedMeasurement<PV> evaluation = (EstimatedMeasurement<PV>) entry.getValue();
                positionLog.add(evaluation);
                velocityLog.add(evaluation);
            }
        }
        System.out.println("Estimated orbit: " + estimated);
        if (logStream != null) {
            logStream.println("Estimated orbit: " + estimated);
        }
        final ParameterDriversList orbitalParameters = estimator.getOrbitalParametersDrivers(true);
        final ParameterDriversList propagatorParameters = estimator.getPropagatorParametersDrivers(true);
        final ParameterDriversList measurementsParameters = estimator.getMeasurementsParametersDrivers(true);
        int length = 0;
        for (final ParameterDriver parameterDriver : orbitalParameters.getDrivers()) {
            length = FastMath.max(length, parameterDriver.getName().length());
        }
        for (final ParameterDriver parameterDriver : propagatorParameters.getDrivers()) {
            length = FastMath.max(length, parameterDriver.getName().length());
        }
        for (final ParameterDriver parameterDriver : measurementsParameters.getDrivers()) {
            length = FastMath.max(length, parameterDriver.getName().length());
        }
        displayParametersChanges(System.out, "Estimated orbital parameters changes: ", false, length, orbitalParameters);
        if (logStream != null) {
            displayParametersChanges(logStream, "Estimated orbital parameters changes: ", false, length, orbitalParameters);
        }
        displayParametersChanges(System.out, "Estimated propagator parameters changes: ", true, length, propagatorParameters);
        if (logStream != null) {
            displayParametersChanges(logStream, "Estimated propagator parameters changes: ", true, length, propagatorParameters);
        }
        displayParametersChanges(System.out, "Estimated measurements parameters changes: ", true, length, measurementsParameters);
        if (logStream != null) {
            displayParametersChanges(logStream, "Estimated measurements parameters changes: ", true, length, measurementsParameters);
        }
        System.out.println("Number of iterations: " + estimator.getIterationsCount());
        System.out.println("Number of evaluations: " + estimator.getEvaluationsCount());
        rangeLog.displaySummary(System.out);
        rangeRateLog.displaySummary(System.out);
        azimuthLog.displaySummary(System.out);
        elevationLog.displaySummary(System.out);
        positionLog.displaySummary(System.out);
        velocityLog.displaySummary(System.out);
        if (logStream != null) {
            logStream.println("Number of iterations: " + estimator.getIterationsCount());
            logStream.println("Number of evaluations: " + estimator.getEvaluationsCount());
            rangeLog.displaySummary(logStream);
            rangeRateLog.displaySummary(logStream);
            azimuthLog.displaySummary(logStream);
            elevationLog.displaySummary(logStream);
            positionLog.displaySummary(logStream);
            velocityLog.displaySummary(logStream);
        }
        rangeLog.displayResiduals();
        rangeRateLog.displayResiduals();
        azimuthLog.displayResiduals();
        elevationLog.displayResiduals();
        positionLog.displayResiduals();
        velocityLog.displayResiduals();
    } finally {
        if (logStream != null) {
            logStream.close();
        }
        rangeLog.close();
        rangeRateLog.close();
        azimuthLog.close();
        elevationLog.close();
        positionLog.close();
        velocityLog.close();
    }
}

Example 8 with RangeRate

use of org.orekit.estimation.measurements.RangeRate 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 RangeRate

use of org.orekit.estimation.measurements.RangeRate in project Orekit by CS-SI.

the class OrbitDeterminationTest method createStationsData.

/**
 * Set up stations.
 * @param parser input file parser
 * @param body central body
 * @return name to station data map
 * @exception OrekitException if some frame transforms cannot be computed
 * @throws NoSuchElementException if input parameters are missing
 */
private Map<String, StationData> createStationsData(final KeyValueFileParser<ParameterKey> parser, final OneAxisEllipsoid body) throws OrekitException, NoSuchElementException {
    final Map<String, StationData> stations = new HashMap<String, StationData>();
    final String[] stationNames = parser.getStringArray(ParameterKey.GROUND_STATION_NAME);
    final double[] stationLatitudes = parser.getAngleArray(ParameterKey.GROUND_STATION_LATITUDE);
    final double[] stationLongitudes = parser.getAngleArray(ParameterKey.GROUND_STATION_LONGITUDE);
    final double[] stationAltitudes = parser.getDoubleArray(ParameterKey.GROUND_STATION_ALTITUDE);
    final boolean[] stationPositionEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_POSITION_ESTIMATED);
    final double[] stationRangeSigma = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_SIGMA);
    final double[] stationRangeBias = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS);
    final double[] stationRangeBiasMin = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS_MIN);
    final double[] stationRangeBiasMax = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS_MAX);
    final boolean[] stationRangeBiasEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_BIAS_ESTIMATED);
    final double[] stationRangeRateSigma = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_SIGMA);
    final double[] stationRangeRateBias = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS);
    final double[] stationRangeRateBiasMin = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_MIN);
    final double[] stationRangeRateBiasMax = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_MAX);
    final boolean[] stationRangeRateBiasEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_ESTIMATED);
    final double[] stationAzimuthSigma = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_SIGMA);
    final double[] stationAzimuthBias = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS);
    final double[] stationAzimuthBiasMin = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS_MIN);
    final double[] stationAzimuthBiasMax = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS_MAX);
    final double[] stationElevationSigma = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_SIGMA);
    final double[] stationElevationBias = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS);
    final double[] stationElevationBiasMin = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS_MIN);
    final double[] stationElevationBiasMax = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS_MAX);
    final boolean[] stationAzElBiasesEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_AZ_EL_BIASES_ESTIMATED);
    final boolean[] stationElevationRefraction = parser.getBooleanArray(ParameterKey.GROUND_STATION_ELEVATION_REFRACTION_CORRECTION);
    final boolean[] stationRangeTropospheric = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_TROPOSPHERIC_CORRECTION);
    for (int i = 0; i < stationNames.length; ++i) {
        // the station itself
        final GeodeticPoint position = new GeodeticPoint(stationLatitudes[i], stationLongitudes[i], stationAltitudes[i]);
        final TopocentricFrame topo = new TopocentricFrame(body, position, stationNames[i]);
        final GroundStation station = new GroundStation(topo);
        station.getEastOffsetDriver().setSelected(stationPositionEstimated[i]);
        station.getNorthOffsetDriver().setSelected(stationPositionEstimated[i]);
        station.getZenithOffsetDriver().setSelected(stationPositionEstimated[i]);
        // range
        final double rangeSigma = stationRangeSigma[i];
        final Bias<Range> rangeBias;
        if (FastMath.abs(stationRangeBias[i]) >= Precision.SAFE_MIN || stationRangeBiasEstimated[i]) {
            rangeBias = new Bias<Range>(new String[] { stationNames[i] + "/range bias" }, new double[] { stationRangeBias[i] }, new double[] { rangeSigma }, new double[] { stationRangeBiasMin[i] }, new double[] { stationRangeBiasMax[i] });
            rangeBias.getParametersDrivers().get(0).setSelected(stationRangeBiasEstimated[i]);
        } else {
            // bias fixed to zero, we don't need to create a modifier for this
            rangeBias = null;
        }
        // range rate
        final double rangeRateSigma = stationRangeRateSigma[i];
        final Bias<RangeRate> rangeRateBias;
        if (FastMath.abs(stationRangeRateBias[i]) >= Precision.SAFE_MIN || stationRangeRateBiasEstimated[i]) {
            rangeRateBias = new Bias<RangeRate>(new String[] { stationNames[i] + "/range rate bias" }, new double[] { stationRangeRateBias[i] }, new double[] { rangeRateSigma }, new double[] { stationRangeRateBiasMin[i] }, new double[] { stationRangeRateBiasMax[i] });
            rangeRateBias.getParametersDrivers().get(0).setSelected(stationRangeRateBiasEstimated[i]);
        } else {
            // bias fixed to zero, we don't need to create a modifier for this
            rangeRateBias = null;
        }
        // angular biases
        final double[] azELSigma = new double[] { stationAzimuthSigma[i], stationElevationSigma[i] };
        final Bias<AngularAzEl> azELBias;
        if (FastMath.abs(stationAzimuthBias[i]) >= Precision.SAFE_MIN || FastMath.abs(stationElevationBias[i]) >= Precision.SAFE_MIN || stationAzElBiasesEstimated[i]) {
            azELBias = new Bias<AngularAzEl>(new String[] { stationNames[i] + "/az bias", stationNames[i] + "/el bias" }, new double[] { stationAzimuthBias[i], stationElevationBias[i] }, azELSigma, new double[] { stationAzimuthBiasMin[i], stationElevationBiasMin[i] }, new double[] { stationAzimuthBiasMax[i], stationElevationBiasMax[i] });
            azELBias.getParametersDrivers().get(0).setSelected(stationAzElBiasesEstimated[i]);
            azELBias.getParametersDrivers().get(1).setSelected(stationAzElBiasesEstimated[i]);
        } else {
            // bias fixed to zero, we don't need to create a modifier for this
            azELBias = null;
        }
        // Refraction correction
        final AngularRadioRefractionModifier refractionCorrection;
        if (stationElevationRefraction[i]) {
            final double altitude = station.getBaseFrame().getPoint().getAltitude();
            final AtmosphericRefractionModel refractionModel = new EarthITU453AtmosphereRefraction(altitude);
            refractionCorrection = new AngularRadioRefractionModifier(refractionModel);
        } else {
            refractionCorrection = null;
        }
        // Tropospheric correction
        final RangeTroposphericDelayModifier rangeTroposphericCorrection;
        if (stationRangeTropospheric[i]) {
            final SaastamoinenModel troposphericModel = SaastamoinenModel.getStandardModel();
            rangeTroposphericCorrection = new RangeTroposphericDelayModifier(troposphericModel);
        } else {
            rangeTroposphericCorrection = null;
        }
        stations.put(stationNames[i], new StationData(station, rangeSigma, rangeBias, rangeRateSigma, rangeRateBias, azELSigma, azELBias, refractionCorrection, rangeTroposphericCorrection));
    }
    return stations;
}

Example 10 with RangeRate

use of org.orekit.estimation.measurements.RangeRate in project Orekit by CS-SI.

the class OrbitDeterminationTest method readMeasurements.

/**
 * Read a measurements file.
 * @param file measurements file
 * @param stations name to stations data map
 * @param pvData PV measurements data
 * @param satRangeBias range bias due to transponder delay
 * @param weights base weights for measurements
 * @param rangeOutliersManager manager for range measurements outliers (null if none configured)
 * @param rangeRateOutliersManager manager for range-rate measurements outliers (null if none configured)
 * @param azElOutliersManager manager for azimuth-elevation measurements outliers (null if none configured)
 * @param pvOutliersManager manager for PV measurements outliers (null if none configured)
 * @return measurements list
 */
private List<ObservedMeasurement<?>> readMeasurements(final File file, final Map<String, StationData> stations, final PVData pvData, final Bias<Range> satRangeBias, final Weights weights, final OutlierFilter<Range> rangeOutliersManager, final OutlierFilter<RangeRate> rangeRateOutliersManager, final OutlierFilter<AngularAzEl> azElOutliersManager, final OutlierFilter<PV> pvOutliersManager) throws UnsupportedEncodingException, IOException, OrekitException {
    final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
    BufferedReader br = null;
    try {
        br = new BufferedReader(new InputStreamReader(new FileInputStream(file), "UTF-8"));
        int lineNumber = 0;
        for (String line = br.readLine(); line != null; line = br.readLine()) {
            ++lineNumber;
            line = line.trim();
            if (line.length() > 0 && !line.startsWith("#")) {
                String[] fields = line.split("\\s+");
                if (fields.length < 2) {
                    throw new OrekitException(OrekitMessages.UNABLE_TO_PARSE_LINE_IN_FILE, lineNumber, file.getName(), line);
                }
                switch(fields[1]) {
                    case "RANGE":
                        final Range range = new RangeParser().parseFields(fields, stations, pvData, satRangeBias, weights, line, lineNumber, file.getName());
                        if (rangeOutliersManager != null) {
                            range.addModifier(rangeOutliersManager);
                        }
                        addIfNonZeroWeight(range, measurements);
                        break;
                    case "RANGE_RATE":
                        final RangeRate rangeRate = new RangeRateParser().parseFields(fields, stations, pvData, satRangeBias, weights, line, lineNumber, file.getName());
                        if (rangeOutliersManager != null) {
                            rangeRate.addModifier(rangeRateOutliersManager);
                        }
                        addIfNonZeroWeight(rangeRate, measurements);
                        break;
                    case "AZ_EL":
                        final AngularAzEl angular = new AzElParser().parseFields(fields, stations, pvData, satRangeBias, weights, line, lineNumber, file.getName());
                        if (azElOutliersManager != null) {
                            angular.addModifier(azElOutliersManager);
                        }
                        addIfNonZeroWeight(angular, measurements);
                        break;
                    case "PV":
                        final PV pv = new PVParser().parseFields(fields, stations, pvData, satRangeBias, weights, line, lineNumber, file.getName());
                        if (pvOutliersManager != null) {
                            pv.addModifier(pvOutliersManager);
                        }
                        addIfNonZeroWeight(pv, measurements);
                        break;
                    default:
                        throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE, "unknown measurement type " + fields[1] + " at line " + lineNumber + " in file " + file.getName());
                }
            }
        }
    } finally {
        if (br != null) {
            br.close();
        }
    }
    if (measurements.isEmpty()) {
        throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE, "not measurements read from file " + file.getAbsolutePath());
    }
    return measurements;
}