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/Core/SatelliteSystem.hpp"
#include "Navigation/GNSS/Positioning/SPP/Keys.hpp"
#include "Navigation/GNSS/SystemModel/MotionModel.hpp"
#include "Navigation/GNSS/SystemModel/ReceiverClockModel.hpp"
#include "Navigation/GNSS/SystemModel/InterFrequencyBiasModel.hpp"
#include "Navigation/GNSS/SystemModel/SystemModel.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(const std::vector<SatelliteSystem>& satelliteSystems);

    void initialize(const KeyedVectorXd<States::StateKeyType>& states, const KeyedMatrixXd<States::StateKeyType, States::StateKeyType>& 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::StateKeyType>& H,
                const KeyedMatrixXd<Meas::MeasKeyTypes, Meas::MeasKeyTypes>& R,
                const KeyedVectorXd<Meas::MeasKeyTypes>& dz,
                const std::string& nameId);

    void addInterFrequencyBias(const Frequency& freq);

    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::StateKeyType>& getStateKeys() const;

    [[nodiscard]] const KeyedVectorXd<States::StateKeyType>& getState() const;

    [[nodiscard]] const KeyedMatrixXd<States::StateKeyType, States::StateKeyType>& getErrorCovarianceMatrix() const;
 
  private:
    // /// @brief All position keys
    // const std::vector<SPP::States::StateKeyType>& PosKey = Keys::Pos<SPP::States::StateKeyType>;

    const std::array<SPP::States::StateKeyType, 3>& VelKey = Keys::Vel<SPP::States::StateKeyType>;
    const std::array<SPP::States::StateKeyType, 6>& PosVelKey = Keys::PosVel<SPP::States::StateKeyType>;

    KeyedKalmanFilterD<SPP::States::StateKeyType, SPP::Meas::MeasKeyTypes> _kalmanFilter{ PosVelKey, {} };

    bool _initialized = false;
 
    SystemModelCalcAlgorithm _systemModelCalcAlgorithm = SystemModelCalcAlgorithm::Taylor1; 
    MotionModel<SPP::States::StateKeyType> _motionModel;                                    
    ReceiverClockModel<SPP::States::StateKeyType> _receiverClockModel;                      
    InterFrequencyBiasModel<SPP::States::StateKeyType> _interFrequencyBiasModel;            
 
    // ###########################################################################################################

    enum class InitCovarianceVelocityUnits : uint8_t
    {
        m_s,   
        m2_s2, 
    };

    InitCovarianceVelocityUnits _gui_initCovarianceVelocityUnit = InitCovarianceVelocityUnits::m_s;

    double _gui_initCovarianceVelocity = 10 /* [ m / s ] */;
    double _initCovarianceVelocity = 100;
 
    // ###########################################################################################################

    enum class InitCovarianceClockDriftUnits : uint8_t
    {
        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