KalmanFilter.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 <array>
#include <set>
 
#include "Navigation/Constants.hpp"
#include "Navigation/GNSS/Core/SatelliteIdentifier.hpp"
#include "Navigation/GNSS/Positioning/SPP/Keys.hpp"
#include "Navigation/Math/KeyedKalmanFilter.hpp"
#include "Navigation/Time/InsTime.hpp"
 
#include "util/Eigen.hpp"
 
namespace NAV::SPP
{
 
class KalmanFilter // NOLINT(clang-analyzer-optin.performance.Padding)
{
  public:
    void reset();
 
    void initialize(const KeyedVectorXd<States::StateKeyTypes>& states, const KeyedMatrixXd<States::StateKeyTypes, States::StateKeyTypes>& variance);
 
    void deinitialize();
 
    [[nodiscard]] bool isInitialized() const { return _initialized; }
 
    void predict(const double& dt, const Eigen::Vector3d& lla_pos, const std::string& nameId);
 
    void update(const std::vector<Meas::MeasKeyTypes>& measKeys,
                const KeyedMatrixXd<Meas::MeasKeyTypes, States::StateKeyTypes>& H,
                const KeyedMatrixXd<Meas::MeasKeyTypes, Meas::MeasKeyTypes>& R,
                const KeyedVectorXd<Meas::MeasKeyTypes>& dz,
                const std::string& nameId);
 
    SatelliteSystem updateInterSystemTimeDifferences(const std::set<SatelliteSystem>& usedSatSystems,
                                                     SatelliteSystem oldRefSys,
                                                     SatelliteSystem newRefSys,
                                                     const std::string& nameId);
 
    void addInterFrequencyBias(const Frequency& freq);
 
    KeyedMatrixXd<States::StateKeyTypes, States::StateKeyTypes>
        calcProcessNoiseMatrixGroves(double dt, const Eigen::Vector3d& lla_pos, const std::string& nameId) const;
 
    bool ShowGuiWidgets(const char* id, bool useDoppler, bool multiConstellation, bool estimateInterFrequencyBiases, float itemWidth, float unitWidth);
 
    void setClockBiasErrorCovariance(double clkPhaseDrift);
 
    [[nodiscard]] const std::vector<SPP::States::StateKeyTypes>& getStateKeys() const;
 
    [[nodiscard]] const KeyedVectorXd<States::StateKeyTypes>& getState() const;
 
    [[nodiscard]] const KeyedMatrixXd<States::StateKeyTypes, States::StateKeyTypes>& getErrorCovarianceMatrix() const;
 
  private:
    KeyedKalmanFilterD<SPP::States::StateKeyTypes, SPP::Meas::MeasKeyTypes> _kalmanFilter{ SPP::States::PosVelRecvClk, {} };
 
    bool _initialized = false;
 
    // ###########################################################################################################
    //                                               GUI settings
    // ###########################################################################################################
 
    enum class QCalculationAlgorithm
    {
        VanLoan, 
        Taylor1, 
    };
    QCalculationAlgorithm _qCalculationAlgorithm = QCalculationAlgorithm::VanLoan;
 
    // ###########################################################################################################
 
    enum class CovarianceAccelUnits
    {
        m_sqrts3, 
        m2_s3,    
    };
    CovarianceAccelUnits _gui_covarianceAccelUnit = CovarianceAccelUnits::m_sqrts3;
 
    std::array<double, 2> _gui_covarianceAccel = { 3.0, 1.5 } /* [ m / √(s^3) ] */;
    std::array<double, 2> _covarianceAccel = { 3.0, 1.5 };
 
    // ###########################################################################################################
 
    enum class CovarianceClkPhaseDriftUnits
    {
        m_sqrts, 
        m2_s,    
    };
    CovarianceClkPhaseDriftUnits _gui_covarianceClkPhaseDriftUnit = CovarianceClkPhaseDriftUnits::m2_s;
 
    double _gui_covarianceClkPhaseDrift = 0.01 /*[ m^2 / s ] */;
    double _covarianceClkPhaseDrift = 0.01;
 
    // ###########################################################################################################
 
    enum class CovarianceClkFrequencyDriftUnits
    {
        m_sqrts3, 
        m2_s3,    
    };
    CovarianceClkFrequencyDriftUnits _gui_covarianceClkFrequencyDriftUnit = CovarianceClkFrequencyDriftUnits::m2_s3;
 
    double _gui_covarianceClkFrequencyDrift = 0.04 /* [ m^2 / s^3 ] */;
    double _covarianceClkFrequencyDrift = 0.04;
 
    // ###########################################################################################################
 
    CovarianceClkPhaseDriftUnits _gui_covarianceInterSysClkPhaseDriftUnit = CovarianceClkPhaseDriftUnits::m2_s;
 
    double _gui_covarianceInterSysClkPhaseDrift = 0.01 /* [m²/s] */;
    double _covarianceInterSysClkPhaseDrift = 0.01;
 
    // ###########################################################################################################
 
    CovarianceClkFrequencyDriftUnits _gui_covarianceInterSysClkFrequencyDriftUnit = CovarianceClkFrequencyDriftUnits::m2_s3;
 
    double _gui_covarianceInterSysClkFrequencyDrift = 0.04 /* [m²/s³] */;
    double _covarianceInterSysClkFrequencyDrift = 0.04;
 
    // ###########################################################################################################
 
    CovarianceClkPhaseDriftUnits _gui_covarianceInterFrequencyBiasUnit = CovarianceClkPhaseDriftUnits::m2_s;
 
    double _gui_covarianceInterFrequencyBias = 1e-6 /* [m²/s] */;
    double _covarianceInterFrequencyBias = 1e-6;
 
    // ###########################################################################################################
    // ###########################################################################################################
 
    enum class InitCovarianceVelocityUnits
    {
        m_s,   
        m2_s2, 
    };
    InitCovarianceVelocityUnits _gui_initCovarianceVelocityUnit = InitCovarianceVelocityUnits::m_s;
 
    double _gui_initCovarianceVelocity = 10 /* [ m / s ] */;
    double _initCovarianceVelocity = 100;
 
    // ###########################################################################################################
 
    enum class InitCovarianceClockDriftUnits
    {
        m_s,   
        s_s,   
        m2_s2, 
        s2_s2, 
    };
    InitCovarianceClockDriftUnits _gui_initCovarianceClockDriftUnit = InitCovarianceClockDriftUnits::s_s;
 
    double _gui_initCovarianceClockDrift = 1e-6 /* [ s / s ] */;
    double _initCovarianceClockDrift = std::pow(1e-6 * InsConst<>::C, 2);
 
    // ###########################################################################################################
 
    InitCovarianceClockDriftUnits _gui_initCovarianceInterSysClockDriftUnit = InitCovarianceClockDriftUnits::s_s;
 
    double _gui_initCovarianceInterSysClockDrift = 1e-6 /* [ s / s ] */;
    double _initCovarianceInterSysClockDrift = std::pow(1e-6 * InsConst<>::C, 2);
 
    // ###########################################################################################################
    // ###########################################################################################################
 
    friend void to_json(json& j, const KalmanFilter& data);
    friend void from_json(const json& j, KalmanFilter& data);
};
 
void to_json(json& j, const KalmanFilter& data);
void from_json(const json& j, KalmanFilter& data);
 
} // namespace NAV::SPP