1 /* Copyright 2002-2021 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
19 import java.util.ArrayList;
20 import java.util.List;
21 import java.util.Map;
22
23 import org.hipparchus.fitting.PolynomialCurveFitter;
24 import org.hipparchus.fitting.WeightedObservedPoint;
25 import org.hipparchus.util.FastMath;
26 import org.orekit.gnss.CombinedObservationData;
27 import org.orekit.gnss.Frequency;
28 import org.orekit.gnss.MeasurementType;
29 import org.orekit.gnss.ObservationData;
30 import org.orekit.gnss.ObservationDataSet;
31 import org.orekit.gnss.SatelliteSystem;
32 import org.orekit.time.AbsoluteDate;
33 import org.orekit.utils.Constants;
34
35 /**
36 * Phase minus code cycle slip detectors.
37 * The detector is based the algorithm given in <a
38 * href="https://gssc.esa.int/navipedia/index.php/Examples_of_single_frequency_Cycle-Slip_Detectors">
39 * Examples of single frequency Cycle-Slip Detectors</a> by Zornoza and M. Hernández-Pajares. Within this class
40 * a polynomial is used to smooth the data. We consider a cycle_slip occurring if the current measurement is too
41 * far from the one predicted with the polynomial (algorithm 1 on Navipedia).
42 * <p>
43 * For building the detector, one should give a threshold and a gap time limit.
44 * After construction of the detectors, one can have access to a List of CycleData. Each CycleDate represents
45 * a link between the station (define by the RINEX file) and a satellite at a specific frequency. For each cycle data,
46 * one has access to the begin and end of availability, and a sorted set which contains all the date at which
47 * cycle-slip have been detected
48 * </p>
49 * <p>
50 * @author David Soulard
51 * @since 10.2
52 */
53 public class PhaseMinusCodeCycleSlipDetector extends AbstractCycleSlipDetector {
54
55 /** Mega Hertz to Hertz conversion factor. */
56 private static final double MHZ_TO_HZ = 1.0e6;
57
58 /** Order of the polynomial used for fitting. */
59 private final int order;
60
61 /** Threshold above which cycle-slip occurs. */
62 private final double threshold;
63
64 /** Polynomial single frequency cycle-slip detector Constructor.
65 * @param dt time gap threshold between two consecutive measurement (if time between two consecutive measurement is greater than dt, a cycle slip is declared)
66 * @param threshold threshold above which cycle-slip occurs
67 * @param n number of measurement before starting
68 * @param order polynomial order
69 */
70 public PhaseMinusCodeCycleSlipDetector(final double dt, final double threshold,
71 final int n, final int order) {
72 super(dt, n);
73 this.threshold = threshold;
74 this.order = order;
75 }
76
77 /** {@inheritDoc} */
78 @Override
79 protected void manageData(final ObservationDataSet observation) {
80
81 // Extract observation data
82 final SatelliteSystem system = observation.getSatelliteSystem();
83 final int prn = observation.getPrnNumber();
84 final AbsoluteDate date = observation.getDate();
85
86 // Initialize list of measurements
87 final List<ObservationData> pseudoRanges = new ArrayList<>();
88 final List<ObservationData> phases = new ArrayList<>();
89
90 // Loop on observation data to fill lists
91 for (final ObservationData od : observation.getObservationData()) {
92 if (!Double.isNaN(od.getValue())) {
93 if (od.getObservationType().getMeasurementType() == MeasurementType.PSEUDO_RANGE) {
94 pseudoRanges.add(od);
95 } else if (od.getObservationType().getMeasurementType() == MeasurementType.CARRIER_PHASE) {
96 phases.add(od);
97 }
98 }
99 }
100
101 // Loop on phase measurements
102 for (final ObservationData phase : phases) {
103 // Loop on range measurement
104 for (final ObservationData pseudoRange : pseudoRanges) {
105 // Change unit of phase measurement
106 final double frequency = phase.getObservationType().getFrequency(system).getMHzFrequency() * MHZ_TO_HZ;
107 final double cOverF = Constants.SPEED_OF_LIGHT / frequency;
108 final ObservationData phaseInMeters = new ObservationData(phase.getObservationType(),
109 cOverF * phase.getValue(),
110 phase.getLossOfLockIndicator(),
111 phase.getSignalStrength());
112
113 // Check if measurement frequencies are the same
114 if (phase.getObservationType().getFrequency(system) == pseudoRange.getObservationType().getFrequency(system)) {
115 // Phase minus Code combination
116 final PhaseMinusCodeCombination phaseMinusCode = MeasurementCombinationFactory.getPhaseMinusCodeCombination(system);
117 final CombinedObservationData cod = phaseMinusCode.combine(phaseInMeters, pseudoRange);
118 final String nameSat = setName(prn, observation.getSatelliteSystem());
119
120 // Check for cycle-slip detection
121 final boolean slip = cycleSlipDetection(nameSat, date, cod.getValue(), phase.getObservationType().getFrequency(system));
122 if (!slip) {
123 // Update cycle slip data
124 cycleSlipDataSet(nameSat, date, cod.getValue(), phase.getObservationType().getFrequency(system));
125 }
126 }
127 }
128 }
129
130 }
131
132 /**
133 * Compute if there is a cycle slip at a specific date.
134 * @param nameSat name of the satellite, on the predefined format (e.g. GPS - 07 for satellite 7 of GPS constellation)
135 * @param currentDate the date at which we check if a cycle-slip occurs
136 * @param phaseMinusCode phase measurement minus code measurement
137 * @param frequency frequency used
138 * @return true if a cycle slip has been detected.
139 */
140 private boolean cycleSlipDetection(final String nameSat, final AbsoluteDate currentDate,
141 final double phaseMinusCode, final Frequency frequency) {
142
143 // Access the cycle slip results to know if a cycle-slip already occurred
144 final List<CycleSlipDetectorResults> data = getResults();
145 final List<Map<Frequency, DataForDetection>> stuff = getStuffReference();
146
147 // If a cycle-slip already occurred
148 if (data != null) {
149
150 // Loop on cycle-slip results
151 for (CycleSlipDetectorResults resultPmC : data) {
152
153 // Found the right cycle data
154 if (resultPmC.getSatelliteName().compareTo(nameSat) == 0 && resultPmC.getCycleSlipMap().containsKey(frequency)) {
155 final Map<Frequency, DataForDetection> values = stuff.get(data.indexOf(resultPmC));
156 final DataForDetection v = values.get(frequency);
157
158 // Check the time gap condition
159 if (FastMath.abs(currentDate.durationFrom(v.getFiguresReference()[v.getWrite()].getDate())) > getMaxTimeBeetween2Measurement()) {
160 resultPmC.addCycleSlipDate(frequency, currentDate);
161 v.resetFigures( new SlipComputationData[getMinMeasurementNumber()], phaseMinusCode, currentDate);
162 resultPmC.setDate(frequency, currentDate);
163 return true;
164 }
165
166 // Compute the fitting polynomial if there are enough measurement since last cycle-slip
167 if (v.getCanBeComputed() >= getMinMeasurementNumber()) {
168 final List<WeightedObservedPoint> xy = new ArrayList<>();
169 for (int i = 0; i < getMinMeasurementNumber(); i++) {
170 final SlipComputationData current = v.getFiguresReference()[i];
171 xy.add(new WeightedObservedPoint(1.0, current.getDate().durationFrom(currentDate),
172 current.getValue()));
173 }
174
175 final PolynomialCurveFitter fitting = PolynomialCurveFitter.create(order);
176 // Check if there is a cycle_slip
177 if (FastMath.abs(fitting.fit(xy)[0] - phaseMinusCode) > threshold) {
178 resultPmC.addCycleSlipDate(frequency, currentDate);
179 v.resetFigures( new SlipComputationData[getMinMeasurementNumber()], phaseMinusCode, currentDate);
180 resultPmC.setDate(frequency, currentDate);
181 return true;
182 }
183
184 } else {
185 break;
186 }
187
188 }
189
190 }
191
192 }
193
194 // No cycle-slip
195 return false;
196 }
197
198 }