0.4.1
Loading...
Searching...
No Matches
KalmanFilter.hpp
Go to the documentation of this file.
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
30
31#include "util/Eigen.hpp"
32
33namespace NAV::SPP
34{
35
36/// @brief The Spp Kalman Filter related options
37class 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
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,
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 𝐏
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::array<SPP::States::StateKeyType, 3>& VelKey = Keys::Vel<SPP::States::StateKeyType>;
106 /// @brief All position and velocity keys
107 const std::array<SPP::States::StateKeyType, 6>& PosVelKey = Keys::PosVel<SPP::States::StateKeyType>;
108
109 /// Kalman Filter representation
111
112 /// Boolean that determines, if Kalman Filter is initialized (from weighted LSE solution)
113 bool _initialized = false;
114
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
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²]
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
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
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
174void 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
178void from_json(const json& j, KalmanFilter& data);
179
180} // namespace NAV::SPP
Holds all Constants.
Vector space operations.
nlohmann::json json
json namespace
Keys for the SPP algorithm for use inside the KeyedMatrices.
The class is responsible for all time-related tasks.
Inter Frequency Bias System Model.
Kalman Filter with keyed states.
Motion System Model.
Receiver Clock System Model.
Structs identifying a unique satellite.
GNSS Satellite System.
System Model.
Frequency definition for different satellite systems.
Definition Frequency.hpp:59
static constexpr double C
Speed of light [m/s].
Definition Constants.hpp:34
Inter Frequency Bias System Model.
Motion System Model.
Receiver Clock System Model.
The Spp Kalman Filter related options.
MotionModel< SPP::States::StateKeyType > _motionModel
Motion Model.
InitCovarianceClockDriftUnits _gui_initCovarianceClockDriftUnit
Gui selection for the Unit of the P matrix initialization clock drift uncertainty.
void deinitialize()
Deinitialize the KF (can be used to reinitialize the Filter when results seem strange)
void setClockBiasErrorCovariance(double clkPhaseDrift)
Set the P matrix entry for the covariance of the clock phase drift.
const std::vector< SPP::States::StateKeyType > & getStateKeys() const
Get the States in the Kalman Filter.
const std::array< SPP::States::StateKeyType, 6 > & PosVelKey
All position and velocity keys.
void reset(const std::vector< SatelliteSystem > &satelliteSystems)
Resets the filter.
void predict(const double &dt, const Eigen::Vector3d &lla_pos, const std::string &nameId)
Does the Kalman Filter prediction.
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)
Does the Kalman Filter update.
InitCovarianceVelocityUnits
Possible Units for the P matrix initialization velocity uncertainty.
const KeyedVectorXd< States::StateKeyType > & getState() const
Returns the State vector x̂
ReceiverClockModel< SPP::States::StateKeyType > _receiverClockModel
Receiver clock Model.
double _initCovarianceClockDrift
Covariance of the P matrix initialization clock drift uncertainty [m²/s²].
void initialize(const KeyedVectorXd< States::StateKeyType > &states, const KeyedMatrixXd< States::StateKeyType, States::StateKeyType > &variance)
Initialize the filter.
double _initCovarianceInterSysClockDrift
Covariance of the P matrix initialization inter system clock drift uncertainty [m²/s²].
void addInterFrequencyBias(const Frequency &freq)
Adds the frequency as inter-frequency bias state.
InitCovarianceClockDriftUnits _gui_initCovarianceInterSysClockDriftUnit
Gui selection for the Unit of the P matrix initialization inter system clock drift uncertainty.
SystemModelCalcAlgorithm _systemModelCalcAlgorithm
Algorithm to calculate the system models with.
friend void to_json(json &j, const KalmanFilter &data)
Converts the provided data into a json object.
InterFrequencyBiasModel< SPP::States::StateKeyType > _interFrequencyBiasModel
Inter-frequency bias Model.
InitCovarianceVelocityUnits _gui_initCovarianceVelocityUnit
Gui selection for the Unit of the P matrix initialization velocity uncertainty.
double _gui_initCovarianceVelocity
GUI selection for the P matrix initialization velocity uncertainty.
KeyedKalmanFilterD< SPP::States::StateKeyType, SPP::Meas::MeasKeyTypes > _kalmanFilter
Kalman Filter representation.
friend void from_json(const json &j, KalmanFilter &data)
Converts the provided json object into the data object.
double _gui_initCovarianceClockDrift
GUI selection for the P matrix initialization clock drift uncertainty.
const std::array< SPP::States::StateKeyType, 3 > & VelKey
All velocity keys.
double _initCovarianceVelocity
Covariance of the P matrix initialization velocity uncertainty [m²/s²].
bool _initialized
Boolean that determines, if Kalman Filter is initialized (from weighted LSE solution)
double _gui_initCovarianceInterSysClockDrift
GUI selection for the P matrix initialization inter system clock drift uncertainty.
bool ShowGuiWidgets(const char *id, bool useDoppler, bool multiConstellation, bool estimateInterFrequencyBiases, float itemWidth, float unitWidth)
Shows the GUI input to select the options.
InitCovarianceClockDriftUnits
Possible Units for the P matrix initialization clock drift uncertainty.
bool isInitialized() const
Checks wether the Kalman filter is initialized.
const KeyedMatrixXd< States::StateKeyType, States::StateKeyType > & getErrorCovarianceMatrix() const
Returns the Error covariance matrix 𝐏
constexpr std::array< StateKeyType, 6 > PosVel
Vector with all position and velocity keys.
constexpr std::array< StateKeyType, 3 > Vel
All velocity keys.
void to_json(json &j, const Algorithm &obj)
Converts the provided object into json.
void from_json(const json &j, Algorithm &obj)
Converts the provided json object into a node object.
KeyedVectorX< double, RowKeyType > KeyedVectorXd
Dynamic size KeyedVector with double types.
KeyedKalmanFilter< double, StateKeyType, MeasKeyType > KeyedKalmanFilterD
Keyed Kalman Filter class with double as type.
KeyedMatrixX< double, RowKeyType, ColKeyType > KeyedMatrixXd
Dynamic size KeyedMatrix with double types.
SystemModelCalcAlgorithm
Algorithms to calculate the system model with.