INSTINCT Code Coverage Report

Directory:	src/
File:	Navigation/GNSS/Positioning/SPP/KalmanFilter.hpp
Date:	2025-02-07 16:54:41

	Total	Coverage
Lines:	1	0.0%
Functions:	1	0.0%
Branches:	0	-%

  
      Line
      Branch
      Exec
      Source
    
      // 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/.
    
      /// @file KalmanFilter.hpp
    
      /// @brief
    
      /// @author T. Topp (topp@ins.uni-stuttgart.de)
    
      /// @author P. Peitschat (paula.peitschat@ins.uni-stuttgart.de)
    
      /// @date 2023-12-22
    
      #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
    
      {
    
      /// @brief The Spp Kalman Filter related options
    
      class KalmanFilter // NOLINT(clang-analyzer-optin.performance.Padding)
    
      {
    
        public:
    
          /// @brief Resets the filter
    
          /// @param satelliteSystems Satellite systems to consider
    
          void reset(const std::vector<SatelliteSystem>& satelliteSystems);
    
          /// @brief Initialize the filter
    
          /// @param states States to initialize with
    
          /// @param variance Variance of the state
    
          void initialize(const KeyedVectorXd<States::StateKeyType>& states, const KeyedMatrixXd<States::StateKeyType, States::StateKeyType>& variance);
    
          /// @brief Deinitialize the KF (can be used to reinitialize the Filter when results seem strange)
    
          void deinitialize();
    
          /// @brief Checks wether the Kalman filter is initialized
    
      ✗
          [[nodiscard]] bool isInitialized() const { return _initialized; }
    
          /// @brief Does the Kalman Filter prediction
    
          /// @param[in] dt Time step [s]
    
          /// @param[in] lla_pos Position in Latitude, Longitude, Altitude [rad, rad, m]
    
          /// @param[in] nameId Name and id of the node calling this (only used for logging purposes)
    
          void predict(const double& dt, const Eigen::Vector3d& lla_pos, const std::string& nameId);
    
          /// @brief Does the Kalman Filter update
    
          /// @param[in] measKeys Measurement keys
    
          /// @param[in] H Measurement sensitivity matrix 𝐇
    
          /// @param[in] R Measurement noise covariance matrix 𝐑
    
          /// @param[in] dz Measurement innovation 𝜹𝐳
    
          /// @param[in] nameId Name and id of the node calling this (only used for logging purposes)
    
          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);
    
          /// @brief Adds the frequency as inter-frequency bias state
    
          /// @param[in] freq Frequency to estimate the inter-frequency bias for
    
          void addInterFrequencyBias(const Frequency& freq);
    
          /// @brief Shows the GUI input to select the options
    
          /// @param[in] id Unique id for ImGui.
    
          /// @param[in] useDoppler Whether to use doppler measurements
    
          /// @param[in] multiConstellation Whether to use multiple constellations
    
          /// @param[in] estimateInterFrequencyBiases Whether to use estimate inter frequency biases
    
          /// @param[in] itemWidth Width of the widgets
    
          /// @param[in] unitWidth  Width on unit inputs
    
          /// @return True when something was changed
    
          bool ShowGuiWidgets(const char* id, bool useDoppler, bool multiConstellation, bool estimateInterFrequencyBiases, float itemWidth, float unitWidth);
    
          /// @brief Set the P matrix entry for the covariance of the clock phase drift
    
          /// @param clkPhaseDrift Clock phase drift variance in [m^2 / s]
    
          void setClockBiasErrorCovariance(double clkPhaseDrift);
    
          /// @brief Get the States in the Kalman Filter
    
          [[nodiscard]] const std::vector<SPP::States::StateKeyType>& getStateKeys() const;
    
          /// @brief Returns the State vector x̂
    
          [[nodiscard]] const KeyedVectorXd<States::StateKeyType>& getState() const;
    
          /// @brief Returns the Error covariance matrix 𝐏
    
          [[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>;
    
          /// @brief All velocity keys
    
          const std::vector<SPP::States::StateKeyType>& VelKey = Keys::Vel<SPP::States::StateKeyType>;
    
          /// @brief All position and velocity keys
    
          const std::vector<SPP::States::StateKeyType>& PosVelKey = Keys::PosVel<SPP::States::StateKeyType>;
    
          /// Kalman Filter representation
    
          KeyedKalmanFilterD<SPP::States::StateKeyType, SPP::Meas::MeasKeyTypes> _kalmanFilter{ PosVelKey, {} };
    
          /// Boolean that determines, if Kalman Filter is initialized (from weighted LSE solution)
    
          bool _initialized = false;
    
          SystemModelCalcAlgorithm _systemModelCalcAlgorithm = SystemModelCalcAlgorithm::Taylor1; ///< Algorithm to calculate the system models with
    
          MotionModel<SPP::States::StateKeyType> _motionModel;                                    ///< Motion Model
    
          ReceiverClockModel<SPP::States::StateKeyType> _receiverClockModel;                      ///< Receiver clock Model
    
          InterFrequencyBiasModel<SPP::States::StateKeyType> _interFrequencyBiasModel;            ///< Inter-frequency bias Model
    
          // ###########################################################################################################
    
          /// Possible Units for the P matrix initialization velocity uncertainty
    
          enum class InitCovarianceVelocityUnits : uint8_t
    
          {
    
              m_s,   ///< [ m / s ]
    
              m2_s2, ///< [ m^2 / s^2 ]
    
          };
    
          /// Gui selection for the Unit of the P matrix initialization velocity uncertainty
    
          InitCovarianceVelocityUnits _gui_initCovarianceVelocityUnit = InitCovarianceVelocityUnits::m_s;
    
          /// @brief GUI selection for the P matrix initialization velocity uncertainty
    
          double _gui_initCovarianceVelocity = 10 /* [ m / s ] */;
    
          /// @brief Covariance of the P matrix initialization velocity uncertainty [m²/s²]
    
          double _initCovarianceVelocity = 100;
    
          // ###########################################################################################################
    
          /// Possible Units for the P matrix initialization clock drift uncertainty
    
          enum class InitCovarianceClockDriftUnits : uint8_t
    
          {
    
              m_s,   ///< [ m / s ]
    
              s_s,   ///< [ s / s ]
    
              m2_s2, ///< [ m^2 / s^2 ]
    
              s2_s2, ///< [ s^2 / s^2 ]
    
          };
    
          /// Gui selection for the Unit of the P matrix initialization clock drift uncertainty
    
          InitCovarianceClockDriftUnits _gui_initCovarianceClockDriftUnit = InitCovarianceClockDriftUnits::s_s;
    
          /// @brief GUI selection for the P matrix initialization clock drift uncertainty
    
          double _gui_initCovarianceClockDrift = 1e-6 /* [ s / s ] */;
    
          /// @brief Covariance of the P matrix initialization clock drift uncertainty [m²/s²]
    
          double _initCovarianceClockDrift = std::pow(1e-6 * InsConst::C, 2);
    
          // ###########################################################################################################
    
          /// Gui selection for the Unit of the P matrix initialization inter system clock drift uncertainty
    
          InitCovarianceClockDriftUnits _gui_initCovarianceInterSysClockDriftUnit = InitCovarianceClockDriftUnits::s_s;
    
          /// @brief GUI selection for the P matrix initialization inter system clock drift uncertainty
    
          double _gui_initCovarianceInterSysClockDrift = 1e-6 /* [ s / s ] */;
    
          /// @brief Covariance of the P matrix initialization inter system clock drift uncertainty [m²/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);
    
      };
    
      /// @brief Converts the provided data into a json object
    
      /// @param[out] j Json object which gets filled with the info
    
      /// @param[in] data Data to convert into json
    
      void to_json(json& j, const KalmanFilter& data);
    
      /// @brief Converts the provided json object into the data object
    
      /// @param[in] j Json object with the needed values
    
      /// @param[out] data Object to fill from the json
    
      void from_json(const json& j, KalmanFilter& data);
    
      } // namespace NAV::SPP

Line	Exec	Source
1		// This file is part of INSTINCT, the INS Toolkit for Integrated
2		// Navigation Concepts and Training by the Institute of Navigation of
3		// the University of Stuttgart, Germany.
4		//
5		// This Source Code Form is subject to the terms of the Mozilla Public
6		// License, v. 2.0. If a copy of the MPL was not distributed with this
7		// file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9		/// @file KalmanFilter.hpp
10		/// @brief
11		/// @author T. Topp (topp@ins.uni-stuttgart.de)
12		/// @author P. Peitschat (paula.peitschat@ins.uni-stuttgart.de)
13		/// @date 2023-12-22
14
15		#pragma once
16
17		#include <array>
18		#include <set>
19
20		#include "Navigation/Constants.hpp"
21		#include "Navigation/GNSS/Core/SatelliteIdentifier.hpp"
22		#include "Navigation/GNSS/Core/SatelliteSystem.hpp"
23		#include "Navigation/GNSS/Positioning/SPP/Keys.hpp"
24		#include "Navigation/GNSS/SystemModel/MotionModel.hpp"
25		#include "Navigation/GNSS/SystemModel/ReceiverClockModel.hpp"
26		#include "Navigation/GNSS/SystemModel/InterFrequencyBiasModel.hpp"
27		#include "Navigation/GNSS/SystemModel/SystemModel.hpp"
28		#include "Navigation/Math/KeyedKalmanFilter.hpp"
29		#include "Navigation/Time/InsTime.hpp"
30
31		#include "util/Eigen.hpp"
32
33		namespace NAV::SPP
34		{
35
36		/// @brief The Spp Kalman Filter related options
37		class KalmanFilter // NOLINT(clang-analyzer-optin.performance.Padding)
38		{
39		public:
40		/// @brief Resets the filter
41		/// @param satelliteSystems Satellite systems to consider
42		void reset(const std::vector<SatelliteSystem>& satelliteSystems);
43
44		/// @brief Initialize the filter
45		/// @param states States to initialize with
46		/// @param variance Variance of the state
47		void initialize(const KeyedVectorXd<States::StateKeyType>& states, const KeyedMatrixXd<States::StateKeyType, States::StateKeyType>& variance);
48
49		/// @brief Deinitialize the KF (can be used to reinitialize the Filter when results seem strange)
50		void deinitialize();
51
52		/// @brief Checks wether the Kalman filter is initialized
53	✗	[[nodiscard]] bool isInitialized() const { return _initialized; }
54
55		/// @brief Does the Kalman Filter prediction
56		/// @param[in] dt Time step [s]
57		/// @param[in] lla_pos Position in Latitude, Longitude, Altitude [rad, rad, m]
58		/// @param[in] nameId Name and id of the node calling this (only used for logging purposes)
59		void predict(const double& dt, const Eigen::Vector3d& lla_pos, const std::string& nameId);
60
61		/// @brief Does the Kalman Filter update
62		/// @param[in] measKeys Measurement keys
63		/// @param[in] H Measurement sensitivity matrix 𝐇
64		/// @param[in] R Measurement noise covariance matrix 𝐑
65		/// @param[in] dz Measurement innovation 𝜹𝐳
66		/// @param[in] nameId Name and id of the node calling this (only used for logging purposes)
67		void update(const std::vector<Meas::MeasKeyTypes>& measKeys,
68		const KeyedMatrixXd<Meas::MeasKeyTypes, States::StateKeyType>& H,
69		const KeyedMatrixXd<Meas::MeasKeyTypes, Meas::MeasKeyTypes>& R,
70		const KeyedVectorXd<Meas::MeasKeyTypes>& dz,
71		const std::string& nameId);
72
73		/// @brief Adds the frequency as inter-frequency bias state
74		/// @param[in] freq Frequency to estimate the inter-frequency bias for
75		void addInterFrequencyBias(const Frequency& freq);
76
77		/// @brief Shows the GUI input to select the options
78		/// @param[in] id Unique id for ImGui.
79		/// @param[in] useDoppler Whether to use doppler measurements
80		/// @param[in] multiConstellation Whether to use multiple constellations
81		/// @param[in] estimateInterFrequencyBiases Whether to use estimate inter frequency biases
82		/// @param[in] itemWidth Width of the widgets
83		/// @param[in] unitWidth Width on unit inputs
84		/// @return True when something was changed
85		bool ShowGuiWidgets(const char* id, bool useDoppler, bool multiConstellation, bool estimateInterFrequencyBiases, float itemWidth, float unitWidth);
86
87		/// @brief Set the P matrix entry for the covariance of the clock phase drift
88		/// @param clkPhaseDrift Clock phase drift variance in [m^2 / s]
89		void setClockBiasErrorCovariance(double clkPhaseDrift);
90
91		/// @brief Get the States in the Kalman Filter
92		[[nodiscard]] const std::vector<SPP::States::StateKeyType>& getStateKeys() const;
93
94		/// @brief Returns the State vector x̂
95		[[nodiscard]] const KeyedVectorXd<States::StateKeyType>& getState() const;
96
97		/// @brief Returns the Error covariance matrix 𝐏
98		[[nodiscard]] const KeyedMatrixXd<States::StateKeyType, States::StateKeyType>& getErrorCovarianceMatrix() const;
99
100		private:
101		// /// @brief All position keys
102		// const std::vector<SPP::States::StateKeyType>& PosKey = Keys::Pos<SPP::States::StateKeyType>;
103
104		/// @brief All velocity keys
105		const std::vector<SPP::States::StateKeyType>& VelKey = Keys::Vel<SPP::States::StateKeyType>;
106		/// @brief All position and velocity keys
107		const std::vector<SPP::States::StateKeyType>& PosVelKey = Keys::PosVel<SPP::States::StateKeyType>;
108
109		/// Kalman Filter representation
110		KeyedKalmanFilterD<SPP::States::StateKeyType, SPP::Meas::MeasKeyTypes> _kalmanFilter{ PosVelKey, {} };
111
112		/// Boolean that determines, if Kalman Filter is initialized (from weighted LSE solution)
113		bool _initialized = false;
114
115		SystemModelCalcAlgorithm _systemModelCalcAlgorithm = SystemModelCalcAlgorithm::Taylor1; ///< Algorithm to calculate the system models with
116		MotionModel<SPP::States::StateKeyType> _motionModel; ///< Motion Model
117		ReceiverClockModel<SPP::States::StateKeyType> _receiverClockModel; ///< Receiver clock Model
118		InterFrequencyBiasModel<SPP::States::StateKeyType> _interFrequencyBiasModel; ///< Inter-frequency bias Model
119
120		// ###########################################################################################################
121
122		/// Possible Units for the P matrix initialization velocity uncertainty
123		enum class InitCovarianceVelocityUnits : uint8_t
124		{
125		m_s, ///< [ m / s ]
126		m2_s2, ///< [ m^2 / s^2 ]
127		};
128		/// Gui selection for the Unit of the P matrix initialization velocity uncertainty
129		InitCovarianceVelocityUnits _gui_initCovarianceVelocityUnit = InitCovarianceVelocityUnits::m_s;
130
131		/// @brief GUI selection for the P matrix initialization velocity uncertainty
132		double _gui_initCovarianceVelocity = 10 /* [ m / s ] */;
133		/// @brief Covariance of the P matrix initialization velocity uncertainty [m²/s²]
134		double _initCovarianceVelocity = 100;
135
136		// ###########################################################################################################
137
138		/// Possible Units for the P matrix initialization clock drift uncertainty
139		enum class InitCovarianceClockDriftUnits : uint8_t
140		{
141		m_s, ///< [ m / s ]
142		s_s, ///< [ s / s ]
143		m2_s2, ///< [ m^2 / s^2 ]
144		s2_s2, ///< [ s^2 / s^2 ]
145		};
146		/// Gui selection for the Unit of the P matrix initialization clock drift uncertainty
147		InitCovarianceClockDriftUnits _gui_initCovarianceClockDriftUnit = InitCovarianceClockDriftUnits::s_s;
148
149		/// @brief GUI selection for the P matrix initialization clock drift uncertainty
150		double _gui_initCovarianceClockDrift = 1e-6 /* [ s / s ] */;
151		/// @brief Covariance of the P matrix initialization clock drift uncertainty [m²/s²]
152		double _initCovarianceClockDrift = std::pow(1e-6 * InsConst::C, 2);
153
154		// ###########################################################################################################
155
156		/// Gui selection for the Unit of the P matrix initialization inter system clock drift uncertainty
157		InitCovarianceClockDriftUnits _gui_initCovarianceInterSysClockDriftUnit = InitCovarianceClockDriftUnits::s_s;
158
159		/// @brief GUI selection for the P matrix initialization inter system clock drift uncertainty
160		double _gui_initCovarianceInterSysClockDrift = 1e-6 /* [ s / s ] */;
161		/// @brief Covariance of the P matrix initialization inter system clock drift uncertainty [m²/s²]
162		double _initCovarianceInterSysClockDrift = std::pow(1e-6 * InsConst::C, 2);
163
164		// ###########################################################################################################
165		// ###########################################################################################################
166
167		friend void to_json(json& j, const KalmanFilter& data);
168		friend void from_json(const json& j, KalmanFilter& data);
169		};
170
171		/// @brief Converts the provided data into a json object
172		/// @param[out] j Json object which gets filled with the info
173		/// @param[in] data Data to convert into json
174		void to_json(json& j, const KalmanFilter& data);
175		/// @brief Converts the provided json object into the data object
176		/// @param[in] j Json object with the needed values
177		/// @param[out] data Object to fill from the json
178		void from_json(const json& j, KalmanFilter& data);
179
180		} // namespace NAV::SPP
181