ErrorModel.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 "internal/Node/Node.hpp"
 
#include "NodeData/GNSS/GnssObs.hpp"
#include "NodeData/IMU/ImuObs.hpp"
#include "NodeData/IMU/ImuObsWDelta.hpp"
#include "NodeData/State/PosVelAtt.hpp"
 
#include "util/Random/RandomNumberGenerator.hpp"
 
#include "util/Eigen.hpp"
#include <random>
#include <map>
 
namespace NAV
{
class ErrorModel : public Node
{
  public:
    ErrorModel();
    ~ErrorModel() override;
    ErrorModel(const ErrorModel&) = delete;
    ErrorModel(ErrorModel&&) = delete;
    ErrorModel& operator=(const ErrorModel&) = delete;
    ErrorModel& operator=(ErrorModel&&) = delete;
 
    [[nodiscard]] static std::string typeStatic();
 
    [[nodiscard]] std::string type() const override;
 
    [[nodiscard]] static std::string category();
 
    void guiConfig() override;
 
    [[nodiscard]] json save() const override;
 
    void restore(const json& j) override;
 
  private:
    constexpr static size_t OUTPUT_PORT_INDEX_FLOW = 0; 
    constexpr static size_t INPUT_PORT_INDEX_FLOW = 0;  
 
    enum class InputType
    {
        None,         
        ImuObs,       
        ImuObsWDelta, 
        PosVelAtt,    
        GnssObs,      
    };
 
    InputType _inputType = InputType::None;
 
    bool resetNode() override;
 
    void afterCreateLink(OutputPin& startPin, InputPin& endPin) override;
 
    void afterDeleteLink(OutputPin& startPin, InputPin& endPin) override;
 
    void receiveObs(InputPin::NodeDataQueue& queue, size_t pinIdx);
 
    [[nodiscard]] std::shared_ptr<ImuObs> receiveImuObs(const std::shared_ptr<ImuObs>& imuObs);
 
    [[nodiscard]] std::shared_ptr<ImuObsWDelta> receiveImuObsWDelta(const std::shared_ptr<ImuObsWDelta>& imuObs);
 
    [[nodiscard]] std::shared_ptr<PosVelAtt> receivePosVelAtt(const std::shared_ptr<PosVelAtt>& posVelAtt);
 
    [[nodiscard]] std::shared_ptr<GnssObs> receiveGnssObs(const std::shared_ptr<GnssObs>& gnssObs);
 
    InsTime _lastObservationTime;
    double _messageFrequency{};
 
    // #########################################################################################################################################
    //                                                                 ImuObs
    // #########################################################################################################################################
    //
    Eigen::Vector3d RandomWalkAccelerometer = Eigen::Vector3d::Zero();
 
    Eigen::Vector3d RandomWalkGyroscope = Eigen::Vector3d::Zero();
 
    Eigen::Vector3d IntegratedRandomWalkGyro = Eigen::Vector3d::Zero();
 
    Eigen::Vector3d IntegratedRandomWalkAccelerometer_velocity = Eigen::Vector3d::Zero();
 
    Eigen::Vector3d IntegratedRandomWalkAccelerometer = Eigen::Vector3d::Zero();
 
    Eigen::Vector3d IntegratedRandomWalkGyroscope_velocity = Eigen::Vector3d::Zero();
 
    Eigen::Vector3d IntegratedRandomWalkGyroscope = Eigen::Vector3d::Zero();
 
    // --------------------------------------------------------------- Offset ------------------------------------------------------------------
 
    enum class ImuAccelerometerBiasUnits
    {
        m_s2, 
    };
    ImuAccelerometerBiasUnits _imuAccelerometerBiasUnit = ImuAccelerometerBiasUnits::m_s2;
    Eigen::Vector3d _imuAccelerometerBias_p = Eigen::Vector3d::Zero();
 
    enum class ImuGyroscopeBiasUnits
    {
        rad_s, 
        deg_s, 
    };
    ImuGyroscopeBiasUnits _imuGyroscopeBiasUnit = ImuGyroscopeBiasUnits::rad_s;
    Eigen::Vector3d _imuGyroscopeBias_p = Eigen::Vector3d::Zero();
 
