Attitude System Model.
More...
|
template<typename Derived> |
std::pair< KeyedMatrix4d< StateKeyType >, KeyedMatrix4d< StateKeyType > > | calcPhiAndQ (double dt, const double &latitude, const double &longitude, const Eigen::QuaternionBase< Derived > &n_quat_b) |
| Calculates the state transition matrix (π½) and the process noise covariance matrix (π)
|
|
void | initialize () |
| Initializes the attitude model.
|
|
bool | ShowGui (float itemWidth, float unitWidth, const char *id) |
| Shows a GUI.
|
|
template<typename Scalar, int Size, typename Derived> |
void | updatePhiAndQ (KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &Phi, KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &Q, double dt, const double &latitude, const double &longitude, const Eigen::QuaternionBase< Derived > &n_quat_b) |
| Updates the provided Phi, Q and G matrix.
|
|
|
template<typename Derived> |
KeyedMatrix4d< StateKeyType > | calcProcessNoiseMatrix (double dt, double latitude, double longitude, const Eigen::QuaternionBase< Derived > &n_quat_b) const |
| Calculates the process noise matrix Q.
|
|
|
Eigen::Vector3d | _covarianceAngularVelocity |
| Covariance of the angular velocity due to user motion (Roll, Pitch, Yaw) [radΒ²/s].
|
|
Eigen::Vector3d | _gui_covarianceAngularVelocity |
| GUI selection for the Standard deviation of the angular velocity due to user motion (Roll, Pitch, Yaw)
|
|
Units::CovarianceAngularVelocityUnits | _gui_covarianceAngularVelocityUnit |
| Gui selection for the Unit of the input covarianceAngularVelocity.
|
|
const std::array< StateKeyType, 4 > & | Att |
| All attitude keys.
|
|
template<typename StateKeyType>
class NAV::AttitudeModel< StateKeyType >
Attitude System Model.
◆ calcPhiAndQ()
template<typename StateKeyType>
template<typename Derived>
std::pair< KeyedMatrix4d< StateKeyType >, KeyedMatrix4d< StateKeyType > > NAV::AttitudeModel< StateKeyType >::calcPhiAndQ |
( |
double | dt, |
|
|
const double & | latitude, |
|
|
const double & | longitude, |
|
|
const Eigen::QuaternionBase< Derived > & | n_quat_b ) |
|
inlinenodiscard |
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] | n_quat_b | Quaternion body to navigation frame |
- Returns
- Phi and Q matrix
◆ calcProcessNoiseMatrix()
template<typename StateKeyType>
template<typename Derived>
KeyedMatrix4d< StateKeyType > NAV::AttitudeModel< StateKeyType >::calcProcessNoiseMatrix |
( |
double | dt, |
|
|
double | latitude, |
|
|
double | longitude, |
|
|
const Eigen::QuaternionBase< Derived > & | n_quat_b ) const |
|
inlinenodiscardprivate |
Calculates the process noise matrix Q.
- Parameters
-
[in] | dt | Time step [s] |
[in] | latitude | Latitude [rad] |
[in] | longitude | Longitude [rad] |
[in] | n_quat_b | Quaternion body to navigation frame |
◆ ShowGui()
template<typename StateKeyType>
bool NAV::AttitudeModel< 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
◆ updatePhiAndQ()
template<typename StateKeyType>
template<typename Scalar, int Size, typename Derived>
void NAV::AttitudeModel< StateKeyType >::updatePhiAndQ |
( |
KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > & | Phi, |
|
|
KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > & | Q, |
|
|
double | dt, |
|
|
const double & | latitude, |
|
|
const double & | longitude, |
|
|
const Eigen::QuaternionBase< Derived > & | n_quat_b ) |
|
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] | dt | Time step size in [s] |
[in] | latitude | Latitude [rad] |
[in] | longitude | Longitude [rad] |
[in] | n_quat_b | Quaternion body to navigation frame |
◆ 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 |
◆ 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 |
The documentation for this class was generated from the following file:
- /home/runner/work/INSTINCT/INSTINCT/src/Navigation/GNSS/SystemModel/AttitudeModel.hpp