PhaseMinusCodeCycleSlipDetector.java

  1. /* Copyright 2002-2024 CS GROUP
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.estimation.measurements.gnss;

  18. import java.util.ArrayList;
  19. import java.util.List;
  20. import java.util.Map;

  21. import org.hipparchus.fitting.PolynomialCurveFitter;
  22. import org.hipparchus.fitting.WeightedObservedPoint;
  23. import org.hipparchus.util.FastMath;
  24. import org.orekit.files.rinex.observation.ObservationData;
  25. import org.orekit.files.rinex.observation.ObservationDataSet;
  26. import org.orekit.gnss.Frequency;
  27. import org.orekit.gnss.MeasurementType;
  28. import org.orekit.gnss.SatelliteSystem;
  29. import org.orekit.time.AbsoluteDate;
  30. import org.orekit.utils.Constants;

  31. /**
  32.  * Phase minus code cycle slip detectors.
  33.  * The detector is based the algorithm given in <a
  34.  * href="https://gssc.esa.int/navipedia/index.php/Examples_of_single_frequency_Cycle-Slip_Detectors">
  35.  * Examples of single frequency Cycle-Slip Detectors</a> by Zornoza and M. Hernández-Pajares. Within this class
  36.  * a polynomial is used to smooth the data. We consider a cycle_slip occurring if the current measurement is  too
  37.  * far from the one predicted with the polynomial (algorithm 1 on Navipedia).
  38.  * <p>
  39.  * For building the detector, one should give a threshold and a gap time limit.
  40.  * After construction of the detectors, one can have access to a List of CycleData. Each CycleDate represents
  41.  * a link between the station (define by the RINEX file) and a satellite at a specific frequency. For each cycle data,
  42.  * one has access to the begin and end of availability, and a sorted set which contains all the date at which
  43.  * cycle-slip have been detected
  44.  * </p>
  45.  * @author David Soulard
  46.  * @since 10.2
  47.  */
  48. public class PhaseMinusCodeCycleSlipDetector extends AbstractCycleSlipDetector {

  49.     /** Mega Hertz to Hertz conversion factor. */
  50.     private static final double MHZ_TO_HZ = 1.0e6;

  51.     /** Order of the polynomial used for fitting. */
  52.     private final int order;

  53.     /** Threshold above which cycle-slip occurs. */
  54.     private final double threshold;

  55.     /** Polynomial single frequency cycle-slip detector Constructor.
  56.      * @param dt time gap threshold between two consecutive measurement (if time between two consecutive measurement is greater than dt, a cycle slip is declared)
  57.      * @param threshold threshold above which cycle-slip occurs
  58.      * @param n number of measurement before starting
  59.      * @param order polynomial order
  60.      */
  61.     public PhaseMinusCodeCycleSlipDetector(final double dt, final double threshold,
  62.                                            final int n, final int order) {
  63.         super(dt, n);
  64.         this.threshold = threshold;
  65.         this.order     = order;
  66.     }

  67.     /** {@inheritDoc} */
  68.     @Override
  69.     protected void manageData(final ObservationDataSet observation) {

  70.         // Extract observation data
  71.         final SatelliteSystem system = observation.getSatellite().getSystem();
  72.         final int             prn    = observation.getSatellite().getPRN();
  73.         final AbsoluteDate    date   = observation.getDate();

  74.         // Initialize list of measurements
  75.         final List<ObservationData> pseudoRanges = new ArrayList<>();
  76.         final List<ObservationData> phases       = new ArrayList<>();

  77.         // Loop on observation data to fill lists
  78.         for (final ObservationData od : observation.getObservationData()) {
  79.             if (!Double.isNaN(od.getValue())) {
  80.                 if (od.getObservationType().getMeasurementType() == MeasurementType.PSEUDO_RANGE) {
  81.                     pseudoRanges.add(od);
  82.                 } else if (od.getObservationType().getMeasurementType() == MeasurementType.CARRIER_PHASE) {
  83.                     phases.add(od);
  84.                 }
  85.             }
  86.         }

  87.         // Loop on phase measurements
  88.         for (final ObservationData phase : phases) {
  89.             // Loop on range measurement
  90.             for (final ObservationData pseudoRange : pseudoRanges) {
  91.                 // Change unit of phase measurement
  92.                 final double frequency = phase.getObservationType().getFrequency(system).getMHzFrequency() * MHZ_TO_HZ;
  93.                 final double cOverF    = Constants.SPEED_OF_LIGHT / frequency;
  94.                 final ObservationData phaseInMeters = new ObservationData(phase.getObservationType(),
  95.                                                                           cOverF * phase.getValue(),
  96.                                                                           phase.getLossOfLockIndicator(),
  97.                                                                           phase.getSignalStrength());

  98.                 // Check if measurement frequencies are the same
  99.                 if (phase.getObservationType().getFrequency(system) == pseudoRange.getObservationType().getFrequency(system)) {
  100.                     // Phase minus Code combination
  101.                     final PhaseMinusCodeCombination phaseMinusCode = MeasurementCombinationFactory.getPhaseMinusCodeCombination(system);
  102.                     final CombinedObservationData cod = phaseMinusCode.combine(phaseInMeters, pseudoRange);
  103.                     final String nameSat = setName(prn, observation.getSatellite().getSystem());

  104.                     // Check for cycle-slip detection
  105.                     final boolean slip = cycleSlipDetection(nameSat, date, cod.getValue(), phase.getObservationType().getFrequency(system));
  106.                     if (!slip) {
  107.                         // Update cycle slip data
  108.                         cycleSlipDataSet(nameSat, date, cod.getValue(), phase.getObservationType().getFrequency(system));
  109.                     }
  110.                 }
  111.             }
  112.         }

  113.     }

  114.     /**
  115.      * Compute if there is a cycle slip at a specific date.
  116.      * @param nameSat name of the satellite, on the predefined format (e.g. GPS - 07 for satellite 7 of GPS constellation)
  117.      * @param currentDate the date at which we check if a cycle-slip occurs
  118.      * @param phaseMinusCode phase measurement minus code measurement
  119.      * @param frequency frequency used
  120.      * @return true if a cycle slip has been detected.
  121.      */
  122.     private boolean cycleSlipDetection(final String nameSat, final AbsoluteDate currentDate,
  123.                                        final double phaseMinusCode, final Frequency frequency) {

  124.         // Access the cycle slip results to know if a cycle-slip already occurred
  125.         final List<CycleSlipDetectorResults>         data  = getResults();
  126.         final List<Map<Frequency, DataForDetection>> stuff = getStuffReference();

  127.         // If a cycle-slip already occurred
  128.         if (data != null) {

  129.             // Loop on cycle-slip results
  130.             for (CycleSlipDetectorResults resultPmC : data) {

  131.                 // Found the right cycle data
  132.                 if (resultPmC.getSatelliteName().compareTo(nameSat) == 0 && resultPmC.getCycleSlipMap().containsKey(frequency)) {
  133.                     final Map<Frequency, DataForDetection> values = stuff.get(data.indexOf(resultPmC));
  134.                     final DataForDetection v = values.get(frequency);

  135.                     // Check the time gap condition
  136.                     if (FastMath.abs(currentDate.durationFrom(v.getFiguresReference()[v.getWrite()].getDate())) > getMaxTimeBeetween2Measurement()) {
  137.                         resultPmC.addCycleSlipDate(frequency, currentDate);
  138.                         v.resetFigures( new SlipComputationData[getMinMeasurementNumber()], phaseMinusCode, currentDate);
  139.                         resultPmC.setDate(frequency, currentDate);
  140.                         return true;
  141.                     }

  142.                     // Compute the fitting polynomial if there are enough measurement since last cycle-slip
  143.                     if (v.getCanBeComputed() >= getMinMeasurementNumber()) {
  144.                         final List<WeightedObservedPoint> xy = new ArrayList<>();
  145.                         for (int i = 0; i < getMinMeasurementNumber(); i++) {
  146.                             final SlipComputationData current = v.getFiguresReference()[i];
  147.                             xy.add(new WeightedObservedPoint(1.0, current.getDate().durationFrom(currentDate),
  148.                                                                  current.getValue()));
  149.                         }

  150.                         final PolynomialCurveFitter fitting = PolynomialCurveFitter.create(order);
  151.                         // Check if there is a cycle_slip
  152.                         if (FastMath.abs(fitting.fit(xy)[0] - phaseMinusCode) > threshold) {
  153.                             resultPmC.addCycleSlipDate(frequency, currentDate);
  154.                             v.resetFigures( new SlipComputationData[getMinMeasurementNumber()], phaseMinusCode, currentDate);
  155.                             resultPmC.setDate(frequency, currentDate);
  156.                             return true;
  157.                         }

  158.                     } else {
  159.                         break;
  160.                     }

  161.                 }

  162.             }

  163.         }

  164.         // No cycle-slip
  165.         return false;
  166.     }

  167. }