1 /* Copyright 2002-2026 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
18 package org.orekit.estimation.measurements.filtering;
19
20 import java.util.ArrayList;
21
22 import org.hipparchus.util.FastMath;
23 import org.orekit.files.rinex.observation.ObservationData;
24 import org.orekit.utils.Constants;
25
26 /**
27 * Hatch Filter using Carrier-Phase measurements taken at two different frequencies,
28 * to form a Divergence-Free phase combination.
29 * <p>
30 * This filter uses a phase combination to mitigate the effects of the
31 * temporally varying ionospheric delays. Still, the spatial variation of the ionospheric delays
32 * are not compensated by this phase combination.
33 * </p>
34 * @see "Subirana, J. S., Hernandez-Pajares, M., and José Miguel Juan Zornoza. (2013).
35 * GNSS Data Processing: Fundamentals and Algorithms. European Space Agency.
36 * Section 4.2.3.1.1"
37 *
38 * @author Louis Aucouturier
39 * @since 11.2
40 */
41 public class DualFrequencyHatchFilter extends HatchFilter {
42
43 /** First wavelength used for smoothing. */
44 private final double wavelengthFreq1;
45
46 /** Second wavelength used for smoothing. */
47 private final double wavelengthFreq2;
48
49 /** List used to store the phase value of the first frequency. */
50 private final ArrayList<Double> phase1History;
51
52 /** List used to store the phase value of the second frequency.*/
53 private final ArrayList<Double> phase2History;
54
55 /**
56 * Constructor for the Dual Frequency Hatch Filter.
57 * <p>
58 * The threshold parameter corresponds to the maximum difference between
59 * non-smoothed and smoothed pseudo range value, above which the filter
60 * is reset.
61 * </p>
62 * @param initCode initial code measurement
63 * @param initPhaseFreq1 initial phase measurement for the first chosen frequency
64 * @param initPhaseFreq2 initial phase measurement for the second chosen frequency
65 * @param wavelengthFreq1 initPhaseFreq1 observed value wavelength (m)
66 * @param wavelengthFreq2 initPhaseFreq2 observed value wavelength (m)
67 * @param threshold threshold for loss of lock detection
68 * (it represents the maximum difference between smoothed
69 * and measured values for loss of lock detection)
70 * @param N window size of the Hatch Filter
71 */
72 public DualFrequencyHatchFilter(final ObservationData initCode,
73 final ObservationData initPhaseFreq1, final ObservationData initPhaseFreq2,
74 final double wavelengthFreq1, final double wavelengthFreq2,
75 final double threshold, final int N) {
76 super(threshold, N);
77 // Initialize wavelength and compute frequencies
78 this.wavelengthFreq1 = wavelengthFreq1;
79 this.wavelengthFreq2 = wavelengthFreq2;
80
81 // Initialize array of phase values used during smoothing
82 this.phase1History = new ArrayList<>();
83 this.phase2History = new ArrayList<>();
84 phase1History.add(initPhaseFreq1.getValue() * wavelengthFreq1);
85 phase2History.add(initPhaseFreq2.getValue() * wavelengthFreq2);
86 updatePreviousSmoothedCode(initCode.getValue());
87 updatePreviousSmoothingValue(divergenceFreeCombination(initPhaseFreq1.getValue(), initPhaseFreq2.getValue(), wavelengthFreq1, wavelengthFreq2));
88 addToSmoothedCodeHistory(initCode.getValue());
89 addToCodeHistory(initCode.getValue());
90 }
91
92 /**
93 * This method filters the provided data given the state of the filter.
94 * @param codeData input code observation data
95 * @param phaseDataFreq1 input phase observation data for the first frequency
96 * @param phaseDataFreq2 input phase observation data for the second frequency
97 * @return the smoothed observation data
98 */
99 public ObservationData filterData(final ObservationData codeData, final ObservationData phaseDataFreq1, final ObservationData phaseDataFreq2) {
100
101 // Current code value
102 final double code = codeData.getValue();
103 addToCodeHistory(code);
104
105 // Computes the phase combination and smoothing value (Ref Eq. 4.32)
106 final double phaseFreq1 = wavelengthFreq1 * phaseDataFreq1.getValue();
107 final double phaseFreq2 = wavelengthFreq2 * phaseDataFreq2.getValue();
108 final double phaseDF = divergenceFreeCombination(phaseDataFreq1.getValue(), phaseDataFreq2.getValue(), wavelengthFreq1, wavelengthFreq2);
109 phase1History.add(phaseFreq1);
110 phase2History.add(phaseFreq2);
111
112 // Check for carrier phase cycle slip (check on the two phase data)
113 final boolean cycleSlip = FastMath.floorMod(phaseDataFreq1.getLossOfLockIndicator(), 2) != 0 ||
114 FastMath.floorMod(phaseDataFreq2.getLossOfLockIndicator(), 2) != 0;
115
116 // Computes the smoothed code value
117 double smoothedValue = smoothedCode(code, phaseDF);
118 updatePreviousSmoothingValue(phaseDF);
119
120 // Check if filter reset needed, if not return smoothedValue, and increase k if necessary.
121 smoothedValue = checkValidData(code, smoothedValue, cycleSlip);
122 addToSmoothedCodeHistory(smoothedValue);
123 updatePreviousSmoothedCode(smoothedValue);
124
125 // Return the smoothed observed data
126 return new ObservationData(codeData.getObservationType(), smoothedValue,
127 codeData.getLossOfLockIndicator(), codeData.getSignalStrength());
128
129 }
130
131 /**
132 * Get the history of phase values of the first frequency.
133 * @return the history of phase values of the first frequency
134 */
135 public ArrayList<Double> getFirstFrequencyPhaseHistory() {
136 return phase1History;
137 }
138
139 /**
140 * Get the history of phase values of the second frequency.
141 * @return the history of phase values of the second frequency
142 */
143 public ArrayList<Double> getSecondFrequencyPhaseHistory() {
144 return phase2History;
145 }
146
147 /**
148 * Divergence-free combination (Ref Eq. 4.32).
149 * <p>
150 * phase_DF = phase_F1 + 2.0 * alpha + (phase_F1 - phase_F2)
151 * </p>
152 * @param phase1 phase value for frequency 1
153 * @param phase2 phase value for frequency 2
154 * @param lambda1 wavelength of the first phase (m)
155 * @param lambda2 wavelength of the second phase (m)
156 * @return the value of the divergence-free combination
157 */
158 private static double divergenceFreeCombination(final double phase1, final double phase2,
159 final double lambda1, final double lambda2) {
160
161 // Multiply phase value by its wavelength
162 final double phaseFreq1 = lambda1 * phase1;
163 final double phaseFreq2 = lambda2 * phase2;
164
165 // Convert wavelength to frequencies
166 final double f1 = Constants.SPEED_OF_LIGHT / lambda1;
167 final double f2 = Constants.SPEED_OF_LIGHT / lambda2;
168
169 // Alpha
170 final double alpha = 1.0 / ((f1 * f1) / (f2 * f2) - 1.0);
171
172 // Return
173 return phaseFreq1 + 2.0 * alpha * (phaseFreq1 - phaseFreq2);
174
175 }
176
177 }