HatchFilter.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;

/** Base class for Hatch filters
 * <p>
 * Hatch filters are generally 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.
 * <p>
 * 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>
 * @see "Subirana, J. S., Hernandez-Pajares, M., and José Miguel Juan Zornoza. (2013).
 *       GNSS Data Processing: Fundamentals and Algorithms. European Space Agency."
 *
 * @see "Zhou, Z., and Li, B. (2017). Optimal Doppler-aided smoothing strategy for GNSS navigation.
 *       GPS solutions, 21(1), 197-210."
 *
 * @author Louis Aucouturier
 * @since 11.2
 */
abstract class HatchFilter {

    /** Current index for the filter. */
    private int k;

    /** Boolean used to delay the reset of the filter. */
    private boolean resetNextBoolean;

    /** Last smoothed value for the pseudo-range. */
    private double previousSmoothedCode;

    /** Last smoothing value. */
    private double previousSmoothingValue;

    /** 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;

    /** Window size of the Hatch Filter. */
    private final int N;

    /**
     * Constructor for the Abstract Hatch Filter.
     * <p>
     * Initialize the variables and set the initial pseudo-range state.
     * </p>
     * @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
     */
    HatchFilter(final double threshold, final int N) {
        // Initialize fields
        this.codeHistory         = new ArrayList<>();
        this.smoothedCodeHistory = new ArrayList<>();
        this.threshold           = threshold;
        this.resetNextBoolean    = false;
        this.k                   = 1;
        this.N                   = N;
    }

    /**
     * Get the history of the pseudo-range values used by the filter.
     * @return the history of the pseudo-range values used by the filter
     */
    public ArrayList<Double> getCodeHistory() {
        return codeHistory;
    }

    /**
     * Get the history of the smoothed pseudo-range values used by the filter.
     * @return the smoothedCodeHistory
     */
    public ArrayList<Double> getSmoothedCodeHistory() {
        return smoothedCodeHistory;
    }

    /**
     * Get the threshold for loss of lock detection.
     * <p>
     * It represents the maximum difference between smoothed
     * and measured values for loss of lock detection.
     * </p>
     * @return the threshold for loss of lock detection
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Update the previous smoothed value for the pseudo-range.
     * @param smoothedCode the previous smoothed value for the pseudo-range
     */
    public void updatePreviousSmoothedCode(final double smoothedCode) {
        this.previousSmoothedCode = smoothedCode;
    }

    /**
     * Update the previous smoothing value.
     * @param smoothingValue the previous smoothing value
     */
    public void updatePreviousSmoothingValue(final double smoothingValue) {
        this.previousSmoothingValue = smoothingValue;
    }

    /**
     * Add a value to the history of the pseudo-range value.
     * @param codeValue the value to add to the history
     */
    public void addToCodeHistory(final double codeValue) {
        codeHistory.add(codeValue);
    }

    /**
     * Add a value to the history of the smoothed pseudo-range value.
     * @param smoothedCodeValue the value to add to the history
     */
    public void addToSmoothedCodeHistory(final double smoothedCodeValue) {
        smoothedCodeHistory.add(smoothedCodeValue);
    }

    /**
     * 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) {
        resetK();
        resetNextBoolean();
        updatePreviousSmoothingValue(Double.NaN);
        updatePreviousSmoothedCode(codeValue);
    }

    /**
     * Set the flag for resetting the filter to true.
     */
    protected void resetNextBoolean() {
        this.resetNextBoolean = true;
    }

    /**
     * Reset the current index of the filter to 1.
     */
    protected void resetK() {
        this.k = 1;
    }

    /**
     * Computes the smoothed code value.
     * <p>
     * This method corresponds to Eq. (4.23) of
     * "Subirana, J. S., Hernandez-Pajares, M., and José Miguel Juan Zornoza.
     *  GNSS Data Processing: Fundamentals and Algorithms.
     *  European Space Agency."
     * </p>
     * @param codeValue value of the input code measurement (non-smoothed)
     * @param smoothingValue value of the measurement used to smooth the code measurement
     * @return the smoothed code value
     */
    protected double smoothedCode(final double codeValue,
                                  final double smoothingValue) {
        // Equation 4.23
        return (codeValue + (k - 1) * (previousSmoothedCode + (smoothingValue - previousSmoothingValue))) / k;
    }

    /**
     * Checks if need to reset the filter or if cycle slip occurred.
     * <p>
     * If yes, the smoothed value is reinitialized to the last code value.
     * If no, the smoothed value value is not changed and the filter counter is updated.
     * </p>
     * @param codeValue value of the input code measurement (non-smoothed)
     * @param smoothedValue the smoothed code value
     * @param cycleSlip true if cycle slip is detected
     * @return the smoothed or non-smoothed value depending if the filter must be reseted
     */
    protected double checkValidData(final double codeValue, final double smoothedValue, final boolean cycleSlip) {

        // Initialize the returned value to the smoothed value
        double validValue = smoothedValue;

        // This algorithm must be initialised every time that a carrier phase cycle slip occurs
        // Check if need to reset the filter
        if (resetNextBoolean || cycleSlip || FastMath.abs(smoothedValue - codeValue) > threshold) {
            // The smoothed value is updated to the non smoothed value, and reset the filter counter to 1
            resetK();
            validValue = codeValue;
        } else {
            // No reset, just increase the counter
            k = (k > N) ? N : k + 1;
        }

        // Return
        return validValue;

    }

}