    // ---------------------------------------------------------------- Noise ------------------------------------------------------------------
 
    enum class ImuAccelerometerNoiseUnits
    {
        m_s2,  
        m2_s4, 
    };
    ImuAccelerometerNoiseUnits _imuAccelerometerNoiseUnit = ImuAccelerometerNoiseUnits::m_s2;
    Eigen::Vector3d _imuAccelerometerNoise = Eigen::Vector3d::Zero();
    RandomNumberGenerator _imuAccelerometerRng;
 
    enum class ImuGyroscopeNoiseUnits
    {
        rad_s,   
        deg_s,   
        rad2_s2, 
        deg2_s2, 
    };
    ImuGyroscopeNoiseUnits _imuGyroscopeNoiseUnit = ImuGyroscopeNoiseUnits::rad_s;
    Eigen::Vector3d _imuGyroscopeNoise = Eigen::Vector3d::Zero();
    RandomNumberGenerator _imuGyroscopeRng;
 
    enum class ImuAccelerometerRWUnits
    {
        m_s2_sqrts, 
        m_s2_sqrth, 
    };
    enum class ImuGyroscopeRWUnits
    {
        rad_s_sqrts, 
        rad_s_sqrth, 
        deg_s_sqrts, 
        deg_s_sqrth, 
    };
 
    enum class ImuAccelerometerIRWUnits
    {
        m_s3_sqrts, 
        m_s3_sqrth, 
    };
    enum class ImuGyroscopeIRWUnits
    {
        rad_s2_sqrts, 
        rad_s2_sqrth, 
        deg_s2_sqrts, 
        deg_s2_sqrth, 
    };
 
    ImuAccelerometerRWUnits _imuAccelerometerRWUnit = ImuAccelerometerRWUnits::m_s2_sqrts;
    Eigen::Vector3d _imuAccelerometerRW = Eigen::Vector3d::Zero();
    RandomNumberGenerator _imuAccelerometerRWRng;
 
    ImuGyroscopeRWUnits _imuGyroscopeRWUnit = ImuGyroscopeRWUnits::rad_s_sqrts;
    Eigen::Vector3d _imuGyroscopeRW = Eigen::Vector3d::Zero();
    RandomNumberGenerator _imuGyroscopeRWRng;
 
    ImuAccelerometerIRWUnits _imuAccelerometerIRWUnit = ImuAccelerometerIRWUnits::m_s3_sqrts;
    Eigen::Vector3d _imuAccelerometerIRW = Eigen::Vector3d::Zero();
    RandomNumberGenerator _imuAccelerometerIRWRng;
 
    ImuGyroscopeIRWUnits _imuGyroscopeIRWUnit = ImuGyroscopeIRWUnits::rad_s2_sqrts;
    Eigen::Vector3d _imuGyroscopeIRW = Eigen::Vector3d::Zero();
    RandomNumberGenerator _imuGyroscopeIRWRng;
 
    double _imuObsWDeltaAverageWindow = 10;
 
    // #########################################################################################################################################
    //                                                                PosVelAtt
    // #########################################################################################################################################
 
    // --------------------------------------------------------------- Offset ------------------------------------------------------------------
 
    enum class PositionBiasUnits
    {
        meter,     
        rad_rad_m, 
        deg_deg_m, 
    };
    PositionBiasUnits _positionBiasUnit = PositionBiasUnits::meter;
    Eigen::Vector3d _positionBias = Eigen::Vector3d::Zero();
 
    enum class VelocityBiasUnits
    {
        m_s, 
    };
    VelocityBiasUnits _velocityBiasUnit = VelocityBiasUnits::m_s;
    Eigen::Vector3d _velocityBias = Eigen::Vector3d::Zero();
 
    enum class AttitudeBiasUnits
    {
        rad, 
        deg, 
    };
    AttitudeBiasUnits _attitudeBiasUnit = AttitudeBiasUnits::deg;
    Eigen::Vector3d _attitudeBias = Eigen::Vector3d::Zero();
 
