Commit 26502763 authored by Bryan Cazabonne's avatar Bryan Cazabonne
Browse files

Improved code for Hatch filters implementations.

Fixes #666
parent 1b243902
......@@ -21,6 +21,9 @@
</properties>
<body>
<release version="11.2" date="TBD" description="TBD">
<action dev="louis" type="add" issue="666">
Added Hatch filters for smoothing of GNSS measurements.
</action>
<action dev="bryan" type="update" issue="895">
Allowed parsing of SP3 files without EOF key.
</action>
......
/* Copyright 2002-2022 CS GROUP
* Licensed to CS GROUP (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.estimation.measurements.filtering;
import java.util.ArrayList;
import org.hipparchus.util.FastMath;
import org.orekit.gnss.ObservationData;
/** Abstract class for Hatch filters
*
* <p>
* Hatch filters are used to smooth the pseudo-range measurements with a set of measurements, that might be combined or not, in order to mitigate the effect of multipath.
* The original Hatch filter uses a single carrier-phase measurement as a smoothing value, while a divergence free combination of carrier phases can be used, as well as Doppler measurements.
* </p>
*
* @author Louis Aucouturier
*
*/
public class AbstractHatchFilter {
/** Current index for the filter. */
private int k;
/** Window size of the Hatch Filter. */
private int N;
/** Boolean used to delay the reset of the filter.*/
private boolean resetNextBoolean;
/** Last smoothed value for the pseudo-range.
* Stored as needed by the Hatch filter. */
private double oldSmoothedCode;
/** History of the pseudo-range value, appended at each filter iteration.*/
private ArrayList<Double> codeHistory;
/** History of the smoothed pseudo-range value, appended at each filter iteration.*/
private ArrayList<Double> smoothedCodeHistory;
/** Threshold for the difference between smoothed and measured values.*/
private double threshold;
/**
* Constructor for the Abstract Hatch Filter.
* Initialize the variables and set the initial pseudo-range state.
*
* @param oldCode
*/
AbstractHatchFilter(final double oldCode) {
this.oldSmoothedCode = oldCode;
this.codeHistory = new ArrayList<Double>();
this.smoothedCodeHistory = new ArrayList<Double>();
this.smoothedCodeHistory.add(oldCode);
this.codeHistory.add(oldCode);
this.threshold = 100;
this.resetNextBoolean = false;
}
/**
* Constructor for the Abstract Hatch Filter.
* Initialize the variables and set the initial pseudo-range state.
*
* @param oldCode
* @param threshold
*/
AbstractHatchFilter(final double oldCode, final double threshold) {
this.oldSmoothedCode = oldCode;
this.codeHistory = new ArrayList<Double>();
this.smoothedCodeHistory = new ArrayList<Double>();
this.smoothedCodeHistory.add(oldCode);
this.codeHistory.add(oldCode);
this.threshold = threshold;
this.resetNextBoolean = false;
}
/**
* @return the codeHistory
*/
public final ArrayList<Double> getCodeHistory() {
return codeHistory;
}
/**
* @return the smoothedCodeHistory
*/
public final ArrayList<Double> getSmoothedCodeHistory() {
return smoothedCodeHistory;
}
/**
* @param threshold the threshold to set
*/
public final void setThreshold(final double threshold) {
this.threshold = threshold;
}
/**
* @return the threshold
*/
public final double getThreshold() {
return threshold;
}
/**
* @param resetNextBoolean the resetNextBoolean to set
*/
public final void setResetNextBoolean(final boolean resetNextBoolean) {
this.resetNextBoolean = resetNextBoolean;
}
/**
* @return the n
*/
public final int getN() {
return N;
}
/**
* @param k the k to set
*/
public final void setK(final int k) {
this.k = k;
}
/**
* @param n the n to set
*/
public final void setN(final int n) {
N = n;
}
/**
* @param oldSmoothedCode the oldSmoothedCode to set
*/
final void setOldSmoothedCode(final double oldSmoothedCode) {
this.oldSmoothedCode = oldSmoothedCode;
}
/**
* Add a value to the code history DescriptiveStatistics object.
* @param codeValue
*/
final void addToCodeHistory(final double codeValue) {
codeHistory.add(codeValue);
}
/**
* Add a value to the smoothed code history DescriptiveStatistics object.
* @param smoothedCodeValue
*/
final void addToSmoothedCodeHistory(final double smoothedCodeValue) {
smoothedCodeHistory.add(smoothedCodeValue);
}
/**
* Smoothing computation, to be moved to specific classes.
*
* @param codeValue
* @param smoothingValue
* @return smoothed Value
*/
protected double filterComputation(final double codeValue, final double smoothingValue) {
final double alpha = k - 1;
final double filteredRange = alpha / k * (oldSmoothedCode + smoothingValue) + codeValue / k;
return filteredRange;
}
protected double checkValidData(final double codeValue, final double smoothedValue, final boolean checkLLI) {
double validValue = smoothedValue;
// Check if need to reset the filter or if Loss of lock
// then set smoothedValue as the non smoothed value, and reset the counter to 1.
// Else use smoothed value, and increase counter.
if (resetNextBoolean || !checkLLI || FastMath.abs(smoothedValue - codeValue) > threshold) {
setK(1);
validValue = codeValue;
} else {
final int tempK = (k > getN()) ? getN() : k + 1;
setK(tempK);
}
return validValue;
}
/**
* Copy and modify an existing ObservationData object,
* namely the code data in the hatch filter case.
*
* @param codeData
* @param newValue
* @return codeData, with smoothed code value.
*/
protected ObservationData modifyObservationData(final ObservationData codeData, final double newValue) {
return new ObservationData(codeData.getObservationType(),
newValue,
codeData.getLossOfLockIndicator(),
codeData.getSignalStrength());
}
}
/* Copyright 2002-2022 CS GROUP
* Licensed to CS GROUP (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.estimation.measurements.filtering;
import java.util.ArrayList;
import org.hipparchus.util.FastMath;
import org.orekit.gnss.ObservationData;
import org.orekit.gnss.ObservationType;
import org.orekit.gnss.SatelliteSystem;
/**Hatch Filter using Carrier-Phase measurements taken at 2 different frequencies,
* to form a Divergence-Free phase combination.
*
* <p>
* This filter uses a phase combination to mitigate the effects of the
* temporally varying ionospheric delays. Still, the spatial variation of the ionospheric delays
* are not compensated by this phase combination.
* The results of this filter were checked against those provided by J.Sanz Subirana in
* GNSS Data Processing Vol I and II, through provided code included in the tests.
* </p>
*
* Based on GNSS Data Processing Vol I and II by J.Sanz Subirana, J.M. Juan Zornoza and M. Hernández-Pajares, Section 4.2.3.1.1
*
* @author Louis Aucouturier
*
*/
public class CarrierPhaseHatchFilterDualFrequency extends AbstractHatchFilter {
/** Dual frequency phase combination used at the previous filter iteration.
* Used in the phase difference of the Divergence-Free Hatch Filter*/
private double oldPhaseDF;
/** Wavelength for the first carrier phase object.
* Linked to the observation type. */
private double wavelengthF1;
/** Wavelength for the second carrier phase object.
* Linked to the observation type. */
private double wavelengthF2;
/** Constant defined as 1/(gamma12 - 1), with gamma12 the squared ratio of frequencies.*/
private double alpha1tilde;
/** ObservationType for the first phase input.*/
private ObservationType obsTypePhaseF1;
/** ObservationType for the second phase input.*/
private ObservationType obsTypePhaseF2;
/** List used to store the phase value of the first frequency. */
private ArrayList<Double> phase1History;
/** List used to store the phase value of the second frequency.*/
private ArrayList<Double> phase2History;
/**
* Constructor for the class.
*
* Computes constant parameters and set the initial state of the filter.
* The threshold parameter corresponds to the maximum difference between
* non-smoothed and smoothed pseudo range value, above which the filter
* is reset.
*
* @param codeData : initial Pseudo Range observation data
* @param phaseDataFreq1 : CarrierPhase ObservationData for the first chosen observationType.
* @param phaseDataFreq2 : CarrierPhase ObservationData for the first chosen observationType.
* @param satSystem :SatelliteSystem used
* @param N : Maximum window size
* @param threshold : Threshold value
*/
public CarrierPhaseHatchFilterDualFrequency(final ObservationData codeData,
final ObservationData phaseDataFreq1,
final ObservationData phaseDataFreq2,
final SatelliteSystem satSystem,
final int N,
final double threshold) {
// Initialize Abstract class
super(codeData.getValue(), threshold);
this.wavelengthF1 = phaseDataFreq1.getObservationType().getFrequency(satSystem).getWavelength();
this.wavelengthF2 = phaseDataFreq2.getObservationType().getFrequency(satSystem).getWavelength();
this.obsTypePhaseF1 = phaseDataFreq1.getObservationType();
this.obsTypePhaseF2 = phaseDataFreq2.getObservationType();
final double f1 = phaseDataFreq1.getObservationType().getFrequency(satSystem).getMHzFrequency();
final double f2 = phaseDataFreq2.getObservationType().getFrequency(satSystem).getMHzFrequency();
this.alpha1tilde = 1 / ( (f1 * f1) / (f2 * f2) - 1 );
this.oldPhaseDF = phaseDataFreq1.getValue() * wavelengthF1 + 2 * alpha1tilde *
(phaseDataFreq1.getValue() * wavelengthF1 - phaseDataFreq2.getValue() * wavelengthF2);
this.phase1History = new ArrayList<>();
this.phase2History = new ArrayList<>();
phase1History.add(phaseDataFreq1.getValue() * wavelengthF1);
phase2History.add(phaseDataFreq2.getValue() * wavelengthF2);
setN(N);
setK(1);
}
/** Reset the filter in the case of a NaN phase value, skipping the smoothing at the present instant
* and initializing at the next one, keeping the current code value.
*
* @param codeValue : pseudo range value before the reset.
* */
public void resetFilterNext(final double codeValue) {
setK(1);
setResetNextBoolean(true);
oldPhaseDF = Double.NaN;
setOldSmoothedCode(codeValue);
}
/** Filters the provided data given the state of the filter.
* Uses the Hatch Filter with the Divergence-free phase combination.
*
* @param codeData : Pseudo Range observation data
* @param phaseDataFreq1 : CarrierPhase ObservationData for the first chosen observationType.
* @param phaseDataFreq2 : CarrierPhase ObservationData for the second chosen observationType.
* @return modified ObservationData : PseudoRange observationData with a smoothed value.
* */
public ObservationData filterData(final ObservationData codeData, final ObservationData phaseDataFreq1, final ObservationData phaseDataFreq2) {
final boolean checkLLI = FastMath.floorMod(phaseDataFreq1.getLossOfLockIndicator(), 2) == 0 || FastMath.floorMod(phaseDataFreq2.getLossOfLockIndicator(), 2) == 0;
phase1History.add(phaseDataFreq1.getValue() * wavelengthF1);
phase2History.add(phaseDataFreq2.getValue() * wavelengthF2);
// Computes the phase combination and smoothing value.
final double phaseDF = phaseDataFreq1.getValue() * wavelengthF1 + 2 * alpha1tilde *
(phaseDataFreq1.getValue() * wavelengthF1 - phaseDataFreq2.getValue() * wavelengthF2);
final double smoothingValue = phaseDF - oldPhaseDF;
final double smoothedValue = filterComputation(codeData.getValue(), smoothingValue);
// Check if filter reset needed, if not return smoothedValue, and increase k if necessary.
final double newValue = checkValidData(codeData.getValue(), smoothedValue, checkLLI);
addToCodeHistory(codeData.getValue());
addToSmoothedCodeHistory(newValue);
// Modify the value stored in the abstract class.
setOldSmoothedCode(newValue);
// Set phase as oldPhase for next step.
oldPhaseDF = phaseDF;
return modifyObservationData(codeData, newValue);
}
/** Getter for the first carrier-phase observationType.
*
* @return the obsTypePhaseF1
*/
public ObservationType getObsTypePhaseF1() {
return obsTypePhaseF1;
}
/** Getter for the second carrier-phase observationType.
*
* @return the obsTypePhaseF2
*/
public ObservationType getObsTypePhaseF2() {
return obsTypePhaseF2;
}
/** Getter for the phase1History list, that stores the phase values corresponding to
* the first frequency carrier-phase observationType.
* This list stores double values.
*
* @return the phase1History
*/
public final ArrayList<Double> getPhase1History() {
return phase1History;
}
/** Getter for the phase2History list, that stores the phase values corresponding to
* the second frequency carrier-phase observationType.
* This list stores double values.
*
* @return the phase2History
*/
public final ArrayList<Double> getPhase2History() {
return phase2History;
}
}
/* Copyright 2002-2022 CS GROUP
* Licensed to CS GROUP (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.estimation.measurements.filtering;
import org.hipparchus.util.FastMath;
import org.orekit.gnss.ObservationData;
import org.orekit.gnss.SatelliteSystem;
/**Hatch Filter using Carrier-Phase measurements taken at a single frequency.
*
* <p>
* This filter uses a carrier phase taken at a single frequency, in order to smooth the pseudo-range
* measurements. This method has the advantage of only requiring a single additional piece of data,
* but has the disadvantage of being subject to divergence due to the ionospheric delays.
* The results of this filter were checked against those provided by J.Sanz Subirana in
* GNSS Data Processing Vol I and II, through provided code included in the tests.
* </p>
*
* Based on GNSS Data Processing Vol I and II by J.Sanz Subirana, J.M. Juan Zornoza and M. Hernández-Pajares, Section 4.2.3
*
* @author Louis Aucouturier
*
*/
public class CarrierPhaseHatchFilterSingleFrequency extends AbstractHatchFilter {
/** Wavelength of the carrier phase.*/
private double wavelength;
/** Previous observed carrier phase.*/
private double oldPhase;
/**
* Constructor for the Single Frequency Hatch Filter.
*
* The threshold parameter corresponds to the maximum difference between
* non-smoothed and smoothed pseudo range value, above which the filter
* is reset.
*
* @param codeData : initial Pseudo Range observation data
* @param phaseData : CarrierPhase ObservationData for the chosen observationType.
* @param satSystem : SatelliteSystem used.
* @param N : Maximum window size
* @param threshold
*/
public CarrierPhaseHatchFilterSingleFrequency(final ObservationData codeData,
final ObservationData phaseData,
final SatelliteSystem satSystem,
final int N,
final double threshold) {
super(codeData.getValue());
this.wavelength = phaseData.getObservationType().getFrequency(satSystem).getWavelength();
this.oldPhase = wavelength * phaseData.getValue();
setN(N);
setK(1);
}
/** Reset the filter in the case of a NaN phase value, skipping the smoothing at the present instant
* and initializing at the next one, keeping the current code value.
* @param codeValue : pseudo range value before the reset.
* */
public void resetFilterNext(final double codeValue) {
setK(1);
setResetNextBoolean(true);
oldPhase = Double.NaN;
setOldSmoothedCode(codeValue);
}
/** Filters the provided data given the state of the filter.
* Uses the Hatch Filter using a single frequency carrier-phase.
*
* @param codeData : Pseudo Range observation data
* @param phaseData : CarrierPhase ObservationData for the chosen observationType.
* @return modified ObservationData : PseudoRange observationData with smoothed value.
*/
public ObservationData filterData(final ObservationData codeData, final ObservationData phaseData) {
final boolean checkLLI = FastMath.floorMod(phaseData.getLossOfLockIndicator(), 2) == 0;
final double smoothingValue = wavelength * phaseData.getValue() - oldPhase;
final double smoothedValue = filterComputation(codeData.getValue(), smoothingValue);
final double newValue = checkValidData(codeData.getValue(), smoothedValue, checkLLI);
oldPhase = wavelength * phaseData.getValue();
addToCodeHistory(codeData.getValue());
addToSmoothedCodeHistory(newValue);
setOldSmoothedCode(newValue);
return super.modifyObservationData(codeData, newValue);
}
}
/* Copyright 2002-2022 CS GROUP
* Licensed to CS GROUP (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.estimation.measurements.filtering;
import org.orekit.gnss.ObservationData;