Observation.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 <cstdint>
#include <memory>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <utility>
 
#include "Navigation/GNSS/Core/SatelliteIdentifier.hpp"
#include "Navigation/GNSS/Functions.hpp"
#include "Navigation/GNSS/Satellite/internal/SatNavData.hpp"
#include "Navigation/Transformations/CoordinateFrames.hpp"
#include "Navigation/Transformations/Units.hpp"
#include "Navigation/Atmosphere/Troposphere/ZenithDelay.hpp"
 
#include "NodeData/GNSS/GnssObs.hpp"
 
#include "util/Container/Unordered_map.hpp"
#include "util/Eigen.hpp"
#include "util/Logger.hpp"
 
namespace NAV
{
 
struct Observations
{
    struct SignalObservation
    {
        ~SignalObservation() = default;
        SignalObservation(const SignalObservation& other)
            : _navData(other._navData), _freqNum(other._freqNum)
        {
            for (const auto& [index, obs] : other.recvObs)
            {
                recvObs.insert(std::make_pair(index, std::make_shared<ReceiverSpecificData>(*obs)));
            }
        }
        SignalObservation& operator=(const SignalObservation& other)
        {
            if (this == &other) { return *this; } // Guard self assignment
 
            for (const auto& [index, obs] : other.recvObs)
            {
                recvObs.insert(std::make_pair(index, std::make_shared<ReceiverSpecificData>(*obs)));
            }
            _navData = other._navData;
            _freqNum = other._freqNum;
 
            return *this;
        }
        SignalObservation(SignalObservation&& other) noexcept
            : recvObs(std::move(other.recvObs)), _navData(std::move(other._navData)), _freqNum(other._freqNum) {}
        SignalObservation& operator=(SignalObservation&& other) noexcept
        {
            if (this == &other) { return *this; } // Guard self assignment
 
            recvObs = std::move(other.recvObs);
            _navData = std::move(other._navData);
            _freqNum = other._freqNum;
 
            return *this;
        }
 
        struct ReceiverSpecificData
        {
            struct Observation
            {
                double estimate = 0.0;    
                double measurement = 0.0; 
                double measVar = 0.0;     
            };
 
            ReceiverSpecificData(std::shared_ptr<const GnssObs> gnssObs,
                                 size_t obsIdx,
                                 Eigen::Vector3d e_satPos,
                                 Eigen::Vector3d e_satVel,
                                 Clock::Corrections satClock)
                : _gnssObs(std::move(gnssObs)),
                  _obsIdx(obsIdx),
                  _e_satPos(std::move(e_satPos)),
                  _e_satVel(std::move(e_satVel)),
                  _satClock(satClock) {}
 
            unordered_map<GnssObs::ObservationType, Observation> obs;
 
            [[nodiscard]] const GnssObs::ObservationData& gnssObsData() const { return _gnssObs->data.at(_obsIdx); }
            [[nodiscard]] const Eigen::Vector3d& e_satPos() const { return _e_satPos; }
            [[nodiscard]] const Eigen::Vector3d& e_satVel() const { return _e_satVel; }
            [[nodiscard]] Eigen::Vector3d e_pLOS(const Eigen::Vector3d& e_recPos) const
            {
                return e_calcLineOfSightUnitVector(e_recPos, _e_satPos);
            }
            [[nodiscard]] Eigen::Vector3d e_vLOS(const Eigen::Vector3d& e_recPos,
                                                 const Eigen::Vector3d& e_recVel) const
            {
                return (_e_satVel - e_recVel) / (_e_satPos - e_recPos).norm();
            }
            [[nodiscard]] double satElevation(const Eigen::Vector3d& e_recPos,
                                              const Eigen::Vector3d& lla_recPos) const
            {
                Eigen::Vector3d n_lineOfSightUnitVector = trafo::n_Quat_e(lla_recPos(0), lla_recPos(1))
                                                          * e_pLOS(e_recPos);
                return calcSatElevation(n_lineOfSightUnitVector);
            }
            [[nodiscard]] double satAzimuth(const Eigen::Vector3d& e_recPos,
                                            const Eigen::Vector3d& lla_recPos) const
            {
                Eigen::Vector3d n_lineOfSightUnitVector = trafo::n_Quat_e(lla_recPos(0), lla_recPos(1))
                                                          * e_pLOS(e_recPos);
                return calcSatAzimuth(n_lineOfSightUnitVector);
            }
            [[nodiscard]] const Clock::Corrections& satClock() const { return _satClock; }
 
            struct CalcTerms
            {
                double rho_r_s = 0.0;         
                ZenithDelay tropoZenithDelay; 
                double dpsr_T_r_s = 0.0;      
                double dpsr_I_r_s = 0.0;      
                double dpsr_ie_r_s = 0.0;     
            };
            CalcTerms terms; 
 
          private:
            std::shared_ptr<const GnssObs> _gnssObs = nullptr; 
            size_t _obsIdx = 0;                                
            Eigen::Vector3d _e_satPos;                         
            Eigen::Vector3d _e_satVel;                         
            Clock::Corrections _satClock;                      
        };
 
        SignalObservation(std::shared_ptr<SatNavData> navData,
                          int8_t freqNum)
            : _navData(std::move(navData)),
              _freqNum(freqNum) {}
 
        std::unordered_map<size_t, std::shared_ptr<ReceiverSpecificData>> recvObs;
 
        [[nodiscard]] const std::shared_ptr<const SatNavData>& navData() const { return _navData; }
        [[nodiscard]] int8_t freqNum() const { return _freqNum; }
 
      private:
        std::shared_ptr<const SatNavData> _navData = nullptr; 
        int8_t _freqNum = -128;                               
    };
 
    unordered_map<SatSigId, SignalObservation> signals; 
 
    std::unordered_set<size_t> receivers;                                             
    std::set<SatelliteSystem> systems;                                                
    std::unordered_set<SatId> satellites;                                             
    std::array<size_t, GnssObs::ObservationType_COUNT> nObservables{};                
    std::array<size_t, GnssObs::ObservationType_COUNT> nObservablesUniqueSatellite{}; 
 
    size_t countObservations(const SatSigId& satSigId, const GnssObs::ObservationType& obsType) const;
 
    void recalcObservableCounts(const std::string& nameId);
 
    bool removeSignal(const SatSigId& satSigId, const std::string& nameId);
 
    bool removeSignal(const SatSigId& satSigId, const GnssObs::ObservationType& obsType, const std::string& nameId);
 
    bool removeSatellite(const SatId& satId, const std::string& nameId);
 
    template<typename ReceiverType>
    bool removeMeasurementsFor(const Code& code, const GnssObs::ObservationType& obsType, [[maybe_unused]] const std::string& nameId)
    {
        LOG_DATA("{}: Searching observations to remove on [{}][{}]", nameId, code, obsType);
        bool somethingRemoved = false;
        for (auto& [satSigId, sigObs] : signals)
        {
            if (satSigId.code != code) { continue; }
 
            for (size_t i = 0; i < sigObs.recvObs.size(); i++)
            {
                if (!sigObs.recvObs.contains(i)) { continue; }
                auto& recvObs = sigObs.recvObs.at(i);
                if (recvObs->obs.contains(obsType))
                {
                    recvObs->obs.erase(obsType);
                    LOG_DATA("{}:   Erasing observation [{}][{}] of receiver '{}'", nameId, satSigId, obsType, to_string(static_cast<ReceiverType>(i)));
                    somethingRemoved = true;
                }
            }
        }
        if (somethingRemoved) { recalcObservableCounts(nameId); }
        return somethingRemoved;
    }
};
 
} // namespace NAV