InterFrequencyBiasModel.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 "Units.hpp"
 
#include "Navigation/GNSS/SystemModel/SystemModel.hpp"
#include "util/Container/KeyedMatrix.hpp"
#include <fmt/format.h>
#include "Navigation/GNSS/SystemModel/Units.hpp"
#include "internal/gui/widgets/InputWithUnit.hpp"
#include "Navigation/GNSS/Core/Frequency.hpp"
 
#include "util/Logger.hpp"
 
namespace NAV
{
 
namespace Keys
{

struct InterFreqBias
{
    bool operator==(const InterFreqBias& rhs) const { return freq == rhs.freq; }
    Frequency freq;
};
 
} // namespace Keys

template<typename StateKeyType>
class InterFrequencyBiasModel
{
  public:
    void initialize()
    {
        // Covariance of the inter-frequency bias [m²/s]
        _covarianceInterFrequencyBias = convertUnit(_gui_covarianceInterFrequencyBias, _gui_covarianceInterFrequencyBiasUnit);
    }

    template<typename Scalar, int Size>
    void initialize(Keys::InterFreqBias bias,
                    KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& F,
                    KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& G,
                    KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& W)
    {
        initialize();
 
        F(bias, bias) = 0;
        G(bias, bias) = 1;
        W(bias, bias) = _covarianceInterFrequencyBias;
    }

    template<typename Scalar, int Size>
    void updatePhiAndQ(KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& Phi,
                       KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& Q,
                       double dt)
    {
        for (const auto& key : Phi.rowKeys())
        {
            if (const auto* bias = std::get_if<Keys::InterFreqBias>(&key))
            {
                Phi(*bias, *bias) = 1;
                Q(*bias, *bias) = _covarianceInterFrequencyBias * dt;
            }
        }
    }

    [[nodiscard]] std::pair<double, double> calcPhiAndQ(double dt, const Frequency& freq)
    {
        std::vector<StateKeyType> key = { Keys::InterFreqBias{ freq } };
 
        KeyedMatrix<double, StateKeyType, StateKeyType, 1, 1> Phi(Eigen::Matrix<double, 1, 1>::Zero(), key, key);
        KeyedMatrix<double, StateKeyType, StateKeyType, 1, 1> Q(Eigen::Matrix<double, 1, 1>::Zero(), key, key);
        updatePhiAndQ(Phi, Q, dt);
 
        return { Phi(all, all)(0), Q(all, all)(0) };
    }

    bool ShowGui(float itemWidth, float unitWidth, const char* id)
    {
        bool changed = false;
 
        if (gui::widgets::InputDoubleWithUnit(fmt::format("{} of the inter-frequency bias (RW)##{}",
                                                          _gui_covarianceInterFrequencyBiasUnit == Units::CovarianceClkPhaseDriftUnits::m_sqrts
                                                              ? "StdDev"
                                                              : "Variance",
                                                          id)
                                                  .c_str(),
                                              itemWidth, unitWidth, &_gui_covarianceInterFrequencyBias,
                                              _gui_covarianceInterFrequencyBiasUnit,
                                              MakeComboItems<Units::CovarianceClkPhaseDriftUnits>().c_str(),
                                              0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
        {
            LOG_DEBUG("{}: _gui_covarianceInterFrequencyBias changed to {}", id, _gui_covarianceInterFrequencyBias);
            LOG_DEBUG("{}: _gui_covarianceInterFrequencyBiasUnit changed to {}", id, to_string(_gui_covarianceInterFrequencyBiasUnit));
            changed = true;
        }
 
        return changed;
    }
 
  private:
    Units::CovarianceClkPhaseDriftUnits _gui_covarianceInterFrequencyBiasUnit = Units::CovarianceClkPhaseDriftUnits::m2_s;

    double _gui_covarianceInterFrequencyBias = 1e-6 /* [m²/s] */;
    double _covarianceInterFrequencyBias = 1e-6;

    friend void to_json(json& j, const InterFrequencyBiasModel& data)
    {
        j = {
            { "covarianceInterFrequencyBiasUnit", data._gui_covarianceInterFrequencyBiasUnit },
            { "covarianceInterFrequencyBias", data._gui_covarianceInterFrequencyBias },
        };
    }

    friend void from_json(const json& j, InterFrequencyBiasModel& data)
    {
        if (j.contains("covarianceInterFrequencyBiasUnit")) { j.at("covarianceInterFrequencyBiasUnit").get_to(data._gui_covarianceInterFrequencyBiasUnit); }
        if (j.contains("covarianceInterFrequencyBias")) { j.at("covarianceInterFrequencyBias").get_to(data._gui_covarianceInterFrequencyBias); }
    }
};
 
} // namespace NAV

std::ostream& operator<<(std::ostream& os, const NAV::Keys::InterFreqBias& obj);
 
namespace std
{

template<>
struct hash<NAV::Keys::InterFreqBias>
{
    size_t operator()(const NAV::Keys::InterFreqBias& interFreqBias) const
    {
        return std::hash<NAV::Frequency>()(interFreqBias.freq);
    }
};
 
} // namespace std
 
#ifndef DOXYGEN_IGNORE

template<>
struct fmt::formatter<NAV::Keys::InterFreqBias> : fmt::formatter<std::string>
{
    template<typename FormatContext>
    auto format(const NAV::Keys::InterFreqBias& interFreqBias, FormatContext& ctx) const
    {
        return fmt::formatter<std::string>::format(fmt::format("InterFreqBias({})", interFreqBias.freq), ctx);
    }
};
 
#endif