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 "Navigation/GNSS/Core/SatelliteSystem.hpp"
#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;              ///< @brief Flow (ImuObs)
    constexpr static size_t INPUT_PORT_INDEX_GNSS_OBS = 1;         ///< @brief Flow (GnssObs)
    constexpr static size_t INPUT_PORT_INDEX_POS_VEL_ATT_INIT = 2; ///< @brief Flow (PosVelAtt)
    constexpr static size_t INPUT_PORT_INDEX_GNSS_NAV_INFO = 2;    ///< @brief GnssNavInfo
    constexpr static size_t OUTPUT_PORT_INDEX_SOLUTION = 0;        ///< @brief Flow (InsGnssTCKFSolution)
    constexpr static size_t OUTPUT_PORT_INDEX_x = 1;               ///< @brief x̂ State vector
    constexpr static size_t OUTPUT_PORT_INDEX_P = 2;               ///< @brief 𝐏 Error covariance matrix
    constexpr static size_t OUTPUT_PORT_INDEX_Phi = 3;             ///< @brief 𝚽 State transition matrix
    constexpr static size_t OUTPUT_PORT_INDEX_Q = 4;               ///< @brief 𝐐 System/Process noise covariance matrix
    constexpr static size_t OUTPUT_PORT_INDEX_z = 5;               ///< @brief 𝐳 Measurement vector
    constexpr static size_t OUTPUT_PORT_INDEX_H = 6;               ///< @brief 𝐇 Measurement sensitivity Matrix
    constexpr static size_t OUTPUT_PORT_INDEX_R = 7;               ///< @brief 𝐑 = 𝐸{𝐰ₘ𝐰ₘᵀ} Measurement noise covariance matrix
    constexpr static size_t OUTPUT_PORT_INDEX_K = 8;               ///< @brief 𝐊 Kalman gain matrix
 
    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{ std::vector<SatelliteSystem>{ GPS } };
 
    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::StateKeyType> _interSysErrs;
    std::vector<SPP::States::StateKeyType> _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 : uint8_t
    {
        mg_sqrtHz,   ///< [mg / √(Hz)]
        m_s2_sqrtHz, ///< [m / s^2 / √(Hz)]
    };
    StdevAccelNoiseUnits _stdevAccelNoiseUnits = StdevAccelNoiseUnits::mg_sqrtHz;
 
    Eigen::Vector3d _stdev_ra = 0.04 * Eigen::Vector3d::Ones(); // [mg/√(Hz)]
 
    // ###########################################################################################################
 
    enum class StdevGyroNoiseUnits : uint8_t
    {
        deg_hr_sqrtHz, ///< [deg / hr /√(Hz)]
        rad_s_sqrtHz,  ///< [rad / s /√(Hz)]
    };
    StdevGyroNoiseUnits _stdevGyroNoiseUnits = StdevGyroNoiseUnits::deg_hr_sqrtHz;
 
    Eigen::Vector3d _stdev_rg = 5 * Eigen::Vector3d::Ones(); // [deg/hr/√(Hz)]^2
 
    // ###########################################################################################################
 
    enum class StdevAccelBiasUnits : uint8_t
    {
        microg, ///< [µg]
        m_s2,   ///< [m / s^2]
    };
    StdevAccelBiasUnits _stdevAccelBiasUnits = StdevAccelBiasUnits::microg;
 
    Eigen::Vector3d _stdev_bad = 10 * Eigen::Vector3d::Ones(); // [µg]
 
    Eigen::Vector3d _tau_bad = 0.1 * Eigen::Vector3d::Ones();
 
    // ###########################################################################################################
 
    enum class StdevGyroBiasUnits : uint8_t
    {
        deg_h, ///< [°/h]
        rad_s, ///< [1/s]
    };
    StdevGyroBiasUnits _stdevGyroBiasUnits = StdevGyroBiasUnits::deg_h;
 
    Eigen::Vector3d _stdev_bgd = 1 * Eigen::Vector3d::Ones(); // [°/h]
 
    Eigen::Vector3d _tau_bgd = 0.1 * Eigen::Vector3d::Ones();
 
    // ###########################################################################################################
 
    enum class StdevClockPhaseUnits : uint8_t
    {
        m_sqrtHz, ///< [m / √(Hz)]
    };
    StdevClockPhaseUnits _stdevClockPhaseUnits = StdevClockPhaseUnits::m_sqrtHz;
 
    double _stdev_cp = 0; // [m / √(Hz)]
 
    // ###########################################################################################################
 
    enum class StdevClockFreqUnits : uint8_t
    {
        m_s_sqrtHz, ///< [m / s / √(Hz)]
    };
    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 : uint8_t
    // {
    //     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 : uint8_t
    // {
    //     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 : uint8_t
    {
        // WhiteNoise,     ///< White noise
        // RandomConstant, ///< Random constant
 
        RandomWalk,   ///< Random Walk
        GaussMarkov1, ///< Gauss-Markov 1st Order
 
        // GaussMarkov2,   ///< Gauss-Markov 2nd Order
        // GaussMarkov3,   ///< Gauss-Markov 3rd Order
    };
 
    RandomProcess _randomProcessAccel = RandomProcess::RandomWalk;
    RandomProcess _randomProcessGyro = RandomProcess::RandomWalk;
 
    // ###########################################################################################################
 
    enum class InitCovarianceClockPhaseUnit : uint8_t
    {
        m2, ///< Variance [m²]
        s2, ///< Variance [s²]
        m,  ///< Standard deviation [m]
        s   ///< Standard deviation [s]
    };
    InitCovarianceClockPhaseUnit _initCovariancePhaseUnit = InitCovarianceClockPhaseUnit::m;
 
    double _initCovariancePhase{ 5 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceClockFreqUnit : uint8_t
    {
        m2_s2, ///< Variance [m²/s²]
        m_s,   ///< Standard deviation [m/s]
    };
    InitCovarianceClockFreqUnit _initCovarianceFreqUnit = InitCovarianceClockFreqUnit::m_s;
 
    double _initCovarianceFreq{ 5 };
 
    // ###########################################################################################################
 
    enum class InitCovariancePositionUnit : uint8_t
    {
        rad2_rad2_m2, ///< Variance LatLonAlt^2 [rad^2, rad^2, m^2]
        rad_rad_m,    ///< Standard deviation LatLonAlt [rad, rad, m]
        meter2,       ///< Variance NED [m^2, m^2, m^2]
        meter,        ///< Standard deviation NED [m, m, m]
    };
    InitCovariancePositionUnit _initCovariancePositionUnit = InitCovariancePositionUnit::meter;
 
    Eigen::Vector3d _initCovariancePosition{ 100, 100, 100 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceVelocityUnit : uint8_t
    {
        m2_s2, ///< Variance [m^2/s^2]
        m_s,   ///< Standard deviation [m/s]
    };
    InitCovarianceVelocityUnit _initCovarianceVelocityUnit = InitCovarianceVelocityUnit::m_s;
 
    Eigen::Vector3d _initCovarianceVelocity{ 10, 10, 10 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceAttitudeAnglesUnit : uint8_t
    {
        rad2, ///< Variance [rad^2]
        deg2, ///< Variance [deg^2]
        rad,  ///< Standard deviation [rad]
        deg,  ///< Standard deviation [deg]
    };
    InitCovarianceAttitudeAnglesUnit _initCovarianceAttitudeAnglesUnit = InitCovarianceAttitudeAnglesUnit::deg;
 
    Eigen::Vector3d _initCovarianceAttitudeAngles{ 10, 10, 10 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceBiasAccelUnit : uint8_t
    {
        m2_s4, ///< Variance [m^2/s^4]
        m_s2,  ///< Standard deviation [m/s^2]
    };
    InitCovarianceBiasAccelUnit _initCovarianceBiasAccelUnit = InitCovarianceBiasAccelUnit::m_s2;
 
    Eigen::Vector3d _initCovarianceBiasAccel{ 1, 1, 1 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceBiasGyroUnit : uint8_t
    {
        rad2_s2, ///< Variance [rad²/s²]
        deg2_s2, ///< Variance [deg²/s²]
        rad_s,   ///< Standard deviation [rad/s]
        deg_s,   ///< Standard deviation [deg/s]
    };
    InitCovarianceBiasGyroUnit _initCovarianceBiasGyroUnit = InitCovarianceBiasGyroUnit::deg_s;
 
    Eigen::Vector3d _initCovarianceBiasGyro{ 0.5, 0.5, 0.5 };
 
    // ###########################################################################################################
 
    enum class PhiCalculationAlgorithm : uint8_t
    {
        Exponential, ///< Van-Loan
        Taylor,      ///< Taylor
    };
    PhiCalculationAlgorithm _phiCalculationAlgorithm = PhiCalculationAlgorithm::Taylor;
 
    int _phiCalculationTaylorOrder = 2;
 
    enum class QCalculationAlgorithm : uint8_t
    {
        VanLoan, ///< Van-Loan
        Taylor1, ///< Taylor
    };
    QCalculationAlgorithm _qCalculationAlgorithm = QCalculationAlgorithm::Taylor1;
 
    // ###########################################################################################################
 
    enum class InitBiasAccelUnit : uint8_t
    {
        m_s2, ///< acceleration [m/s^2]
    };
    InitBiasAccelUnit _initBiasAccelUnit = InitBiasAccelUnit::m_s2;
 
    Eigen::Vector3d _initBiasAccel{ 0.0, 0.0, 0.0 };
 
    // ###########################################################################################################
 
    enum class InitBiasGyroUnit : uint8_t
    {
        rad_s, ///< angular rate [rad/s]
        deg_s, ///< angular rate [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
 
*/