CycleSlipDetector.hpp

Go to the documentation of this file.

// This file is part of INSTINCT, the INS Toolkit for Integrated
// Navigation Concepts and Training by the Institute of Navigation of
// the University of Stuttgart, Germany.
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at https://mozilla.org/MPL/2.0/.
 
 
#pragma once
 
#include <variant>
#include <array>
#include "NodeData/GNSS/GnssObs.hpp"
#include "Navigation/GNSS/Core/SatelliteIdentifier.hpp"
#include "CycleSlipDetector/PolynomialCycleSlipDetector.hpp"
 
namespace NAV
{
 
class CycleSlipDetector
{
  public:
    enum class Detector
    {
        LLI,             
        SingleFrequency, 
        DualFrequency,   
    };
 
    [[nodiscard]] bool isEnabled(const Detector& detector) const
    {
        switch (detector)
        {
        case Detector::LLI:
            return _enableLLICheck;
        case Detector::SingleFrequency:
            return _singleFrequencyDetector.isEnabled();
        case Detector::DualFrequency:
            return _dualFrequencyDetector.isEnabled();
        };
        return false;
    }
    void setEnabled(bool enabled, const Detector& detector)
    {
        switch (detector)
        {
        case Detector::LLI:
            _enableLLICheck = enabled;
            break;
        case Detector::SingleFrequency:
            _singleFrequencyDetector.setEnabled(enabled);
            break;
        case Detector::DualFrequency:
            _dualFrequencyDetector.setEnabled(enabled);
            break;
        };
    }
 
    [[nodiscard]] size_t getWindowSize(const Detector& detector) const
    {
        return detector == Detector::SingleFrequency ? _singleFrequencyDetector.getWindowSize() : _dualFrequencyDetector.getWindowSize();
    }
    void setWindowSize(size_t windowSize, const Detector& detector)
    {
        if (detector == Detector::SingleFrequency) { _singleFrequencyDetector.setWindowSize(windowSize); }
        else { _dualFrequencyDetector.setWindowSize(windowSize); }
    }
 
    [[nodiscard]] double getThreshold(const Detector& detector) const
    {
        return detector == Detector::SingleFrequency ? _singleFrequencyThresholdPercentage : _dualFrequencyThresholdPercentage;
    }
    void setThreshold(double threshold, const Detector& detector)
    {
        if (detector == Detector::SingleFrequency) { _singleFrequencyThresholdPercentage = threshold; }
        else { _dualFrequencyThresholdPercentage = threshold; }
    }
 
    [[nodiscard]] size_t getPolynomialDegree(const Detector& detector) const
    {
        return detector == Detector::SingleFrequency ? _singleFrequencyDetector.getPolynomialDegree() : _dualFrequencyDetector.getPolynomialDegree();
    }
    void setPolynomialDegree(size_t polyDegree, const Detector& detector)
    {
        if (detector == Detector::SingleFrequency) { _singleFrequencyDetector.setPolynomialDegree(polyDegree); }
        else { _dualFrequencyDetector.setPolynomialDegree(polyDegree); }
    }
 
    using Strategy = PolynomialRegressor<>::Strategy;
 
    [[nodiscard]] Strategy getFitStrategy(const Detector& detector) const
    {
        return detector == Detector::SingleFrequency ? _singleFrequencyDetector.getFitStrategy() : _dualFrequencyDetector.getFitStrategy();
    }
    void setFitStrategy(Strategy strategy, const Detector& detector)
    {
        if (detector == Detector::SingleFrequency) { _singleFrequencyDetector.setFitStrategy(strategy); }
        else { _dualFrequencyDetector.setFitStrategy(strategy); }
    }
 
    struct CycleSlipLossOfLockIndicator
    {
        SatSigId signal; 
    };
    struct CycleSlipSingleFrequency
    {
        SatSigId signal; 
    };
    struct CycleSlipDualFrequency
    {
        std::array<SatSigId, 2> signals; 
    };
 
    using Result = std::variant<CycleSlipLossOfLockIndicator, CycleSlipDualFrequency, CycleSlipSingleFrequency>;
 
    struct SatelliteObservation
    {
        struct Signal
        {
            Code code;                                          
            GnssObs::ObservationData::CarrierPhase measurement; 
        };
 
        SatId satId;                 
        std::vector<Signal> signals; 
        int8_t freqNum = -128;       
    };
 
    [[nodiscard]] std::vector<Result> checkForCycleSlip(InsTime insTime, const std::vector<SatelliteObservation>& satObs, const std::string& nameId);
 
    void resetSignal(const SatSigId& satSigId);
 
    void reset();
 
    struct DualFrequencyCombination
    {
        constexpr bool operator==(const DualFrequencyCombination& rhs) const { return satId == rhs.satId && sig1 == rhs.sig1 && sig2 == rhs.sig2; }
 
        constexpr bool operator<(const DualFrequencyCombination& rhs) const
        {
            return satId == rhs.satId ? (sig1 == rhs.sig1
                                             ? sig2 < rhs.sig2
                                             : sig1 < rhs.sig1)
                                      : satId < rhs.satId;
        }
 
        SatId satId; 
        Code sig1;   
        Code sig2;   
    };
 
  private:
    bool _enableLLICheck = true;
 
    double _singleFrequencyThresholdPercentage = 11.0; 
    double _dualFrequencyThresholdPercentage = 0.6;    
 
    PolynomialCycleSlipDetector<SatSigId> _singleFrequencyDetector{ 4, 2, false };
 
    PolynomialCycleSlipDetector<DualFrequencyCombination> _dualFrequencyDetector{ 2, 1 };
 
    friend bool CycleSlipDetectorGui(const char* label, CycleSlipDetector& cycleSlipDetector, float width, bool dualFrequencyAvailable);
    friend void to_json(json& j, const CycleSlipDetector& data);
    friend void from_json(const json& j, CycleSlipDetector& data);
};
 
bool CycleSlipDetectorGui(const char* label, CycleSlipDetector& cycleSlipDetector, float width = 0.0F, bool dualFrequencyAvailable = true);
 
void to_json(json& j, const CycleSlipDetector& data);
void from_json(const json& j, CycleSlipDetector& data);
 
[[nodiscard]] std::string to_string(const CycleSlipDetector::Result& cycleSlip);
 
} // namespace NAV
 
std::ostream& operator<<(std::ostream& os, const NAV::CycleSlipDetector::Result& obj);
 
namespace std
{
template<>
struct hash<NAV::CycleSlipDetector::DualFrequencyCombination>
{
    std::size_t operator()(const NAV::CycleSlipDetector::DualFrequencyCombination& c) const
    {
        auto hash1 = std::hash<NAV::SatId>{}(c.satId);
        auto hash2 = std::hash<NAV::Code>{}(c.sig1);
        auto hash3 = std::hash<NAV::Code>{}(c.sig2);
 
        return hash1 | (hash2 << 24) | (hash3 << 48);
    }
};
} // namespace std
 
#ifndef DOXYGEN_IGNORE
 
template<>
struct fmt::formatter<NAV::CycleSlipDetector::Result> : fmt::formatter<std::string>
{
    template<typename FormatContext>
    auto format(const NAV::CycleSlipDetector::Result& cycleSlip, FormatContext& ctx) const
    {
        return fmt::formatter<std::string>::format(to_string(cycleSlip), ctx);
    }
};
 
#endif