PolynomialCycleSlipDetector.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 <vector>
#include <utility>
#include <optional>
 
#include "Navigation/GNSS/Core/SatelliteIdentifier.hpp"
#include "Navigation/Math/PolynomialRegressor.hpp"
#include "Navigation/Time/InsTime.hpp"
#include "util/Container/Unordered_map.hpp"
#include "util/Container/Pair.hpp"
 
#include "internal/gui/widgets/imgui_ex.hpp"
#include "internal/gui/widgets/EnumCombo.hpp"
 
namespace NAV
{
 
class GnssAnalyzer;
class CycleSlipDetector;
 
enum class PolynomialCycleSlipDetectorResult
{
    Disabled,               
    LessDataThanWindowSize, 
    NoCycleSlip,            
    CycleSlip,              
};
 
template<typename Key>
class PolynomialCycleSlipDetector
{
  public:
    explicit PolynomialCycleSlipDetector(size_t windowSize, size_t polyDegree, bool enabled = true)
        : _enabled(enabled), _windowSize(windowSize), _polyDegree(polyDegree) {}
 
    [[nodiscard]] PolynomialCycleSlipDetectorResult checkForCycleSlip(const Key& key, InsTime insTime, double measurementDifference, double threshold)
    {
        if (!_enabled) { return PolynomialCycleSlipDetectorResult::Disabled; }
        if (!_detectors.contains(key))
        {
            addMeasurement(key, insTime, measurementDifference);
            return PolynomialCycleSlipDetectorResult::LessDataThanWindowSize;
        }
 
        const auto& detector = _detectors.at(key);
        if (!detector.polyReg.windowSizeReached())
        {
            addMeasurement(key, insTime, measurementDifference);
            return PolynomialCycleSlipDetectorResult::LessDataThanWindowSize;
        }
 
        auto polynomial = detector.polyReg.calcPolynomial();
        auto predictedValue = polynomial.f(calcRelativeTime(insTime, detector));
 
        if (std::abs(measurementDifference - predictedValue) > threshold)
        {
            reset(key);
            addMeasurement(key, insTime, measurementDifference);
            return PolynomialCycleSlipDetectorResult::CycleSlip;
        }
        addMeasurement(key, insTime, measurementDifference);
        return PolynomialCycleSlipDetectorResult::NoCycleSlip;
    }
 
    void clear()
    {
        _detectors.clear();
    }
 
    void reset(const Key& key)
    {
        if (_detectors.contains(key))
        {
            _detectors.erase(key);
        }
    }
 
    [[nodiscard]] bool isEnabled() const
    {
        return _enabled;
    }
    void setEnabled(bool enabled)
    {
        _enabled = enabled;
    }
 
    [[nodiscard]] size_t getWindowSize() const { return _windowSize; }
    void setWindowSize(size_t windowSize)
    {
        _windowSize = windowSize;
        for (auto& detector : _detectors)
        {
            detector.second.polyReg.setWindowSize(windowSize);
        }
    }
 
    [[nodiscard]] size_t getPolynomialDegree() const { return _polyDegree; }
    void setPolynomialDegree(size_t polyDegree)
    {
        _polyDegree = polyDegree;
        for (auto& detector : _detectors)
        {
            detector.second.polyReg.setPolynomialDegree(polyDegree);
        }
    }
 
    using Strategy = PolynomialRegressor<>::Strategy;
 
    [[nodiscard]] Strategy getFitStrategy() const { return _strategy; }
    void setFitStrategy(Strategy strategy)
    {
        _strategy = strategy;
        for (auto& detector : _detectors)
        {
            detector.second.polyReg.setStrategy(strategy);
        }
    }
 
  private:
    struct SignalDetector
    {
        SignalDetector(InsTime startTime, size_t windowSize, size_t polyDegree, Strategy strategy)
            : startTime(startTime), polyReg(polyDegree, windowSize, strategy) {}
 
        InsTime startTime;                   
        PolynomialRegressor<double> polyReg; 
    };
 
    bool _enabled = true;                          
    size_t _windowSize;                            
    size_t _polyDegree = 2;                        
    Strategy _strategy = Strategy::HouseholderQR;  
    unordered_map<Key, SignalDetector> _detectors; 
 
    [[nodiscard]] static double calcRelativeTime(const InsTime& insTime, const SignalDetector& detector)
    {
        return static_cast<double>((insTime - detector.startTime).count());
    }
    [[nodiscard]] std::optional<double> calcRelativeTime(const Key& key, const InsTime& insTime) const
    {
        if (!_detectors.contains(key)) { return {}; }
 
        return calcRelativeTime(insTime, _detectors.at(key));
    }
 
