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 <memory>
#include <set>
#include <unordered_set>
 
#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
    {
        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,
                                 const Eigen::Vector3d& e_recPos,
                                 const Eigen::Vector3d& lla_recPos,
                                 const Eigen::Vector3d& e_recVel,
                                 const Eigen::Vector3d& e_satPos,
                                 const Eigen::Vector3d& e_satVel,
                                 Clock::Corrections satClock)
                : _gnssObs(std::move(gnssObs)), _obsIdx(obsIdx), _e_satPos(e_satPos), _e_satVel(e_satVel), _satClock(satClock)
            {
                _e_pLOS = e_calcLineOfSightUnitVector(e_recPos, e_satPos);
                _e_vLOS = (e_recVel - e_satVel) / (e_recPos - e_satPos).norm();
                Eigen::Vector3d n_lineOfSightUnitVector = trafo::n_Quat_e(lla_recPos(0), lla_recPos(1)) * _e_pLOS;
                _satElevation = calcSatElevation(n_lineOfSightUnitVector);
                _satAzimuth = calcSatAzimuth(n_lineOfSightUnitVector);
 
                LOG_DATA("    e_lineOfSightUnitVector {}", _e_pLOS.transpose());
                LOG_DATA("    n_lineOfSightUnitVector {}", n_lineOfSightUnitVector.transpose());
                LOG_DATA("    satElevation {}°", rad2deg(_satElevation));
                LOG_DATA("    satAzimuth   {}°", rad2deg(_satAzimuth));
            }
 
            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]] const Eigen::Vector3d& e_pLOS() const { return _e_pLOS; }
            [[nodiscard]] const Eigen::Vector3d& e_vLOS() const { return _e_vLOS; }
            [[nodiscard]] const double& satElevation() const { return _satElevation; }
            [[nodiscard]] const double& satAzimuth() const { return _satAzimuth; }
            [[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;                         
            Eigen::Vector3d _e_pLOS;                           
            Eigen::Vector3d _e_vLOS;                           
            Clock::Corrections _satClock;                      
            double _satElevation = 0.0;                        
            double _satAzimuth = 0.0;                          
        };
 
        SignalObservation(std::shared_ptr<SatNavData> navData,
                          int8_t freqNum)
            : _navData(std::move(navData)),
              _freqNum(freqNum) {}
 
        std::vector<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::set<SatelliteSystem> systems;                                                
    std::unordered_set<SatId> satellites;                                             
    std::array<size_t, GnssObs::ObservationType_COUNT> nObservables{};                
    std::array<size_t, GnssObs::ObservationType_COUNT> nObservablesUniqueSatellite{}; 
 
    void recalcObservableCounts();
};
 
} // namespace NAV