LooselyCoupledKF.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/Time/InsTime.hpp"
#include "Navigation/INS/InertialIntegrator.hpp"
 
#include "NodeData/IMU/ImuObs.hpp"
#include "NodeData/State/PosVelAtt.hpp"
 
#include "Navigation/Math/KeyedKalmanFilter.hpp"
 
namespace NAV
{
class LooselyCoupledKF : public Node
{
  public:
    LooselyCoupledKF();
    ~LooselyCoupledKF() override;
    LooselyCoupledKF(const LooselyCoupledKF&) = delete;
    LooselyCoupledKF(LooselyCoupledKF&&) = delete;
    LooselyCoupledKF& operator=(const LooselyCoupledKF&) = delete;
    LooselyCoupledKF& operator=(LooselyCoupledKF&&) = 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;
 
    enum KFStates : uint8_t
    {
        Roll = 0,      
        Pitch = 1,     
        Yaw = 2,       
        VelN = 3,      
        VelE = 4,      
        VelD = 5,      
        PosLat = 6,    
        PosLon = 7,    
        PosAlt = 8,    
        AccBiasX = 9,  
        AccBiasY = 10, 
        AccBiasZ = 11, 
        GyrBiasX = 12, 
        GyrBiasY = 13, 
        GyrBiasZ = 14, 
 
        Psi_eb_1 = Roll,  
        Psi_eb_2 = Pitch, 
        Psi_eb_3 = Yaw,   
        VelX = VelN,      
        VelY = VelE,      
        VelZ = VelD,      
        PosX = PosLat,    
        PosY = PosLon,    
        PosZ = PosAlt,    
    };
 
    enum KFMeas : uint8_t
    {
        dPosLat = 0, 
        dPosLon = 1, 
        dPosAlt = 2, 
        dVelN = 3,   
        dVelE = 4,   
        dVelD = 5,   
 
        dPosX = dPosLat, 
        dPosY = dPosLon, 
        dPosZ = dPosAlt, 
        dVelX = dVelN,   
        dVelY = dVelE,   
        dVelZ = dVelD,   
    };
 
  private:
    constexpr static size_t INPUT_PORT_INDEX_IMU = 0;              
    constexpr static size_t INPUT_PORT_INDEX_GNSS = 1;             
    constexpr static size_t INPUT_PORT_INDEX_POS_VEL_ATT_INIT = 2; 
    constexpr static size_t OUTPUT_PORT_INDEX_SOLUTION = 0;        
 
    bool initialize() override;
 
    void deinitialize() override;
 
    void invokeCallbackWithPosVelAtt(const PosVelAtt& posVelAtt);
 
    void recvImuObservation(InputPin::NodeDataQueue& queue, size_t pinIdx);
 
    void recvPosVelObservation(InputPin::NodeDataQueue& queue, size_t pinIdx);
 
    void recvPosVelAttInit(InputPin::NodeDataQueue& queue, size_t pinIdx);
 
    void looselyCoupledPrediction(const std::shared_ptr<const PosVelAtt>& inertialNavSol, double tau_i, const ImuPos& imuPos);
 
    void looselyCoupledUpdate(const std::shared_ptr<const PosVel>& posVelObs);
 
    void updateExternalPvaInitPin();
 
    InertialIntegrator _inertialIntegrator;
    bool _preferAccelerationOverDeltaMeasurements = false;
 
    std::shared_ptr<const ImuObs> _lastImuObs = nullptr;
 
    std::array<double, 3> _initalRollPitchYaw{};
    bool _initializeStateOverExternalPin{};
    InsTime _externalInitTime;
 
    bool _initialSensorBiasesApplied = false;
 
