Motion System Model.
More...
|
std::pair< KeyedMatrix6d< StateKeyType >, KeyedMatrix6d< StateKeyType > > | calcPhiAndQ (double dt, const double &latitude, const double &longitude, SystemModelCalcAlgorithm algorithm) |
| Calculates the state transition matrix (𝚽) and the process noise covariance matrix (𝐐)
|
|
void | initialize () |
| Initializes the motion model.
|
|
template<typename Scalar, int Size> |
void | initialize (KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &F, KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &W) |
| Initializes the motion model.
|
|
bool | ShowGui (float itemWidth, float unitWidth, const char *id) |
| Shows a GUI.
|
|
template<typename Scalar, int Size> |
void | updatePhiAndQ (KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &Phi, KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &Q, KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &G, const KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &F, const KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &W, double dt, const double &latitude, const double &longitude, SystemModelCalcAlgorithm algorithm) |
| Updates the provided Phi, Q and G matrix.
|
|
|
std::array< double, 2 > | _covarianceAccel |
| Covariance of the acceleration 𝜎_a due to user motion in horizontal and vertical component [m²/s³].
|
|
std::array< double, 2 > | _gui_covarianceAccel |
| GUI selection for the Standard deviation of the acceleration 𝜎_a due to user motion in horizontal and vertical component.
|
|
Units::CovarianceAccelUnits | _gui_covarianceAccelUnit |
| Gui selection for the Unit of the input covarianceAccel parameter for the StDev due to acceleration due to user motion.
|
|
const std::array< StateKeyType, 3 > & | Pos |
| All position keys.
|
|
const std::array< StateKeyType, 6 > & | PosVel |
| All position and velocity keys.
|
|
const std::array< StateKeyType, 3 > & | Vel |
| All velocity keys.
|
|
template<typename StateKeyType>
class NAV::MotionModel< StateKeyType >
Motion System Model.
Definition at line 79 of file MotionModel.hpp.
◆ calcPhiAndQ()
template<typename StateKeyType>
Calculates the state transition matrix (𝚽) and the process noise covariance matrix (𝐐)
- Parameters
-
[in] | dt | Time step size in [s] |
[in] | latitude | Latitude [rad] |
[in] | longitude | Longitude [rad] |
[in] | algorithm | Algorithm to use for the calculation |
- Returns
- Phi and Q matrix
Definition at line 151 of file MotionModel.hpp.
◆ calcProcessNoiseMatrixTaylor()
template<typename StateKeyType>
KeyedMatrix6d< StateKeyType > NAV::MotionModel< StateKeyType >::calcProcessNoiseMatrixTaylor |
( |
double | dt, |
|
|
double | latitude, |
|
|
double | longitude ) const |
|
inlinenodiscardprivate |
Calculates the process noise matrix Q.
- Parameters
-
[in] | dt | Time step [s] |
[in] | latitude | Latitude [rad] |
[in] | longitude | Longitude [rad] |
- Note
- See [17] Groves, ch. 9.4.2.2, eq. 9.152, p. 417
Definition at line 218 of file MotionModel.hpp.
◆ initialize() [1/2]
template<typename StateKeyType>
◆ initialize() [2/2]
template<typename StateKeyType>
template<typename Scalar, int Size>
Initializes the motion model.
- Parameters
-
[in,out] | F | System model matrix |
[in,out] | W | Noise scale matrix |
Definition at line 96 of file MotionModel.hpp.
◆ ShowGui()
template<typename StateKeyType>
bool NAV::MotionModel< StateKeyType >::ShowGui |
( |
float | itemWidth, |
|
|
float | unitWidth, |
|
|
const char * | id ) |
|
inline |
Shows a GUI.
- Parameters
-
[in] | itemWidth | Width of the space for the config items |
[in] | unitWidth | Width of the units |
[in] | id | Unique id for ImGui |
- Returns
- True if something was changed
Definition at line 170 of file MotionModel.hpp.
◆ updatePhiAndQ()
template<typename StateKeyType>
template<typename Scalar, int Size>
void NAV::MotionModel< StateKeyType >::updatePhiAndQ |
( |
KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > & | Phi, |
|
|
KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > & | Q, |
|
|
KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > & | G, |
|
|
const KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > & | F, |
|
|
const KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > & | W, |
|
|
double | dt, |
|
|
const double & | latitude, |
|
|
const double & | longitude, |
|
|
SystemModelCalcAlgorithm | algorithm ) |
|
inline |
Updates the provided Phi, Q and G matrix.
- Parameters
-
[in,out] | Phi | State transition matrix |
[in,out] | Q | System/Process noise covariance matrix |
[in,out] | G | Noise input matrix |
[in] | F | System model matrix |
[in] | W | Noise scale matrix |
[in] | dt | Time step size in [s] |
[in] | latitude | Latitude [rad] |
[in] | longitude | Longitude [rad] |
[in] | algorithm | Algorithm to use for the calculation |
Definition at line 116 of file MotionModel.hpp.
◆ from_json
template<typename StateKeyType>
Converts the provided json object into the data object.
- Parameters
-
[in] | j | Json object with the needed values |
[out] | data | Object to fill from the json |
Definition at line 263 of file MotionModel.hpp.
◆ to_json
template<typename StateKeyType>
Converts the provided data into a json object.
- Parameters
-
[out] | j | Json object which gets filled with the info |
[in] | data | Data to convert into json |
Definition at line 253 of file MotionModel.hpp.
◆ _covarianceAccel
template<typename StateKeyType>
Covariance of the acceleration 𝜎_a due to user motion in horizontal and vertical component [m²/s³].
Definition at line 248 of file MotionModel.hpp.
◆ _gui_covarianceAccel
template<typename StateKeyType>
GUI selection for the Standard deviation of the acceleration 𝜎_a due to user motion in horizontal and vertical component.
- Note
- See Groves (2013) eq. (9.156)
Definition at line 245 of file MotionModel.hpp.
◆ _gui_covarianceAccelUnit
template<typename StateKeyType>
Gui selection for the Unit of the input covarianceAccel parameter for the StDev due to acceleration due to user motion.
Definition at line 242 of file MotionModel.hpp.
◆ Pos
template<typename StateKeyType>
◆ PosVel
template<typename StateKeyType>
◆ Vel
template<typename StateKeyType>
The documentation for this class was generated from the following file:
- /home/runner/work/INSTINCT/INSTINCT/src/Navigation/GNSS/SystemModel/MotionModel.hpp