TightlyCoupledKF.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 "Navigation/GNSS/Core/Frequency.hpp"
#include "Navigation/GNSS/Core/Code.hpp"
#include "Navigation/Time/InsTime.hpp"
#include "NodeData/State/PosVelAtt.hpp"
#include "NodeData/GNSS/GnssObs.hpp"
#include "Navigation/INS/InertialIntegrator.hpp"
#include "Navigation/GNSS/Positioning/ReceiverClock.hpp"
#include "Navigation/Atmosphere/Ionosphere/Ionosphere.hpp"
#include "Navigation/Atmosphere/Troposphere/Troposphere.hpp"
#include "Navigation/GNSS/Positioning/SPP/Algorithm.hpp"
#include "NodeData/State/InsGnssTCKFSolution.hpp"
#include "NodeData/IMU/ImuObs.hpp"
 
#include "Navigation/Math/KalmanFilter.hpp"
#include "Navigation/Transformations/Units.hpp"
 
namespace NAV
{
class TightlyCoupledKF : public Node
{
  public:
    TightlyCoupledKF();
    ~TightlyCoupledKF() override;
    TightlyCoupledKF(const TightlyCoupledKF&) = delete;
    TightlyCoupledKF(TightlyCoupledKF&&) = delete;
    TightlyCoupledKF& operator=(const TightlyCoupledKF&) = delete;
    TightlyCoupledKF& operator=(TightlyCoupledKF&&) = 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 INPUT_PORT_INDEX_IMU = 0;              
    constexpr static size_t INPUT_PORT_INDEX_GNSS_OBS = 1;         
    constexpr static size_t INPUT_PORT_INDEX_POS_VEL_ATT_INIT = 2; 
    constexpr static size_t INPUT_PORT_INDEX_GNSS_NAV_INFO = 2;    
    constexpr static size_t OUTPUT_PORT_INDEX_SOLUTION = 0;        
    constexpr static size_t OUTPUT_PORT_INDEX_x = 1;               
    constexpr static size_t OUTPUT_PORT_INDEX_P = 2;               
    constexpr static size_t OUTPUT_PORT_INDEX_Phi = 3;             
    constexpr static size_t OUTPUT_PORT_INDEX_Q = 4;               
    constexpr static size_t OUTPUT_PORT_INDEX_z = 5;               
    constexpr static size_t OUTPUT_PORT_INDEX_H = 6;               
    constexpr static size_t OUTPUT_PORT_INDEX_R = 7;               
    constexpr static size_t OUTPUT_PORT_INDEX_K = 8;               
 
    bool initialize() override;
 
    void deinitialize() override;
 
    void invokeCallbackWithPosVelAtt(const PosVelAtt& posVelAtt);
 
    void recvImuObservation(InputPin::NodeDataQueue& queue, size_t pinIdx);
 
    void recvGnssObs(InputPin::NodeDataQueue& queue, size_t pinIdx);
 
    void recvPosVelAttInit(InputPin::NodeDataQueue& queue, size_t pinIdx);
 
    void tightlyCoupledPrediction(const std::shared_ptr<const PosVelAtt>& inertialNavSol, double tau_i, const ImuPos& imuPos);
 
    void tightlyCoupledUpdate(const std::shared_ptr<const GnssObs>& gnssObservation);
 
    void addKalmanMatricesPins();
 
    void removeKalmanMatricesPins();
 
    void updateExternalPvaInitPin();
 
    int _dragAndDropPinIndex = -1;
    size_t _nNavInfoPins = 1;
    void updateNumberOfInputPins();
 
    InertialIntegrator _inertialIntegrator;
    bool _preferAccelerationOverDeltaMeasurements = false;
 
    std::shared_ptr<const ImuObs> _lastImuObs = nullptr;
 
    std::array<double, 3> _initalRollPitchYaw{};
    bool _initializeStateOverExternalPin{};
    InsTime _externalInitTime;
 
    ReceiverClock _recvClk;
 
    Frequency _filterFreq = G01;
    Code _filterCode = Code_Default;
    std::vector<SatId> _excludedSatellites;
    double _elevationMask = static_cast<double>(15.0_deg);
 
    IonosphereModel _ionosphereModel = IonosphereModel::Klobuchar;
 
    TroposphereModelSelection _troposphereModels;
 
    std::vector<SPP::States::StateKeyTypes> _interSysErrs{};
    std::vector<SPP::States::StateKeyTypes> _interSysDrifts{};
 
    InsTime _lastEpochTime; // TODO: Remove?
 
    KalmanFilter _kalmanFilter{ 17, 8 };
 
    // ###########################################################################################################
    //                                               GUI Settings
    // ###########################################################################################################
 
