INSTINCT Code Coverage Report

Directory:	src/
File:	Nodes/DataProcessor/SensorCombiner/ImuFusion.hpp
Date:	2025-06-02 15:19:59

	Total	Coverage
Lines:	9	0.0%
Functions:	1	0.0%
Branches:	4	0.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 ImuFusion.hpp
    
      /// @brief Combines signals of sensors that provide the same signal-type to one signal
    
      /// @author M. Maier (marcel.maier@ins.uni-stuttgart.de)
    
      /// @date 2022-03-24
    
      #pragma once
    
      #include "Nodes/DataProvider/IMU/Imu.hpp"
    
      #include "NodeData/IMU/ImuObs.hpp"
    
      #include "Navigation/Math/KalmanFilter.hpp"
    
      #include "Navigation/INS/SensorCombiner/PinData.hpp"
    
      #include "internal/gui/widgets/TimeEdit.hpp"
    
      namespace NAV
    
      {
    
      /// @brief Combines signals of sensors that provide the same signal-type to one signal
    
      class ImuFusion : public Imu
    
      {
    
        public:
    
          /// @brief Default constructor
    
          ImuFusion();
    
          /// @brief Destructor
    
          ~ImuFusion() override;
    
          /// @brief Copy constructor
    
          ImuFusion(const ImuFusion&) = delete;
    
          /// @brief Move constructor
    
          ImuFusion(ImuFusion&&) = delete;
    
          /// @brief Copy assignment operator
    
          ImuFusion& operator=(const ImuFusion&) = delete;
    
          /// @brief Move assignment operator
    
          ImuFusion& operator=(ImuFusion&&) = delete;
    
          /// @brief String representation of the Class Type
    
          [[nodiscard]] static std::string typeStatic();
    
          /// @brief String representation of the Class Type
    
          [[nodiscard]] std::string type() const override;
    
          /// @brief String representation of the Class Category
    
          [[nodiscard]] static std::string category();
    
          /// @brief ImGui config window which is shown on double click
    
          /// @attention Don't forget to set _hasConfig to true in the constructor of the node
    
          void guiConfig() override;
    
          /// @brief Saves the node into a json object
    
          [[nodiscard]] json save() const override;
    
          /// @brief Restores the node from a json object
    
          /// @param[in] j Json object with the node state
    
          void restore(const json& j) override;
    
        protected:
    
          /// Position and rotation information for conversion from platform to body frame
    
          ImuPos _imuPos;
    
        private:
    
          constexpr static size_t OUTPUT_PORT_INDEX_COMBINED_SIGNAL = 0; ///< @brief Flow (ImuObs)
    
          constexpr static size_t OUTPUT_PORT_INDEX_BIASES = 1;          ///< @brief Flow (ImuBiases)
    
          /// @brief Initialize the node
    
          bool initialize() override;
    
          /// @brief Deinitialize the node
    
          void deinitialize() override;
    
          /// @brief Adds/Deletes Input Pins depending on the variable _nInputPins
    
          void updateNumberOfInputPins();
    
          /// @brief Initializes the rotation matrices used for the mounting angles of the sensors
    
          void initializeMountingAngles();
    
          /// @brief Receive Function for the signal at the time tₖ
    
          /// @param[in] queue Queue with all the received data messages
    
          /// @param[in] pinIdx Index of the pin the data is received on
    
          void recvSignal(InputPin::NodeDataQueue& queue, size_t pinIdx);
    
          /// @brief Calculates the adaptive measurement noise matrix R
    
          /// @param[in] alpha Forgetting factor (i.e. weight on previous estimates), 0 < alpha < 1
    
          /// @param[in] R Measurement noise covariance matrix at the previous epoch
    
          /// @param[in] e Vector of residuals
    
          /// @param[in] H Design matrix
    
          /// @param[in] P Error covariance matrix
    
          /// @return Measurement noise matrix R
    
          /// @note See https://arxiv.org/pdf/1702.00884.pdf
    
          [[nodiscard]] static Eigen::MatrixXd measurementNoiseMatrix_R_adaptive(double alpha,
    
                                                                                 const Eigen::MatrixXd& R,
    
                                                                                 const Eigen::VectorXd& e,
    
                                                                                 const Eigen::MatrixXd& H,
    
                                                                                 const Eigen::MatrixXd& P);
    
          /// @brief Previous observation (for timestamp)
    
          InsTime _lastFiltObs;
    
          /// @brief Calculates the initial measurement noise matrix R
    
          /// @param[in] R Measurement noise uncertainty matrix for sensor at 'pinIndex'
    
          /// @param[in] pinIndex Index of pin to identify sensor
    
          void measurementNoiseMatrix_R(Eigen::MatrixXd& R, size_t pinIndex = 0) const;
    
          /// @brief Combines the signals
    
          /// @param[in] imuObs Imu observation
    
          void combineSignals(const std::shared_ptr<const ImuObs>& imuObs);
    
          // --------------------------------------- Kalman filter config ------------------------------------------
    
          /// Number of input pins
    
          size_t _nInputPins = 2;
    
          /// @brief Number of states estimated by the IRW-KF (angular rate, angular acceleration, specific force, jerk, all in 3D: 4*3 = 12)
    
          const uint8_t _numStatesEstIRWKF = 12;
    
          /// @brief Number of states estimated by the B-spline KF (3 stacked B-splines in 3D for angular rate and specific force: 3*3*2 = 18)
    
          const uint8_t _numStatesEstBsplineKF = 18;
    
          /// @brief Number of quadratic B-splines that make up the entire 3D stacked B-spline
    
          const uint8_t _numBsplines = 9;
    
          /// @brief Number of states per pin (biases of accel and gyro)
    
          const uint8_t _numStatesPerPin = 6;
    
          /// @brief Number of measurements overall
    
          const uint8_t _numMeasurements = 6;
    
          /// @brief Number of estimated states (depends on imuFusionType)
    
          uint8_t _numStatesEst{};
    
          /// @brief Number of states overall
    
          uint8_t _numStates = 12;
    
          /// Kalman Filter representation
    
          KalmanFilter _kalmanFilter{ _numStates, _numMeasurements };
    
          // ---------------------------------------- Kalman filter init -------------------------------------------
    
          /// @brief Highest IMU sample rate in [Hz] (for time step in KF prediction)
    
          double _imuFrequency{ 100 };
    
          /// @brief Time until averaging ends and filtering starts in [s]
    
          double _averageEndTime{ 1 };
    
          /// @brief Time until averaging ends and filtering starts as 'InsTime'
    
          InsTime _avgEndTime;
    
          /// @brief Auto-initialize the Kalman Filter - GUI setting
    
          bool _autoInitKF = true;
    
          /// @brief If the multiple IMUs have the same characteristics, GUI input cells can be reduced considerably
    
          bool _imuCharacteristicsIdentical = true;
    
          /// @brief If the multiple IMUs have the same bias, GUI input cells can be reduced considerably
    
          bool _imuBiasesIdentical = true;
    
          /// @brief Container that collects all imuObs for averaging for auto-init of the KF
    
          std::vector<std::shared_ptr<const NAV::ImuObs>> _cumulatedImuObs;
    
          /// @brief Container that collects all pinIds for averaging for auto-init of the KF
    
          std::vector<size_t> _cumulatedPinIds;
    
          /// @brief flag to determine how jerk and angular acceleration states are initialized if '_autoInitKF = true'
    
          bool _initJerkAngAcc = true;
    
          /// @brief flag to check whether KF has been auto-initialized
    
          bool _kfInitialized = false;
    
          // -------------------------------------------- Sensor info ----------------------------------------------
    
          /// @brief Stores parameter data for each connected sensor
    
          std::vector<PinData> _pinData;
    
          /// @brief Stores IRW-KF specific parameter data
    
          PinDataIRWKF _pinDataIRWKF;
    
          /// @brief Stores Bspline-KF specific parameter data
    
          PinDataBsplineKF _pinDataBsplineKF;
    
          /// @brief Temporary vector for the initial coefficients for angular rate
    
          Eigen::Vector3d _initCoeffsAngRateTemp;
    
          /// @brief Temporary vector for the initial coefficients for acceleration
    
          Eigen::Vector3d _initCoeffsAccelTemp;
    
          /// @brief Temporary vector for the initial coefficients' initial covariance for the angular rate
    
          Eigen::Vector3d _initCovarianceCoeffsAngRateTemp;
    
          /// @brief Temporary vector for the initial coefficients' initial covariance for the acceleration
    
          Eigen::Vector3d _initCovarianceCoeffsAccelTemp;
    
          /// @brief Temporary vector for the initial coefficients' process noise for the angular rate
    
          Eigen::Vector3d _procNoiseCoeffsAngRateTemp;
    
          /// @brief Temporary vector for the initial coefficients' process noise for the acceleration
    
          Eigen::Vector3d _procNoiseCoeffsAccelTemp;
    
          /// @brief Container for the bias covariances
    
          std::vector<Eigen::Vector3d> _biasCovariances;
    
          /// @brief Container for process noise of each state
    
          std::vector<Eigen::VectorXd> _processNoiseVariances;
    
          /// @brief Container for measurement noises of each sensor
    
          std::vector<Eigen::Vector3d> _measurementNoiseVariances;
    
          /// @brief Rotations of all connected accelerometers - key: pinIndex, value: Rotation matrix of the accelerometer platform to body frame
    
          std::vector<Eigen::Matrix3d> _imuRotations_accel;
    
          /// @brief Rotations of all connected gyros - key: pinIndex, value: Rotation matrix of the gyro platform to body frame
    
          std::vector<Eigen::Matrix3d> _imuRotations_gyro;
    
          // ----------------------------------------------- Time --------------------------------------------------
    
          /// @brief Saves the timestamp of the measurement before in [s]
    
          InsTime _latestTimestamp;
    
          /// @brief Saves the first timestamp in [s]
    
          InsTime _firstTimestamp;
    
          /// @brief Time since startup in [s]
    
          double _timeSinceStartup{};
    
          /// @brief Latest knot in [s]
    
          double _latestKnot{};
    
          /// @brief Time difference between two quadratic B-splines in the stacked B-spline
    
          double _splineSpacing = 1.0;
    
          // ------------------------------------------- Miscellaneous ---------------------------------------------
    
          /// @brief Check the rank of the Kalman matrices every iteration (computationally expensive)
    
          bool _checkKalmanMatricesRanks = false;
    
          /// @brief Check whether the combined solution has an '_imuPos' set
    
          bool _imuPosSet = false;
    
          /// Possible KF-types for the IMU fusion
    
          enum class ImuFusionType : uint8_t
    
          {
    
              IRWKF,   ///< IRW Kalman filter
    
              Bspline, ///< B-spline Kalman filter
    
              COUNT,   ///< Number of items in the enum
    
          };
    
          /// @brief Converts the enum to a string
    
          /// @param[in] value Enum value to convert into text
    
          /// @return String representation of the enum
    
          friend constexpr const char* to_string(ImuFusionType value);
    
          /// KF-type for the IMU fusion, selected in the GUI
    
          ImuFusionType _imuFusionType = ImuFusionType::Bspline;
    
      };
    
      ✗
      constexpr const char* to_string(NAV::ImuFusion::ImuFusionType value)
    
      {
    
      ✗
          switch (value)
    
          {
    
      ✗
          case NAV::ImuFusion::ImuFusionType::IRWKF:
    
      ✗
              return "IRW KF";
    
      ✗
          case NAV::ImuFusion::ImuFusionType::Bspline:
    
      ✗
              return "B-spline KF";
    
      ✗
          case NAV::ImuFusion::ImuFusionType::COUNT:
    
      ✗
              return "";
    
          }
    
      ✗
          return "";
    
      }
    
      } // namespace NAV

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 ImuFusion.hpp
10		/// @brief Combines signals of sensors that provide the same signal-type to one signal
11		/// @author M. Maier (marcel.maier@ins.uni-stuttgart.de)
12		/// @date 2022-03-24
13
14		#pragma once
15
16		#include "Nodes/DataProvider/IMU/Imu.hpp"
17		#include "NodeData/IMU/ImuObs.hpp"
18
19		#include "Navigation/Math/KalmanFilter.hpp"
20
21		#include "Navigation/INS/SensorCombiner/PinData.hpp"
22
23		#include "internal/gui/widgets/TimeEdit.hpp"
24
25		namespace NAV
26		{
27		/// @brief Combines signals of sensors that provide the same signal-type to one signal
28		class ImuFusion : public Imu
29		{
30		public:
31		/// @brief Default constructor
32		ImuFusion();
33		/// @brief Destructor
34		~ImuFusion() override;
35		/// @brief Copy constructor
36		ImuFusion(const ImuFusion&) = delete;
37		/// @brief Move constructor
38		ImuFusion(ImuFusion&&) = delete;
39		/// @brief Copy assignment operator
40		ImuFusion& operator=(const ImuFusion&) = delete;
41		/// @brief Move assignment operator
42		ImuFusion& operator=(ImuFusion&&) = delete;
43
44		/// @brief String representation of the Class Type
45		[[nodiscard]] static std::string typeStatic();
46
47		/// @brief String representation of the Class Type
48		[[nodiscard]] std::string type() const override;
49
50		/// @brief String representation of the Class Category
51		[[nodiscard]] static std::string category();
52
53		/// @brief ImGui config window which is shown on double click
54		/// @attention Don't forget to set _hasConfig to true in the constructor of the node
55		void guiConfig() override;
56
57		/// @brief Saves the node into a json object
58		[[nodiscard]] json save() const override;
59
60		/// @brief Restores the node from a json object
61		/// @param[in] j Json object with the node state
62		void restore(const json& j) override;
63
64		protected:
65		/// Position and rotation information for conversion from platform to body frame
66		ImuPos _imuPos;
67
68		private:
69		constexpr static size_t OUTPUT_PORT_INDEX_COMBINED_SIGNAL = 0; ///< @brief Flow (ImuObs)
70		constexpr static size_t OUTPUT_PORT_INDEX_BIASES = 1; ///< @brief Flow (ImuBiases)
71
72		/// @brief Initialize the node
73		bool initialize() override;
74
75		/// @brief Deinitialize the node
76		void deinitialize() override;
77
78		/// @brief Adds/Deletes Input Pins depending on the variable _nInputPins
79		void updateNumberOfInputPins();
80
81		/// @brief Initializes the rotation matrices used for the mounting angles of the sensors
82		void initializeMountingAngles();
83
84		/// @brief Receive Function for the signal at the time tₖ
85		/// @param[in] queue Queue with all the received data messages
86		/// @param[in] pinIdx Index of the pin the data is received on
87		void recvSignal(InputPin::NodeDataQueue& queue, size_t pinIdx);
88
89		/// @brief Calculates the adaptive measurement noise matrix R
90		/// @param[in] alpha Forgetting factor (i.e. weight on previous estimates), 0 < alpha < 1
91		/// @param[in] R Measurement noise covariance matrix at the previous epoch
92		/// @param[in] e Vector of residuals
93		/// @param[in] H Design matrix
94		/// @param[in] P Error covariance matrix
95		/// @return Measurement noise matrix R
96		/// @note See https://arxiv.org/pdf/1702.00884.pdf
97		[[nodiscard]] static Eigen::MatrixXd measurementNoiseMatrix_R_adaptive(double alpha,
98		const Eigen::MatrixXd& R,
99		const Eigen::VectorXd& e,
100		const Eigen::MatrixXd& H,
101		const Eigen::MatrixXd& P);
102
103		/// @brief Previous observation (for timestamp)
104		InsTime _lastFiltObs;
105
106		/// @brief Calculates the initial measurement noise matrix R
107		/// @param[in] R Measurement noise uncertainty matrix for sensor at 'pinIndex'
108		/// @param[in] pinIndex Index of pin to identify sensor
109		void measurementNoiseMatrix_R(Eigen::MatrixXd& R, size_t pinIndex = 0) const;
110
111		/// @brief Combines the signals
112		/// @param[in] imuObs Imu observation
113		void combineSignals(const std::shared_ptr<const ImuObs>& imuObs);
114
115		// --------------------------------------- Kalman filter config ------------------------------------------
116		/// Number of input pins
117		size_t _nInputPins = 2;
118
119		/// @brief Number of states estimated by the IRW-KF (angular rate, angular acceleration, specific force, jerk, all in 3D: 4*3 = 12)
120		const uint8_t _numStatesEstIRWKF = 12;
121
122		/// @brief Number of states estimated by the B-spline KF (3 stacked B-splines in 3D for angular rate and specific force: 332 = 18)
123		const uint8_t _numStatesEstBsplineKF = 18;
124
125		/// @brief Number of quadratic B-splines that make up the entire 3D stacked B-spline
126		const uint8_t _numBsplines = 9;
127
128		/// @brief Number of states per pin (biases of accel and gyro)
129		const uint8_t _numStatesPerPin = 6;
130
131		/// @brief Number of measurements overall
132		const uint8_t _numMeasurements = 6;
133
134		/// @brief Number of estimated states (depends on imuFusionType)
135		uint8_t _numStatesEst{};
136
137		/// @brief Number of states overall
138		uint8_t _numStates = 12;
139
140		/// Kalman Filter representation
141		KalmanFilter _kalmanFilter{ _numStates, _numMeasurements };
142
143		// ---------------------------------------- Kalman filter init -------------------------------------------
144		/// @brief Highest IMU sample rate in [Hz] (for time step in KF prediction)
145		double _imuFrequency{ 100 };
146
147		/// @brief Time until averaging ends and filtering starts in [s]
148		double _averageEndTime{ 1 };
149
150		/// @brief Time until averaging ends and filtering starts as 'InsTime'
151		InsTime _avgEndTime;
152
153		/// @brief Auto-initialize the Kalman Filter - GUI setting
154		bool _autoInitKF = true;
155
156		/// @brief If the multiple IMUs have the same characteristics, GUI input cells can be reduced considerably
157		bool _imuCharacteristicsIdentical = true;
158
159		/// @brief If the multiple IMUs have the same bias, GUI input cells can be reduced considerably
160		bool _imuBiasesIdentical = true;
161
162		/// @brief Container that collects all imuObs for averaging for auto-init of the KF
163		std::vector<std::shared_ptr<const NAV::ImuObs>> _cumulatedImuObs;
164
165		/// @brief Container that collects all pinIds for averaging for auto-init of the KF
166		std::vector<size_t> _cumulatedPinIds;
167
168		/// @brief flag to determine how jerk and angular acceleration states are initialized if '_autoInitKF = true'
169		bool _initJerkAngAcc = true;
170
171		/// @brief flag to check whether KF has been auto-initialized
172		bool _kfInitialized = false;
173
174		// -------------------------------------------- Sensor info ----------------------------------------------
175		/// @brief Stores parameter data for each connected sensor
176		std::vector<PinData> _pinData;
177		/// @brief Stores IRW-KF specific parameter data
178		PinDataIRWKF _pinDataIRWKF;
179		/// @brief Stores Bspline-KF specific parameter data
180		PinDataBsplineKF _pinDataBsplineKF;
181
182		/// @brief Temporary vector for the initial coefficients for angular rate
183		Eigen::Vector3d _initCoeffsAngRateTemp;
184		/// @brief Temporary vector for the initial coefficients for acceleration
185		Eigen::Vector3d _initCoeffsAccelTemp;
186		/// @brief Temporary vector for the initial coefficients' initial covariance for the angular rate
187		Eigen::Vector3d _initCovarianceCoeffsAngRateTemp;
188		/// @brief Temporary vector for the initial coefficients' initial covariance for the acceleration
189		Eigen::Vector3d _initCovarianceCoeffsAccelTemp;
190		/// @brief Temporary vector for the initial coefficients' process noise for the angular rate
191		Eigen::Vector3d _procNoiseCoeffsAngRateTemp;
192		/// @brief Temporary vector for the initial coefficients' process noise for the acceleration
193		Eigen::Vector3d _procNoiseCoeffsAccelTemp;
194
195		/// @brief Container for the bias covariances
196		std::vector<Eigen::Vector3d> _biasCovariances;
197		/// @brief Container for process noise of each state
198		std::vector<Eigen::VectorXd> _processNoiseVariances;
199		/// @brief Container for measurement noises of each sensor
200		std::vector<Eigen::Vector3d> _measurementNoiseVariances;
201
202		/// @brief Rotations of all connected accelerometers - key: pinIndex, value: Rotation matrix of the accelerometer platform to body frame
203		std::vector<Eigen::Matrix3d> _imuRotations_accel;
204
205		/// @brief Rotations of all connected gyros - key: pinIndex, value: Rotation matrix of the gyro platform to body frame
206		std::vector<Eigen::Matrix3d> _imuRotations_gyro;
207
208		// ----------------------------------------------- Time --------------------------------------------------
209		/// @brief Saves the timestamp of the measurement before in [s]
210		InsTime _latestTimestamp;
211
212		/// @brief Saves the first timestamp in [s]
213		InsTime _firstTimestamp;
214
215		/// @brief Time since startup in [s]
216		double _timeSinceStartup{};
217
218		/// @brief Latest knot in [s]
219		double _latestKnot{};
220
221		/// @brief Time difference between two quadratic B-splines in the stacked B-spline
222		double _splineSpacing = 1.0;
223
224		// ------------------------------------------- Miscellaneous ---------------------------------------------
225		/// @brief Check the rank of the Kalman matrices every iteration (computationally expensive)
226		bool _checkKalmanMatricesRanks = false;
227
228		/// @brief Check whether the combined solution has an '_imuPos' set
229		bool _imuPosSet = false;
230
231		/// Possible KF-types for the IMU fusion
232		enum class ImuFusionType : uint8_t
233		{
234		IRWKF, ///< IRW Kalman filter
235		Bspline, ///< B-spline Kalman filter
236		COUNT, ///< Number of items in the enum
237		};
238		/// @brief Converts the enum to a string
239		/// @param[in] value Enum value to convert into text
240		/// @return String representation of the enum
241		friend constexpr const char* to_string(ImuFusionType value);
242
243		/// KF-type for the IMU fusion, selected in the GUI
244		ImuFusionType _imuFusionType = ImuFusionType::Bspline;
245		};
246
247	✗	constexpr const char* to_string(NAV::ImuFusion::ImuFusionType value)
248		{
249	✗	switch (value)
250		{
251	✗	case NAV::ImuFusion::ImuFusionType::IRWKF:
252	✗	return "IRW KF";
253	✗	case NAV::ImuFusion::ImuFusionType::Bspline:
254	✗	return "B-spline KF";
255	✗	case NAV::ImuFusion::ImuFusionType::COUNT:
256	✗	return "";
257		}
258	✗	return "";
259		}
260
261		} // namespace NAV
262