DualFrequencyHatchFilter.java

/* Copyright 2002-2024 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.files.rinex.observation.ObservationData;
import org.orekit.utils.Constants;

/**
 * Hatch Filter using Carrier-Phase measurements taken at two 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.
 * </p>
 * @see "Subirana, J. S., Hernandez-Pajares, M., and José Miguel Juan Zornoza. (2013).
 *       GNSS Data Processing: Fundamentals and Algorithms. European Space Agency.
 *       Section 4.2.3.1.1"
 *
 * @author Louis Aucouturier
 * @since 11.2
 */
public class DualFrequencyHatchFilter extends HatchFilter {

    /** First wavelength used for smoothing. */
    private double wavelengthFreq1;

    /** Second wavelength used for smoothing. */
    private double wavelengthFreq2;

    /** 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 Dual Frequency Hatch Filter.
     * <p>
     * The threshold parameter corresponds to the maximum difference between
     * non-smoothed and smoothed pseudo range value, above which the filter
     * is reset.
     * </p>
     * @param initCode        initial code measurement
     * @param initPhaseFreq1  initial phase measurement for the first chosen frequency
     * @param initPhaseFreq2  initial phase measurement for the second chosen frequency
     * @param wavelengthFreq1 initPhaseFreq1 observed value wavelength (m)
     * @param wavelengthFreq2 initPhaseFreq2 observed value wavelength (m)
     * @param threshold       threshold for loss of lock detection
     *                        (it represents the maximum difference between smoothed
     *                        and measured values for loss of lock detection)
     * @param N               window size of the Hatch Filter
     */
    public DualFrequencyHatchFilter(final ObservationData initCode,
                                    final ObservationData initPhaseFreq1, final ObservationData initPhaseFreq2,
                                    final double wavelengthFreq1, final double wavelengthFreq2,
                                    final double threshold, final int N) {
        super(threshold, N);
        // Initialize wavelength and compute frequencies
        this.wavelengthFreq1 = wavelengthFreq1;
        this.wavelengthFreq2 = wavelengthFreq2;

        // Initialize array of phase values used during smoothing
        this.phase1History = new ArrayList<>();
        this.phase2History = new ArrayList<>();
        phase1History.add(initPhaseFreq1.getValue() * wavelengthFreq1);
        phase2History.add(initPhaseFreq2.getValue() * wavelengthFreq2);
        updatePreviousSmoothedCode(initCode.getValue());
        updatePreviousSmoothingValue(divergenceFreeCombination(initPhaseFreq1.getValue(), initPhaseFreq2.getValue(), wavelengthFreq1, wavelengthFreq2));
        addToSmoothedCodeHistory(initCode.getValue());
        addToCodeHistory(initCode.getValue());
    }

    /**
     * This method filters the provided data given the state of the filter.
     * @param codeData       input code observation data
     * @param phaseDataFreq1 input phase observation data for the first frequency
     * @param phaseDataFreq2 input phase observation data for the second frequency
     * @return the smoothed observation data
     */
    public ObservationData filterData(final ObservationData codeData, final ObservationData phaseDataFreq1, final ObservationData phaseDataFreq2) {

        // Current code value
        final double code = codeData.getValue();
        addToCodeHistory(code);

        // Computes the phase combination and smoothing value (Ref Eq. 4.32)
        final double phaseFreq1 = wavelengthFreq1 * phaseDataFreq1.getValue();
        final double phaseFreq2 = wavelengthFreq2 * phaseDataFreq2.getValue();
        final double phaseDF = divergenceFreeCombination(phaseDataFreq1.getValue(), phaseDataFreq2.getValue(), wavelengthFreq1, wavelengthFreq2);
        phase1History.add(phaseFreq1);
        phase2History.add(phaseFreq2);

        // Check for carrier phase cycle slip (check on the two phase data)
        final boolean cycleSlip = FastMath.floorMod(phaseDataFreq1.getLossOfLockIndicator(), 2) != 0 ||
                        FastMath.floorMod(phaseDataFreq2.getLossOfLockIndicator(), 2) != 0;

        // Computes the smoothed code value
        double smoothedValue = smoothedCode(code, phaseDF);
        updatePreviousSmoothingValue(phaseDF);

        // Check if filter reset needed, if not return smoothedValue, and increase k if necessary.
        smoothedValue = checkValidData(code, smoothedValue, cycleSlip);
        addToSmoothedCodeHistory(smoothedValue);
        updatePreviousSmoothedCode(smoothedValue);

        // Return the smoothed observed data
        return new ObservationData(codeData.getObservationType(), smoothedValue,
                                   codeData.getLossOfLockIndicator(), codeData.getSignalStrength());

    }

    /**
     * Get the history of phase values of the first frequency.
     * @return the history of phase values of the first frequency
     */
    public ArrayList<Double> getFirstFrequencyPhaseHistory() {
        return phase1History;
    }

    /**
     * Get the history of phase values of the second frequency.
     * @return the history of phase values of the second frequency
     */
    public ArrayList<Double> getSecondFrequencyPhaseHistory() {
        return phase2History;
    }

    /**
     * Divergence-free combination (Ref Eq. 4.32).
     * <p>
     * phase_DF = phase_F1 + 2.0 * alpha + (phase_F1 - phase_F2)
     * </p>
     * @param phase1  phase value for frequency 1
     * @param phase2  phase value for frequency 2
     * @param lambda1 wavelength of the first phase (m)
     * @param lambda2 wavelength of the second phase (m)
     * @return the value of the divergence-free combination
     */
    private static double divergenceFreeCombination(final double phase1, final double phase2,
                                                    final double lambda1, final double lambda2) {

        // Multiply phase value by its wavelength
        final double phaseFreq1 = lambda1 * phase1;
        final double phaseFreq2 = lambda2 * phase2;

        // Convert wavelength to frequencies
        final double f1 = Constants.SPEED_OF_LIGHT / lambda1;
        final double f2 = Constants.SPEED_OF_LIGHT / lambda2;

        // Alpha
        final double alpha = 1.0 / ((f1 * f1) / (f2 * f2) - 1.0);

        // Return
        return  phaseFreq1 + 2.0 * alpha * (phaseFreq1 - phaseFreq2);

    }

}