    inline static const std::vector<KFStates> States = { KFStates::Roll, KFStates::Pitch, KFStates::Yaw,
                                                         KFStates::VelN, KFStates::VelE, KFStates::VelD,
                                                         KFStates::PosLat, KFStates::PosLon, KFStates::PosAlt,
                                                         KFStates::AccBiasX, KFStates::AccBiasY, KFStates::AccBiasZ,
                                                         KFStates::GyrBiasX, KFStates::GyrBiasY, KFStates::GyrBiasZ };
    inline static const std::vector<KFStates> KFPos = { KFStates::PosLat, KFStates::PosLon, KFStates::PosAlt };
    inline static const std::vector<KFStates> KFVel = { KFStates::VelN, KFStates::VelE, KFStates::VelD };
    inline static const std::vector<KFStates> KFAtt = { KFStates::Roll, KFStates::Pitch, KFStates::Yaw };
    inline static const std::vector<KFStates> KFAccBias = { KFStates::AccBiasX, KFStates::AccBiasY, KFStates::AccBiasZ };
    inline static const std::vector<KFStates> KFGyrBias = { KFStates::GyrBiasX, KFStates::GyrBiasY, KFStates::GyrBiasZ };
 
    inline static const std::vector<KFStates> KFPosVel = { KFStates::PosLat, KFStates::PosLon, KFStates::PosAlt,
                                                           KFStates::VelN, KFStates::VelE, KFStates::VelD };
 
    inline static const std::vector<KFMeas> Meas = { KFMeas::dPosLat, KFMeas::dPosLon, KFMeas::dPosAlt, KFMeas::dVelN, KFMeas::dVelE, KFMeas::dVelD };
    inline static const std::vector<KFMeas> dPos = { KFMeas::dPosLat, KFMeas::dPosLon, KFMeas::dPosAlt };
    inline static const std::vector<KFMeas> dVel = { KFMeas::dVelN, KFMeas::dVelE, KFMeas::dVelD };
 
    KeyedKalmanFilterD<KFStates, KFMeas> _kalmanFilter{ States, Meas };
 
    // #########################################################################################################################################
    //                                                              GUI settings
    // #########################################################################################################################################
 
    bool _checkKalmanMatricesRanks = true;
 
    // ###########################################################################################################
    //                                                Parameters
    // ###########################################################################################################
 
    Eigen::Vector3d _b_leverArm_InsGnss{ 0.0, 0.0, 0.0 };
 
    // ###########################################################################################################
 
    enum class StdevAccelNoiseUnits : uint8_t
    {
        mg_sqrtHz,   
        m_s2_sqrtHz, 
    };
    StdevAccelNoiseUnits _stdevAccelNoiseUnits = StdevAccelNoiseUnits::mg_sqrtHz;
 
    Eigen::Vector3d _stdev_ra = 0.04 /* [mg/√(Hz)] */ * Eigen::Vector3d::Ones();
 
    // ###########################################################################################################
 
