INSTINCT Code Coverage Report

Directory:	src/
File:	Nodes/DataProcessor/SensorCombiner/ImuFusion.hpp
Date:	2025-02-07 16:54:41

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