    bool _showKalmanFilterOutputPins = false;
 
    bool _checkKalmanMatricesRanks = true;
 
    // ###########################################################################################################
    //                                                Parameters
    // ###########################################################################################################
 
    Eigen::Vector3d _b_leverArm_InsGnss{ 0.0, 0.0, 0.0 };
 
    // ###########################################################################################################
 
    enum class StdevAccelNoiseUnits
    {
        mg_sqrtHz,   
        m_s2_sqrtHz, 
    };
    StdevAccelNoiseUnits _stdevAccelNoiseUnits = StdevAccelNoiseUnits::mg_sqrtHz;
 
    Eigen::Vector3d _stdev_ra = 0.04 /* [mg/√(Hz)] */ * Eigen::Vector3d::Ones();
 
    // ###########################################################################################################
 
    enum class StdevGyroNoiseUnits
    {
        deg_hr_sqrtHz, 
        rad_s_sqrtHz,  
    };
    StdevGyroNoiseUnits _stdevGyroNoiseUnits = StdevGyroNoiseUnits::deg_hr_sqrtHz;
 
    Eigen::Vector3d _stdev_rg = 5 /* [deg/hr/√(Hz)]^2 */ * Eigen::Vector3d::Ones();
 
    // ###########################################################################################################
 
    enum class StdevAccelBiasUnits
    {
        microg, 
        m_s2,   
    };
    StdevAccelBiasUnits _stdevAccelBiasUnits = StdevAccelBiasUnits::microg;
 
    Eigen::Vector3d _stdev_bad = 10 /* [µg] */ * Eigen::Vector3d::Ones();
 
    Eigen::Vector3d _tau_bad = 0.1 * Eigen::Vector3d::Ones();
 
    // ###########################################################################################################
 
    enum class StdevGyroBiasUnits
    {
        deg_h, 
        rad_s, 
    };
    StdevGyroBiasUnits _stdevGyroBiasUnits = StdevGyroBiasUnits::deg_h;
 
    Eigen::Vector3d _stdev_bgd = 1 /* [°/h] */ * Eigen::Vector3d::Ones();
 
    Eigen::Vector3d _tau_bgd = 0.1 * Eigen::Vector3d::Ones();
 
    // ###########################################################################################################
 
    enum class StdevClockPhaseUnits
    {
        m_sqrtHz, 
    };
    StdevClockPhaseUnits _stdevClockPhaseUnits = StdevClockPhaseUnits::m_sqrtHz;
 
    double _stdev_cp = 0 /* [m / √(Hz)] */;
 
    // ###########################################################################################################
 
    enum class StdevClockFreqUnits
    {
        m_s_sqrtHz, 
    };
    StdevClockFreqUnits _stdevClockFreqUnits = StdevClockFreqUnits::m_s_sqrtHz;
 
    double _stdev_cf = 5 /* [m / s / √(Hz)] */;
 
    // ###########################################################################################################
 
    // TODO: Replace with GNSS Measurement Error Model (see SPP node)
    // /// Possible Units for the Standard deviation of the pseudorange measurement
    // enum class GnssMeasurementUncertaintyPseudorangeUnit
    // {
    //     meter2, ///< Variance [m²]
    //     meter,  ///< Standard deviation [m]
    // };
    // /// Gui selection for the Unit of the input gnssMeasurementUncertaintyPseudorangeUnit parameter
    // GnssMeasurementUncertaintyPseudorangeUnit _gnssMeasurementUncertaintyPseudorangeUnit = GnssMeasurementUncertaintyPseudorangeUnit::meter;
 
    // /// @brief GUI selection of the GNSS pseudorange measurement uncertainty (standard deviation σ or Variance σ²).
    // double _gnssMeasurementUncertaintyPseudorange = 5 /* [m] */;
 
    // // ###########################################################################################################
 
    // /// Possible Units for the Standard deviation of the pseudorange-rate measurement
    // enum class GnssMeasurementUncertaintyPseudorangeRateUnit
    // {
    //     m2_s2, ///< Variance [m²/s²]
    //     m_s,   ///< Standard deviation [m/s]
    // };
    // /// Gui selection for the Unit of the input gnssMeasurementUncertaintyPseudorangeRateUnit parameter
    // GnssMeasurementUncertaintyPseudorangeRateUnit _gnssMeasurementUncertaintyPseudorangeRateUnit = GnssMeasurementUncertaintyPseudorangeRateUnit::m_s;
 
    // /// @brief GUI selection of the GNSS pseudorange-rate measurement uncertainty (standard deviation σ or Variance σ²).
    // double _gnssMeasurementUncertaintyPseudorangeRate = 5 /* [m/s] */;
 
    // ###########################################################################################################
 
