0.4.1
Loading...
Searching...
No Matches
IRWKF.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 IRWKF.hpp
10/// @brief Kalman filter matrices for the Integrated-Random-Walk (IRW) Multi-IMU fusion
11/// @author M. Maier (marcel.maier@ins.uni-stuttgart.de)
12/// @date 2024-04-02
13
14#pragma once
15
16#include "util/Eigen.hpp"
17#include "../PinData.hpp"
20
21#include <vector>
22
23// TODO: Refactor: 'NAV::ImuFusion::IRWKF'
24namespace NAV::IRWKF
25{
26/// @brief Calculates the state-transition-matrix 𝚽
27/// @param[in] dt Time difference between two successive measurements
28/// @param[in] numStates Number of KF states
29/// @return State-transition-matrix 𝚽
30[[nodiscard]] Eigen::MatrixXd initialStateTransitionMatrix_Phi(const double& dt, const uint8_t& numStates);
31
32/// @brief Initial error covariance matrix P_0
33/// @param[in] initCovariances Initial covariances of the states
34/// @param[in] numStates Number of KF states
35/// @param[in] imuCharacteristicsIdentical If the multiple IMUs have the same characteristics, GUI input cells can be reduced considerably
36/// @return The (numStates) x (numStates) matrix of initial state variances
37[[nodiscard]] Eigen::MatrixXd initialErrorCovarianceMatrix_P0(const std::vector<Eigen::Vector3d>& initCovariances, const uint8_t& numStates, const bool& imuCharacteristicsIdentical = false);
38
39/// @brief Calculates the process noise matrix Q
40/// @param[in] Q Process noise matrix of the previous time step
41/// @param[in] dt Time difference between two successive measurements
42/// @param[in] processNoiseVariances Vector that contains the variances for each state's process noise
43/// @param[in] numStates Number of KF states
44/// @param[in] imuCharacteristicsIdentical If the multiple IMUs have the same characteristics, GUI input cells can be reduced considerably
45void processNoiseMatrix_Q(Eigen::MatrixXd& Q, const double& dt, const std::vector<Eigen::VectorXd>& processNoiseVariances, const uint8_t& numStates, const bool& imuCharacteristicsIdentical = false);
46
47/// @brief Calculates the state-transition-matrix 𝚽
48/// @param[in] Phi State transition matrix from previous iteration. Returns the matrix for the current iteration.
49/// @param[in] dt Time difference between two successive measurements
50void stateTransitionMatrix_Phi(Eigen::MatrixXd& Phi, const double& dt);
51
52/// @brief Calculates the design matrix H
53/// @param[in] DCM_accel Rotation matrix of mounting angles of an accelerometer w.r.t. a common reference
54/// @param[in] DCM_gyro Rotation matrix of mounting angles of a gyroscope w.r.t. a common reference
55/// @param[in] pinIndex Index of pin to identify sensor
56/// @param[in] numMeasurements Number of measurements
57/// @param[in] numStates Number of KF states
58/// @param[in] numStatesEst Number of estimated states (depends on imuFusionType)
59/// @param[in] numStatesPerPin Number of states per pin (biases of accel and gyro)
60/// @return Design matrix H
61[[nodiscard]] Eigen::MatrixXd designMatrix_H(const Eigen::Matrix3d& DCM_accel,
62 const Eigen::Matrix3d& DCM_gyro,
63 const size_t& pinIndex,
64 const uint8_t& numMeasurements,
65 const uint8_t& numStates,
66 const uint8_t& numStatesEst,
67 const uint8_t& numStatesPerPin);
68
69/// @brief Initializes the IRWKF manually (i.e. using GUI inputs instead of averaging)
70/// @param[in] numInputPins Number of input pins of the IMU fusion node
71/// @param[in] pinData pin data (i.e. GUI settings for the IRWKF with manual initialization)
72/// @param[in] pinDataIRWKF pin data specific to the IRWKF
73/// @param[in] numStates Number of states of the IRWKF
74/// @param[in] dtInit Initial value for dt (discrete state transition time)
75/// @param[in] processNoiseVariances Container for process noise of each state
76/// @param[in] kalmanFilter Initial IRWKF object, which has the size defined in the GUI (numStates and numMeasurements)
77/// @param[in] imuCharacteristicsIdentical If the multiple IMUs have the same characteristics, GUI input cells can be reduced considerably
78/// @param[in] imuBiasesIdentical If the multiple IMUs have the same characteristics, GUI input cells can be reduced considerably
79/// @return Initialized IRWKF object
80KalmanFilter initializeKalmanFilterManually(const size_t& numInputPins,
81 const std::vector<PinData>& pinData,
82 const PinDataIRWKF& pinDataIRWKF,
83 const uint8_t& numStates,
84 const double& dtInit,
85 std::vector<Eigen::VectorXd>& processNoiseVariances,
86 KalmanFilter& kalmanFilter,
87 const bool& imuCharacteristicsIdentical,
88 const bool& imuBiasesIdentical);
89
90/// @brief Initializes the IRWKF automatically, i.e. init values are calculated by averaging the data in the first T seconds
91/// @param[in] nInputPins Number of input pins of the IMU fusion node
92/// @param[in] pinData pin data (i.e. GUI settings for the IRWKF)
93/// @param[in] pinDataIRWKF pin data (i.e. GUI settings specific to the IRWKF)
94/// @param[in] cumulatedPinIds Container that collects all pinIds for averaging for auto-init of the KF
95/// @param[in] cumulatedImuObs Container that collects all imuObs for averaging for auto-init of the KF
96/// @param[in] initJerkAngAcc flag to determine how jerk and angular acceleration states are initialized if '_autoInitKF = true'
97/// @param[in] dtInit Initial value for dt (discrete state transition time)
98/// @param[in] processNoiseVariances Container for process noise of each state
99/// @param[in] numStates Number of states of the IRWKF
100/// @param[in] numMeasurements Number of measurements of the IRWKF
101/// @param[in] kalmanFilter Initial IRWKF object, which has the size defined in the GUI (numStates and numMeasurements)
102/// @return Initialized IRWKF object
103KalmanFilter initializeKalmanFilterAuto(const size_t& nInputPins,
104 const std::vector<PinData>& pinData,
105 const PinDataIRWKF& pinDataIRWKF,
106 const std::vector<size_t>& cumulatedPinIds,
107 const std::vector<std::shared_ptr<const NAV::ImuObs>>& cumulatedImuObs,
108 const bool& initJerkAngAcc,
109 const double& dtInit,
110 const uint8_t& numStates,
111 const uint8_t& numMeasurements,
112 std::vector<Eigen::VectorXd>& processNoiseVariances,
113 KalmanFilter& kalmanFilter);
114
115/// @brief Calculates the mean values of each axis in a vector that contains 3d measurements of a certain sensor type
116/// @param[in] sensorType type of measurement, i.e. Acceleration or gyro measurements in 3d (axisIndex / msgIndex)
117/// @param[in] containerPos position-Index in 'sensorType' where data starts (e.g. Accel at 0, Gyro at 3)
118/// @return Vector of mean values in 3d of a certain sensor type
119Eigen::Vector3d mean(const std::vector<std::vector<double>>& sensorType, const size_t& containerPos);
120
121/// @brief Calculates the variance of each axis in a vector that contains 3d measurements of a certain sensor type
122/// @param[in] sensorType type of measurement, i.e. Acceleration or gyro measurements in 3d (axisIndex / msgIndex)
123/// @param[in] containerPos position-Index in 'sensorType' where data starts (e.g. Accel at 0, Gyro at 3)
124/// @return Vector of variance values in 3d of a certain sensor type
125Eigen::Vector3d variance(const std::vector<std::vector<double>>& sensorType, const size_t& containerPos);
126} // namespace NAV::IRWKF
Vector space operations.
Parent Class for all IMU Observations.
Generalized Kalman Filter class.
Information about a sensor which is connected to a certain pin.
Generalized Kalman Filter class.
void stateTransitionMatrix_Phi(Eigen::MatrixXd &Phi, const double &dt)
Calculates the state-transition-matrix 𝚽
Definition IRWKF.cpp:76
Eigen::MatrixXd initialStateTransitionMatrix_Phi(const double &dt, const uint8_t &numStates)
Calculates the state-transition-matrix 𝚽
Definition IRWKF.cpp:14
Eigen::MatrixXd designMatrix_H(const Eigen::Matrix3d &DCM_accel, const Eigen::Matrix3d &DCM_gyro, const size_t &pinIndex, const uint8_t &numMeasurements, const uint8_t &numStates, const uint8_t &numStatesEst, const uint8_t &numStatesPerPin)
Calculates the design matrix H.
Definition IRWKF.cpp:82
void processNoiseMatrix_Q(Eigen::MatrixXd &Q, const double &dt, const std::vector< Eigen::VectorXd > &processNoiseVariances, const uint8_t &numStates, const bool &imuCharacteristicsIdentical=false)
Calculates the process noise matrix Q.
Definition IRWKF.cpp:49
Eigen::Vector3d mean(const std::vector< std::vector< double > > &sensorType, const size_t &containerPos)
Calculates the mean values of each axis in a vector that contains 3d measurements of a certain sensor...
Definition IRWKF.cpp:541
KalmanFilter initializeKalmanFilterManually(const size_t &numInputPins, const std::vector< PinData > &pinData, const PinDataIRWKF &pinDataIRWKF, const uint8_t &numStates, const double &dtInit, std::vector< Eigen::VectorXd > &processNoiseVariances, KalmanFilter &kalmanFilter, const bool &imuCharacteristicsIdentical, const bool &imuBiasesIdentical)
Initializes the IRWKF manually (i.e. using GUI inputs instead of averaging)
Definition IRWKF.cpp:102
Eigen::Vector3d variance(const std::vector< std::vector< double > > &sensorType, const size_t &containerPos)
Calculates the variance of each axis in a vector that contains 3d measurements of a certain sensor ty...
Definition IRWKF.cpp:555
Eigen::MatrixXd initialErrorCovarianceMatrix_P0(const std::vector< Eigen::Vector3d > &initCovariances, const uint8_t &numStates, const bool &imuCharacteristicsIdentical=false)
Initial error covariance matrix P_0.
Definition IRWKF.cpp:25
KalmanFilter initializeKalmanFilterAuto(const size_t &nInputPins, const std::vector< PinData > &pinData, const PinDataIRWKF &pinDataIRWKF, const std::vector< size_t > &cumulatedPinIds, const std::vector< std::shared_ptr< const NAV::ImuObs > > &cumulatedImuObs, const bool &initJerkAngAcc, const double &dtInit, const uint8_t &numStates, const uint8_t &numMeasurements, std::vector< Eigen::VectorXd > &processNoiseVariances, KalmanFilter &kalmanFilter)
Initializes the IRWKF automatically, i.e. init values are calculated by averaging the data in the fir...
Definition IRWKF.cpp:357
Sensor information specific to the IRW-KF.
Definition PinData.hpp:105