    [[nodiscard]] std::optional<double> predictValue(const Key& key, const InsTime& insTime) const
    {
        if (!_detectors.contains(key)) { return {}; }
 
        const auto& detector = _detectors.at(key);
 
        auto polynomial = detector.polyReg.calcPolynomial();
        return polynomial.f(calcRelativeTime(insTime, detector));
    }
 
    [[nodiscard]] std::optional<Polynomial<double>> calcPolynomial(const Key& key) const
    {
        if (!_detectors.contains(key)) { return {}; }
 
        return _detectors.at(key).polyReg.calcPolynomial();
    }
 
    void addMeasurement(const Key& key, InsTime insTime, double measurementDifference)
    {
        if (!_enabled) { return; }
        auto& detector = _detectors.insert({ key, SignalDetector(insTime, _windowSize, _polyDegree, _strategy) }).first->second;
 
        detector.polyReg.push_back(calcRelativeTime(insTime, detector), measurementDifference);
    }
 
    friend class GnssAnalyzer;
    friend class CycleSlipDetector;
};
 
template<typename Key>
bool PolynomialCycleSlipDetectorGui(const char* label, PolynomialCycleSlipDetector<Key>& polynomialCycleSlipDetector, float width = 0.0F)
{
    bool changed = false;
 
    bool enabled = polynomialCycleSlipDetector.isEnabled();
    if (ImGui::Checkbox(fmt::format("Enabled##{}", label).c_str(), &enabled))
    {
        changed = true;
        polynomialCycleSlipDetector.setEnabled(enabled);
    }
 
    if (!enabled) { ImGui::BeginDisabled(); }
 
    ImGui::SetNextItemWidth(width);
    if (int windowSize = static_cast<int>(polynomialCycleSlipDetector.getWindowSize());
        ImGui::InputIntL(fmt::format("Window size##{}", label).c_str(), &windowSize,
                         std::max(1, static_cast<int>(polynomialCycleSlipDetector.getPolynomialDegree()) + 1)))
    {
        changed = true;
        polynomialCycleSlipDetector.setWindowSize(static_cast<size_t>(windowSize));
    }
 
    ImGui::SetNextItemWidth(width);
    if (int polyDegree = static_cast<int>(polynomialCycleSlipDetector.getPolynomialDegree());
        ImGui::InputIntL(fmt::format("Polynomial Degree##{}", label).c_str(), &polyDegree,
                         0, std::min(static_cast<int>(polynomialCycleSlipDetector.getWindowSize()) - 1, std::numeric_limits<int>::max())))
    {
        changed = true;
        polynomialCycleSlipDetector.setPolynomialDegree(static_cast<size_t>(polyDegree));
    }
 
    ImGui::SetNextItemWidth(width);
    if (auto strategy = polynomialCycleSlipDetector.getFitStrategy();
        gui::widgets::EnumCombo(fmt::format("Strategy##{}", label).c_str(), strategy))
    {
        changed = true;
        polynomialCycleSlipDetector.setFitStrategy(strategy);
    }
 
    if (!enabled) { ImGui::EndDisabled(); }
 
    return changed;
}
 
template<typename Key>
void to_json(json& j, const PolynomialCycleSlipDetector<Key>& data)
{
    j = json{
        { "enabled", data.isEnabled() },
        { "windowSize", data.getWindowSize() },
        { "polynomialDegree", data.getPolynomialDegree() },
        { "strategy", data.getFitStrategy() },
    };
}
template<typename Key>
void from_json(const json& j, PolynomialCycleSlipDetector<Key>& data)
{
    if (j.contains("enabled"))
    {
        auto enabled = j.at("enabled").get<bool>();
        data.setEnabled(enabled);
    }
    if (j.contains("windowSize"))
    {
        auto windowSize = j.at("windowSize").get<size_t>();
        data.setWindowSize(windowSize);
    }
    if (j.contains("polynomialDegree"))
    {
        auto polynomialDegree = j.at("polynomialDegree").get<size_t>();
        data.setPolynomialDegree(polynomialDegree);
    }
    if (j.contains("strategy"))
    {
        auto strategy = j.at("strategy").get<size_t>();
        data.setFitStrategy(static_cast<typename PolynomialCycleSlipDetector<Key>::Strategy>(strategy));
    }
}
 
} // namespace NAV