    enum class StdevGyroNoiseUnits : uint8_t
    {
        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 : uint8_t
    {
        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 : uint8_t
    {
        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 RandomProcess : uint8_t
    {
        // 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 GnssMeasurementUncertaintyPositionUnit : uint8_t
    {
        rad2_rad2_m2, 
        rad_rad_m,    
        meter2,       
        meter,        
    };
    GnssMeasurementUncertaintyPositionUnit _gnssMeasurementUncertaintyPositionUnit = GnssMeasurementUncertaintyPositionUnit::meter;
 
    Eigen::Vector3d _gnssMeasurementUncertaintyPosition{ 0.3, 0.3, 0.3 * 3 };
 
    bool _gnssMeasurementUncertaintyPositionOverride = false;
 
    // ###########################################################################################################
 
    enum class GnssMeasurementUncertaintyVelocityUnit : uint8_t
    {
        m2_s2, 
        m_s,   
    };
    GnssMeasurementUncertaintyVelocityUnit _gnssMeasurementUncertaintyVelocityUnit = GnssMeasurementUncertaintyVelocityUnit::m_s;
 
    Eigen::Vector3d _gnssMeasurementUncertaintyVelocity{ 0.5, 0.5, 0.5 };
 
    bool _gnssMeasurementUncertaintyVelocityOverride = false;
 
    // ###########################################################################################################
 
    enum class InitCovariancePositionUnit : uint8_t
    {
        rad2_rad2_m2, 
        rad_rad_m,    
        meter2,       
        meter,        
    };
    InitCovariancePositionUnit _initCovariancePositionUnit = InitCovariancePositionUnit::meter;
 
    Eigen::Vector3d _initCovariancePosition{ 100.0, 100.0, 100.0 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceVelocityUnit : uint8_t
    {
        m2_s2, 
        m_s,   
    };
    InitCovarianceVelocityUnit _initCovarianceVelocityUnit = InitCovarianceVelocityUnit::m_s;
 
    Eigen::Vector3d _initCovarianceVelocity{ 10.0, 10.0, 10.0 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceAttitudeAnglesUnit : uint8_t
    {
        rad2, 
        deg2, 
        rad,  
        deg,  
    };
    InitCovarianceAttitudeAnglesUnit _initCovarianceAttitudeAnglesUnit = InitCovarianceAttitudeAnglesUnit::deg;
 
    Eigen::Vector3d _initCovarianceAttitudeAngles{ 10.0, 10.0, 10.0 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceBiasAccelUnit : uint8_t
    {
        m2_s4, 
        m_s2,  
    };
    InitCovarianceBiasAccelUnit _initCovarianceBiasAccelUnit = InitCovarianceBiasAccelUnit::m_s2;
 
    Eigen::Vector3d _initCovarianceBiasAccel{ 1.0, 1.0, 1.0 };
 
    // ###########################################################################################################
 
    enum class InitCovarianceBiasGyroUnit : uint8_t
    {
        rad2_s2, 
        deg2_s2, 
        rad_s,   
        deg_s,   
    };
    InitCovarianceBiasGyroUnit _initCovarianceBiasGyroUnit = InitCovarianceBiasGyroUnit::deg_s;
 
    Eigen::Vector3d _initCovarianceBiasGyro{ 0.5, 0.5, 0.5 };
 
    // ###########################################################################################################
 
    enum class InitBiasAccelUnit : uint8_t
    {
        m_s2, 
    };
    InitBiasAccelUnit _initBiasAccelUnit = InitBiasAccelUnit::m_s2;
 
    Eigen::Vector3d _initBiasAccel{ 0.0, 0.0, 0.0 };
 
    // ###########################################################################################################
 
    enum class InitBiasGyroUnit : uint8_t
    {
        rad_s, 
        deg_s, 
    };
    InitBiasGyroUnit _initBiasGyroUnit = InitBiasGyroUnit::deg_s;
 
    Eigen::Vector3d _initBiasGyro{ 0.0, 0.0, 0.0 };
 
    // ###########################################################################################################
 
    enum class PhiCalculationAlgorithm : uint8_t
    {
        Exponential, 
        Taylor,      
    };
    PhiCalculationAlgorithm _phiCalculationAlgorithm = PhiCalculationAlgorithm::Taylor;
 
    int _phiCalculationTaylorOrder = 2;
 
    enum class QCalculationAlgorithm : uint8_t
    {
        VanLoan, 
        Taylor1, 
    };
    QCalculationAlgorithm _qCalculationAlgorithm = QCalculationAlgorithm::Taylor1;
 
    // ###########################################################################################################
    //                                                Prediction
    // ###########################################################################################################
 
    // ###########################################################################################################
    //                                             System matrix 𝐅
    // ###########################################################################################################
 
    [[nodiscard]] KeyedMatrix<double, KFStates, KFStates, 15, 15> 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]] KeyedMatrix<double, KFStates, KFStates, 15, 15> 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 KeyedMatrix<double, KFStates, KFStates, 15, 15> noiseInputMatrix_G(const Eigen::Quaterniond& ien_Quat_b);
 
    [[nodiscard]] Eigen::Matrix<double, 15, 15> noiseScaleMatrix_W(const Eigen::Vector3d& sigma_ra, const Eigen::Vector3d& sigma_rg,
                                                                   const Eigen::Vector3d& sigma_bad, const Eigen::Vector3d& sigma_bgd,
                                                                   const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd);
 
    [[nodiscard]] static KeyedMatrix<double, KFStates, KFStates, 15, 15> 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 Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p,
                                                                                                         const Eigen::Matrix3d& n_Dcm_b, const double& tau_s);
 
    [[nodiscard]] static KeyedMatrix<double, KFStates, KFStates, 15, 15> 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 Eigen::Matrix3d& e_F_21,
                                                                                                         const Eigen::Matrix3d& e_Dcm_b, const double& tau_s);
 
    // ###########################################################################################################
    //                                         Error covariance matrix P
    // ###########################################################################################################
 
    [[nodiscard]] KeyedMatrix<double, KFStates, KFStates, 15, 15> 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;
 
    // ###########################################################################################################
    //                                                Correction
    // ###########################################################################################################
 
    [[nodiscard]] static KeyedMatrix<double, KFMeas, KFStates, 6, 15> n_measurementMatrix_H(const Eigen::Matrix3d& T_rn_p,
                                                                                            const Eigen::Matrix3d& n_Dcm_b,
                                                                                            const Eigen::Vector3d& b_omega_ib,
                                                                                            const Eigen::Vector3d& b_leverArm_InsGnss,
                                                                                            const Eigen::Matrix3d& n_Omega_ie);
 
    [[nodiscard]] static KeyedMatrix<double, KFMeas, KFStates, 6, 15> e_measurementMatrix_H(const Eigen::Matrix3d& e_Dcm_b,
                                                                                            const Eigen::Vector3d& b_omega_ib,
                                                                                            const Eigen::Vector3d& b_leverArm_InsGnss,
                                                                                            const Eigen::Matrix3d& e_Omega_ie);
 
    [[nodiscard]] static KeyedMatrix<double, KFMeas, KFMeas, 6, 6> n_measurementNoiseCovariance_R(const Eigen::Vector3d& gnssVarianceLatLonAlt,
                                                                                                  const Eigen::Vector3d& gnssVarianceVelocity);
 
    [[nodiscard]] static KeyedMatrix<double, KFMeas, KFMeas, 6, 6> e_measurementNoiseCovariance_R(const Eigen::Vector3d& gnssVariancePosition,
                                                                                                  const Eigen::Vector3d& gnssVarianceVelocity);
 
    [[nodiscard]] static KeyedVector<double, KFMeas, 6> n_measurementInnovation_dz(const Eigen::Vector3d& lla_positionMeasurement, const Eigen::Vector3d& lla_positionEstimate,
                                                                                   const Eigen::Vector3d& n_velocityMeasurement, const Eigen::Vector3d& n_velocityEstimate,
                                                                                   const Eigen::Matrix3d& T_rn_p, const Eigen::Quaterniond& n_Quat_b, const Eigen::Vector3d& b_leverArm_InsGnss,
                                                                                   const Eigen::Vector3d& b_omega_ib, const Eigen::Matrix3d& n_Omega_ie);
 
    [[nodiscard]] static KeyedVector<double, KFMeas, 6> e_measurementInnovation_dz(const Eigen::Vector3d& e_positionMeasurement, const Eigen::Vector3d& e_positionEstimate,
                                                                                   const Eigen::Vector3d& e_velocityMeasurement, const Eigen::Vector3d& e_velocityEstimate,
                                                                                   const Eigen::Quaterniond& e_Quat_b, const Eigen::Vector3d& b_leverArm_InsGnss,
                                                                                   const Eigen::Vector3d& b_omega_ib, const Eigen::Matrix3d& e_Omega_ie);
};
 
} // namespace NAV
 
#ifndef DOXYGEN_IGNORE
 
template<>
struct fmt::formatter<NAV::LooselyCoupledKF::KFStates> : fmt::formatter<const char*>
{
    template<typename FormatContext>
    auto format(const NAV::LooselyCoupledKF::KFStates& st, FormatContext& ctx) const
    {
        switch (st)
        {
        case NAV::LooselyCoupledKF::KFStates::Roll:
            return fmt::formatter<const char*>::format("Roll/Psi_eb_1", ctx);
        case NAV::LooselyCoupledKF::KFStates::Pitch:
            return fmt::formatter<const char*>::format("Pitch/Psi_eb_2", ctx);
        case NAV::LooselyCoupledKF::KFStates::Yaw:
            return fmt::formatter<const char*>::format("Yaw/Psi_eb_3", ctx);
        case NAV::LooselyCoupledKF::KFStates::VelN:
            return fmt::formatter<const char*>::format("VelN/VelX", ctx);
        case NAV::LooselyCoupledKF::KFStates::VelE:
            return fmt::formatter<const char*>::format("VelE/VelY", ctx);
        case NAV::LooselyCoupledKF::KFStates::VelD:
            return fmt::formatter<const char*>::format("VelD/VelZ", ctx);
        case NAV::LooselyCoupledKF::KFStates::PosLat:
            return fmt::formatter<const char*>::format("PosLat/PosX", ctx);
        case NAV::LooselyCoupledKF::KFStates::PosLon:
            return fmt::formatter<const char*>::format("PosLon/PosY", ctx);
        case NAV::LooselyCoupledKF::KFStates::PosAlt:
            return fmt::formatter<const char*>::format("PosAlt/PosZ", ctx);
        case NAV::LooselyCoupledKF::KFStates::AccBiasX:
            return fmt::formatter<const char*>::format("AccBiasX", ctx);
        case NAV::LooselyCoupledKF::KFStates::AccBiasY:
            return fmt::formatter<const char*>::format("AccBiasY", ctx);
        case NAV::LooselyCoupledKF::KFStates::AccBiasZ:
            return fmt::formatter<const char*>::format("AccBiasZ", ctx);
        case NAV::LooselyCoupledKF::KFStates::GyrBiasX:
            return fmt::formatter<const char*>::format("GyrBiasX", ctx);
        case NAV::LooselyCoupledKF::KFStates::GyrBiasY:
            return fmt::formatter<const char*>::format("GyrBiasY", ctx);
        case NAV::LooselyCoupledKF::KFStates::GyrBiasZ:
            return fmt::formatter<const char*>::format("GyrBiasZ", ctx);
        }
 
        return fmt::formatter<const char*>::format("ERROR", ctx);
    }
};
template<>
struct fmt::formatter<NAV::LooselyCoupledKF::KFMeas> : fmt::formatter<const char*>
{
    template<typename FormatContext>
    auto format(const NAV::LooselyCoupledKF::KFMeas& st, FormatContext& ctx) const
    {
        switch (st)
        {
        case NAV::LooselyCoupledKF::KFMeas::dPosLat:
            return fmt::formatter<const char*>::format("dPosLat/dPosX", ctx);
        case NAV::LooselyCoupledKF::KFMeas::dPosLon:
            return fmt::formatter<const char*>::format("dPosLon/dPosY", ctx);
        case NAV::LooselyCoupledKF::KFMeas::dPosAlt:
            return fmt::formatter<const char*>::format("dPosAlt/dPosZ", ctx);
        case NAV::LooselyCoupledKF::KFMeas::dVelN:
            return fmt::formatter<const char*>::format("dVelN/dVelX", ctx);
        case NAV::LooselyCoupledKF::KFMeas::dVelE:
            return fmt::formatter<const char*>::format("dVelE/dVelY", ctx);
        case NAV::LooselyCoupledKF::KFMeas::dVelD:
            return fmt::formatter<const char*>::format("dVelD/dVelZ", ctx);
        }
 
        return fmt::formatter<const char*>::format("ERROR", ctx);
    }
};
 
#endif
 
std::ostream& operator<<(std::ostream& os, const NAV::LooselyCoupledKF::KFStates& obj);
 
std::ostream& operator<<(std::ostream& os, const NAV::LooselyCoupledKF::KFMeas& obj);