Algorithm.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 <fmt/format.h>
#include <cstdint>
#include <set>
 
#include "Navigation/GNSS/Positioning/Observation.hpp"
#include "Navigation/GNSS/Positioning/ObservationEstimator.hpp"
#include "Navigation/GNSS/Positioning/ObservationFilter.hpp"
#include "Navigation/GNSS/Positioning/Receiver.hpp"
#include "Navigation/GNSS/Positioning/SPP/Keys.hpp"
#include "Navigation/GNSS/Positioning/SPP/KalmanFilter.hpp"
 
#include "NodeData/GNSS/GnssObs.hpp"
#include "NodeData/GNSS/SppSolution.hpp"
 
namespace NAV
{
 
namespace SPP
{

class Algorithm
{
  public:
    enum class EstimatorType : uint8_t
    {
        LeastSquares,         
        WeightedLeastSquares, 
        KalmanFilter,         
        COUNT,                
    };

    enum ReceiverType : uint8_t
    {
        Rover,              
        ReceiverType_COUNT, 
    };

    bool ShowGuiWidgets(const char* id, float itemWidth, float unitWidth);

    void reset();

    [[nodiscard]] EstimatorType getEstimatorType() const { return _estimatorType; }

    const InsTime& getLastUpdateTime() const { return _lastUpdate; }

    std::shared_ptr<SppSolution> calcSppSolution(const std::shared_ptr<const GnssObs>& gnssObs,
                                                 const std::vector<const GnssNavInfo*>& gnssNavInfos,
                                                 const std::string& nameId);

    ObservationFilter _obsFilter{ ReceiverType_COUNT,
                                  /* available */ std::unordered_set{ GnssObs::Pseudorange, GnssObs::Doppler },
                                  /*   needed  */ std::unordered_set{ GnssObs::Pseudorange } };

    ObservationEstimator _obsEstimator{ ReceiverType_COUNT };

    bool _estimateInterFreqBiases = true;
 
  private:
    using Receiver = NAV::Receiver<ReceiverType>; 

    const std::array<SPP::States::StateKeyType, 3>& PosKey = Keys::Pos<SPP::States::StateKeyType>;
    const std::array<SPP::States::StateKeyType, 3>& VelKey = Keys::Vel<SPP::States::StateKeyType>;
    const std::array<SPP::States::StateKeyType, 6>& PosVelKey = Keys::PosVel<SPP::States::StateKeyType>;

    [[nodiscard]] bool canCalculateVelocity(size_t nDoppMeas) const;

    [[nodiscard]] bool canEstimateInterFrequencyBias() const;

    void updateInterFrequencyBiases(const Observations& observations, const std::string& nameId);

    std::vector<States::StateKeyType> determineStateKeys(const std::set<SatelliteSystem>& usedSatSystems, size_t nDoppMeas, const std::string& nameId) const;

    std::vector<Meas::MeasKeyTypes> determineMeasKeys(const Observations& observations, size_t nPsrMeas, size_t nDoppMeas, const std::string& nameId) const;

    [[nodiscard]] KeyedMatrixXd<Meas::MeasKeyTypes, States::StateKeyType> calcMatrixH(const std::vector<States::StateKeyType>& stateKeys,
                                                                                      const std::vector<Meas::MeasKeyTypes>& measKeys,
                                                                                      const Observations& observations,
                                                                                      const std::string& nameId) const;

    [[nodiscard]] static KeyedMatrixXd<Meas::MeasKeyTypes, Meas::MeasKeyTypes> calcMatrixR(const std::vector<Meas::MeasKeyTypes>& measKeys,
                                                                                           const Observations& observations,
                                                                                           const std::string& nameId);

    [[nodiscard]] static KeyedVectorXd<Meas::MeasKeyTypes> calcMeasInnovation(const std::vector<Meas::MeasKeyTypes>& measKeys,
                                                                              const Observations& observations,
                                                                              const std::string& nameId);

    void assignLeastSquaresResult(const KeyedVectorXd<States::StateKeyType>& state,
                                  const KeyedMatrixXd<States::StateKeyType, States::StateKeyType>& variance,
                                  const Eigen::Vector3d& e_oldPos,
                                  size_t nParams, size_t nUniqueDopplerMeas, double dt, const std::string& nameId);

    void assignKalmanFilterResult(const KeyedVectorXd<States::StateKeyType>& state,
                                  const KeyedMatrixXd<States::StateKeyType, States::StateKeyType>& variance,
                                  const std::string& nameId);

    void computeDOPs(const std::shared_ptr<SppSolution>& sppSol,
                     const KeyedMatrixXd<Meas::MeasKeyTypes, States::StateKeyType>& H,
                     const std::string& nameId);

    Receiver _receiver{ Rover, _obsFilter.getSystemFilter().toVector() };

    EstimatorType _estimatorType = EstimatorType::WeightedLeastSquares;

    SPP::KalmanFilter _kalmanFilter;

    InsTime _lastUpdate;
 
    Eigen::Matrix3d _e_lastPositionCovarianceMatrix; 
    Eigen::Matrix3d _e_lastVelocityCovarianceMatrix; 
 
    friend void to_json(json& j, const Algorithm& obj);
    friend void from_json(const json& j, Algorithm& obj);
};

void to_json(json& j, const Algorithm& obj);
void from_json(const json& j, Algorithm& obj);
 
} // namespace SPP

[[nodiscard]] const char* to_string(SPP::Algorithm::EstimatorType estimatorType);

[[nodiscard]] const char* to_string(SPP::Algorithm::ReceiverType receiver);
 
} // namespace NAV

std::ostream& operator<<(std::ostream& os, const NAV::SPP::Algorithm::EstimatorType& obj);

std::ostream& operator<<(std::ostream& os, const NAV::SPP::Algorithm::ReceiverType& obj);
 
#ifndef DOXYGEN_IGNORE

template<>
struct fmt::formatter<NAV::SPP::Algorithm::EstimatorType> : fmt::formatter<const char*>
{
    template<typename FormatContext>
    auto format(const NAV::SPP::Algorithm::EstimatorType& type, FormatContext& ctx) const
    {
        return fmt::formatter<const char*>::format(NAV::to_string(type), ctx);
    }
};

template<>
struct fmt::formatter<NAV::SPP::Algorithm::ReceiverType> : fmt::formatter<const char*>
{
    template<typename FormatContext>
    auto format(const NAV::SPP::Algorithm::ReceiverType& type, FormatContext& ctx) const
    {
        return fmt::formatter<const char*>::format(NAV::to_string(type), ctx);
    }
};
 
#endif