    // ---------------------------------------------------------------- Noise ------------------------------------------------------------------
 
    enum class PositionNoiseUnits
    {
        meter,        
        rad_rad_m,    
        deg_deg_m,    
        meter2,       
        rad2_rad2_m2, 
        deg2_deg2_m2, 
    };
 
    PositionNoiseUnits _positionNoiseUnit = PositionNoiseUnits::meter;
    Eigen::Vector3d _positionNoise = Eigen::Vector3d::Zero();
    RandomNumberGenerator _positionRng;
    enum class VelocityNoiseUnits
    {
        m_s,   
        m2_s2, 
    };
    VelocityNoiseUnits _velocityNoiseUnit = VelocityNoiseUnits::m_s;
    Eigen::Vector3d _velocityNoise = Eigen::Vector3d::Zero();
    RandomNumberGenerator _velocityRng;
 
    enum class AttitudeNoiseUnits
    {
        rad,  
        deg,  
        rad2, 
        deg2, 
    };
    AttitudeNoiseUnits _attitudeNoiseUnit = AttitudeNoiseUnits::deg;
    Eigen::Vector3d _attitudeNoise = Eigen::Vector3d::Zero();
    RandomNumberGenerator _attitudeRng;
 
    // #########################################################################################################################################
    //                                                                GnssObs
    // #########################################################################################################################################
 
    // ---------------------------------------------------------------- Noise ------------------------------------------------------------------
 
    enum class PseudorangeNoiseUnits
    {
        meter, 
    };
    PseudorangeNoiseUnits _gui_pseudorangeNoiseUnit = PseudorangeNoiseUnits::meter;
    double _gui_pseudorangeNoise{ 0.3 };
    RandomNumberGenerator _pseudorangeRng;
 
    enum class CarrierPhaseNoiseUnits
    {
        meter, 
    };
    CarrierPhaseNoiseUnits _gui_carrierPhaseNoiseUnit = CarrierPhaseNoiseUnits::meter;
    double _gui_carrierPhaseNoise{ 0.003 };
    RandomNumberGenerator _carrierPhaseRng;
 
    enum class DopplerNoiseUnits
    {
        m_s, 
    };
    DopplerNoiseUnits _gui_dopplerNoiseUnit = DopplerNoiseUnits::m_s;
    double _gui_dopplerNoise{ 0.05 };
    RandomNumberGenerator _dopplerRng;
 
    // -------------------------------------------------------------- Ambiguity ----------------------------------------------------------------
 
    std::array<int, 2> _gui_ambiguityLimits = { { -20, 20 } };
    RandomNumberGenerator _ambiguityRng;
    std::map<SatSigId, std::vector<std::pair<InsTime, int>>> _ambiguities;
 
    // ------------------------------------------------------------- Cycle-slip ----------------------------------------------------------------
 
    enum class CycleSlipFrequencyUnits
    {
        per_day,    
        per_hour,   
        per_minute, 
    };
    CycleSlipFrequencyUnits _gui_cycleSlipFrequencyUnit = CycleSlipFrequencyUnits::per_hour;
    double _gui_cycleSlipFrequency{ 0.0 };
    int _gui_cycleSlipRange = 20;
    InsTime _cycleSlipWindowStartTime;
 
    enum class CycleSlipDetectionProbabilityUnits
    {
        percent, 
    };
    CycleSlipDetectionProbabilityUnits _gui_cycleSlipDetectionProbabilityUnit = CycleSlipDetectionProbabilityUnits::percent;
    double _gui_cycleSlipDetectionProbability{ 100.0 };
 
    RandomNumberGenerator _cycleSlipRng;
 
    struct CycleSlipInfo
    {
        InsTime time;      
        SatSigId satSigId; 
        bool LLI;          
    };
    std::vector<CycleSlipInfo> _cycleSlips;
 
    Frequency _filterFreq = G01;
    Code _filterCode = Code_Default;
};
 
} // namespace NAV