    enum class RandomProcess
    {
        // WhiteNoise,     ///< White noise
        // RandomConstant, ///< Random constant
 
        RandomWalk,   
        GaussMarkov1, 
 
        // GaussMarkov2,   ///< Gauss-Markov 2nd Order
        // GaussMarkov3,   ///< Gauss-Markov 3rd Order
    };
 
    RandomProcess _randomProcessAccel = RandomProcess::RandomWalk;
    RandomProcess _randomProcessGyro = RandomProcess::RandomWalk;
 
    // ###########################################################################################################
 
    enum class InitCovarianceClockPhaseUnit
    {
        m2, 
        s2, 
        m,  
        s   
    };
    InitCovarianceClockPhaseUnit _initCovariancePhaseUnit = InitCovarianceClockPhaseUnit::m;
 
    double _initCovariancePhase{ 5 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceClockFreqUnit
    {
        m2_s2, 
        m_s,   
    };
    InitCovarianceClockFreqUnit _initCovarianceFreqUnit = InitCovarianceClockFreqUnit::m_s;
 
    double _initCovarianceFreq{ 5 };
 
    // ###########################################################################################################
 
    enum class InitCovariancePositionUnit
    {
        rad2_rad2_m2, 
        rad_rad_m,    
        meter2,       
        meter,        
    };
    InitCovariancePositionUnit _initCovariancePositionUnit = InitCovariancePositionUnit::meter;
 
    Eigen::Vector3d _initCovariancePosition{ 100, 100, 100 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceVelocityUnit
    {
        m2_s2, 
        m_s,   
    };
    InitCovarianceVelocityUnit _initCovarianceVelocityUnit = InitCovarianceVelocityUnit::m_s;
 
    Eigen::Vector3d _initCovarianceVelocity{ 10, 10, 10 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceAttitudeAnglesUnit
    {
        rad2, 
        deg2, 
        rad,  
        deg,  
    };
    InitCovarianceAttitudeAnglesUnit _initCovarianceAttitudeAnglesUnit = InitCovarianceAttitudeAnglesUnit::deg;
 
    Eigen::Vector3d _initCovarianceAttitudeAngles{ 10, 10, 10 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceBiasAccelUnit
    {
        m2_s4, 
        m_s2,  
    };
    InitCovarianceBiasAccelUnit _initCovarianceBiasAccelUnit = InitCovarianceBiasAccelUnit::m_s2;
 
    Eigen::Vector3d _initCovarianceBiasAccel{ 1, 1, 1 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceBiasGyroUnit
    {
        rad2_s2, 
        deg2_s2, 
        rad_s,   
        deg_s,   
    };
    InitCovarianceBiasGyroUnit _initCovarianceBiasGyroUnit = InitCovarianceBiasGyroUnit::deg_s;
 
    Eigen::Vector3d _initCovarianceBiasGyro{ 0.5, 0.5, 0.5 };
 
    // ###########################################################################################################
 
    enum class PhiCalculationAlgorithm
    {
        Exponential, 
        Taylor,      
    };
    PhiCalculationAlgorithm _phiCalculationAlgorithm = PhiCalculationAlgorithm::Taylor;
 
    int _phiCalculationTaylorOrder = 2;
 
    enum class QCalculationAlgorithm
    {
        VanLoan, 
        Taylor1, 
    };
    QCalculationAlgorithm _qCalculationAlgorithm = QCalculationAlgorithm::Taylor1;
 
    // ###########################################################################################################
 
    enum class InitBiasAccelUnit
    {
        m_s2, 
    };
    InitBiasAccelUnit _initBiasAccelUnit = InitBiasAccelUnit::m_s2;
 
    Eigen::Vector3d _initBiasAccel{ 0.0, 0.0, 0.0 };
 
    // ###########################################################################################################
 
    enum class InitBiasGyroUnit
    {
        rad_s, 
        deg_s, 
    };
    InitBiasGyroUnit _initBiasGyroUnit = InitBiasGyroUnit::deg_s;
 
    Eigen::Vector3d _initBiasGyro{ 0.0, 0.0, 0.0 };
 
    // ###########################################################################################################
    //                                                Prediction
    // ###########################################################################################################
 
    // ------------------------------------------- System matrix 𝐅 ----------------------------------------------
 
    [[nodiscard]] Eigen::Matrix<double, 17, 17> n_systemMatrix_F(const Eigen::Quaterniond& n_Quat_b,
                                                                 const Eigen::Vector3d& b_specForce_ib,
                                                                 const Eigen::Vector3d& n_omega_in,
                                                                 const Eigen::Vector3d& n_velocity,
                                                                 const Eigen::Vector3d& lla_position,
                                                                 double R_N,
                                                                 double R_E,
                                                                 double g_0,
                                                                 double r_eS_e,
                                                                 const Eigen::Vector3d& tau_bad,
                                                                 const Eigen::Vector3d& tau_bgd) const;
 
    [[nodiscard]] Eigen::Matrix<double, 17, 17> e_systemMatrix_F(const Eigen::Quaterniond& e_Quat_b,
                                                                 const Eigen::Vector3d& b_specForce_ib,
                                                                 const Eigen::Vector3d& e_position,
                                                                 const Eigen::Vector3d& e_gravitation,
                                                                 double r_eS_e,
                                                                 const Eigen::Vector3d& e_omega_ie,
                                                                 const Eigen::Vector3d& tau_bad,
                                                                 const Eigen::Vector3d& tau_bgd) const;
 
    // ----------------------------- Noise input matrix 𝐆 & Noise scale matrix 𝐖 -------------------------------
    // ----------------------------------- System noise covariance matrix 𝐐 -------------------------------------
 
    [[nodiscard]] static Eigen::Matrix<double, 17, 14> noiseInputMatrix_G(const Eigen::Quaterniond& ien_Quat_b);
 
    [[nodiscard]] Eigen::Matrix<double, 14, 14> noiseScaleMatrix_W(const Eigen::Vector3d& sigma2_ra, const Eigen::Vector3d& sigma2_rg,
                                                                   const Eigen::Vector3d& sigma2_bad, const Eigen::Vector3d& sigma2_bgd,
                                                                   const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
                                                                   const double& sigma2_cPhi, const double& sigma2_cf);
 
    [[nodiscard]] static Eigen::Matrix<double, 17, 17> n_systemNoiseCovarianceMatrix_Q(const Eigen::Vector3d& sigma2_ra, const Eigen::Vector3d& sigma2_rg,
                                                                                       const Eigen::Vector3d& sigma2_bad, const Eigen::Vector3d& sigma2_bgd,
                                                                                       const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
                                                                                       const double& sigma2_cPhi, const double& sigma2_cf,
                                                                                       const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p,
                                                                                       const Eigen::Matrix3d& n_Dcm_b, const double& tau_s);
 
    [[nodiscard]] static Eigen::Matrix<double, 17, 17> e_systemNoiseCovarianceMatrix_Q(const Eigen::Vector3d& sigma2_ra, const Eigen::Vector3d& sigma2_rg,
                                                                                       const Eigen::Vector3d& sigma2_bad, const Eigen::Vector3d& sigma2_bgd,
                                                                                       const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
                                                                                       const double& sigma2_cPhi, const double& sigma2_cf,
                                                                                       const Eigen::Matrix3d& e_F_21,
                                                                                       const Eigen::Matrix3d& e_Dcm_b, const double& tau_s);
 
    // --------------------------------------- Error covariance matrix P -----------------------------------------
 
    [[nodiscard]] Eigen::Matrix<double, 17, 17> initialErrorCovarianceMatrix_P0(const Eigen::Vector3d& variance_angles,
                                                                                const Eigen::Vector3d& variance_vel,
                                                                                const Eigen::Vector3d& variance_pos,
                                                                                const Eigen::Vector3d& variance_accelBias,
                                                                                const Eigen::Vector3d& variance_gyroBias,
                                                                                const double& variance_clkPhase,
                                                                                const double& variance_clkFreq) const;
 
    // ###########################################################################################################
    //                                                  Update
    // ###########################################################################################################
 
    [[nodiscard]] static Eigen::MatrixXd n_measurementMatrix_H(const double& R_N,
                                                               const double& R_E,
                                                               const Eigen::Vector3d& lla_position,
                                                               const std::vector<Eigen::Vector3d>& n_lineOfSightUnitVectors,
                                                               std::vector<double>& pseudoRangeRateObservations);
 
    [[nodiscard]] static Eigen::MatrixXd measurementNoiseCovariance_R(const double& sigma_rhoZ,
                                                                      const double& sigma_rZ,
                                                                      const std::vector<double>& satElevation);
 
    [[nodiscard]] static double sigma2(const double& satElevation,
                                       const double& sigma_Z,
                                       const double& sigma_C,
                                       const double& sigma_A,
                                       const double& CN0,
                                       const double& rangeAccel);
 
    [[nodiscard]] static Eigen::MatrixXd measurementInnovation_dz(const std::vector<double>& pseudoRangeObservations,
                                                                  const std::vector<double>& pseudoRangeEstimates,
                                                                  const std::vector<double>& pseudoRangeRateObservations,
                                                                  const std::vector<double>& pseudoRangeRateEstimates);
};
 
} // namespace NAV