50template<
typename DerivedA,
typename DerivedB>
52 const Eigen::MatrixBase<DerivedB>& e_Quat_b_coeffs)
54 using T =
typename DerivedA::Scalar;
60 A << T(0.0) , b_omega_eb(2), -b_omega_eb(1), b_omega_eb(0),
61 -b_omega_eb(2), T(0.0) , b_omega_eb(0), b_omega_eb(1),
62 b_omega_eb(1), -b_omega_eb(0), T(0.0) , b_omega_eb(2),
63 -b_omega_eb(0), -b_omega_eb(1), -b_omega_eb(2), T(0.0) ;
67 return 0.5 * A * e_Quat_b_coeffs;
87template<
typename DerivedA,
typename DerivedB,
typename DerivedC,
typename DerivedD>
89 const Eigen::MatrixBase<DerivedB>& e_coriolisAcceleration,
90 const Eigen::MatrixBase<DerivedC>& e_gravitation,
91 const Eigen::MatrixBase<DerivedD>& e_centrifugalAcceleration)
93 return e_measuredForce
94 - e_coriolisAcceleration
96 - e_centrifugalAcceleration;
109template<
typename Derived>
125 Eigen::Vector<T, 10> y_dot = Eigen::Vector<T, 10>::Zero();
129 Eigen::Quaternion<T> e_Quat_b{ y.template segment<4>(6) };
130 e_Quat_b.normalize();
131 Eigen::Quaternion<T> n_Quat_e =
trafo::n_Quat_e(lla_position(0), lla_position(1));
133 Eigen::Quaternion<T> b_Quat_e = e_Quat_b.conjugate();
134 Eigen::Quaternion<T> e_Quat_n = n_Quat_e.conjugate();
136 LOG_DATA(
"e_velocity = {} [m/s]", y.template segment<3>(3).transpose());
137 LOG_DATA(
"e_position = {} [m]", y.template segment<3>(0).transpose());
140 Eigen::Vector3<T> b_omega_eb = z.template segment<3>(3)
142 LOG_DATA(
"b_omega_eb = {} [rad/s]", b_omega_eb.transpose());
147 : Eigen::Vector3<T>::Zero();
148 LOG_DATA(
"e_coriolisAcceleration = {} [m/s^2]", e_coriolisAcceleration.transpose());
152 : Eigen::Vector3<T>::Zero();
153 LOG_DATA(
"e_centrifugalAcceleration = {} [m/s^2]", e_centrifugalAcceleration.transpose());
161 e_coriolisAcceleration,
163 e_centrifugalAcceleration);
166 y.template segment<4>(6));
168 LOG_DATA(
"e_position_dot = {} [m/s]", y_dot.template segment<3>(0).transpose());
169 LOG_DATA(
"e_velocity_dot = {} [m/s^2]", y_dot.template segment<3>(3).transpose());
170 LOG_DATA(
"e_Quat_b_dot = {} ", y_dot.template segment<4>(6).transpose());
Transformation collection.
Functions concerning the ellipsoid model.
Different Gravity Models.
Inertial Navigation Helper Functions.
Utility class for logging to console and file.
#define LOG_DATA
All output which occurs repeatedly every time observations are received.
Inertial Navigation Mechanization Functions.
static const Eigen::Vector3< double > e_omega_ie
ω_ie_e = ω_ie_i Nominal mean angular velocity of the Earth in [rad/s], in earth coordinates
Eigen::Vector3< typename Derived::Scalar > ecef2lla_WGS84(const Eigen::MatrixBase< Derived > &e_position)
Converts Earth-centered-Earth-fixed coordinates into latitude, longitude and altitude using WGS84.
Eigen::Quaternion< Scalar > n_Quat_e(const Scalar &latitude, const Scalar &longitude)
Quaternion for rotations from Earth-fixed to navigation frame.
Eigen::Vector3< typename Derived::Scalar > n_calcGravitation(const Eigen::MatrixBase< Derived > &lla_position, GravitationModel gravitationModel=GravitationModel::EGM96)
Calculates the gravitation (acceleration due to mass attraction of the Earth)
Eigen::Vector3< typename DerivedA::Scalar > e_calcCentrifugalAcceleration(const Eigen::MatrixBase< DerivedA > &e_position, const Eigen::MatrixBase< DerivedB > &e_omega_ie=InsConst::e_omega_ie)
Calculates the centrifugal acceleration in [m/s^2] (acceleration that makes a body follow a curved pa...
Eigen::Vector4< typename DerivedA::Scalar > calcTimeDerivativeFor_e_Quat_b(const Eigen::MatrixBase< DerivedA > &b_omega_eb, const Eigen::MatrixBase< DerivedB > &e_Quat_b_coeffs)
Calculates the time derivative of the quaternion e_Quat_b.
const char * to_string(gui::widgets::PositionWithFrame::ReferenceFrame refFrame)
Converts the enum to a string.
Eigen::Vector3< typename DerivedB::Scalar > e_calcCoriolisAcceleration(const Eigen::MatrixBase< DerivedA > &e_omega_ie, const Eigen::MatrixBase< DerivedB > &e_velocity)
Calculates the coriolis acceleration in [m/s^2] (acceleration due to motion in rotating reference fra...
Eigen::Vector< T, 10 > e_calcPosVelAttDerivative(const Eigen::Vector< T, 10 > &y, const Eigen::Vector< T, 6 > &z, const PosVelAttDerivativeConstants &c, double=0.0)
Calculates the derivative of the quaternion, velocity and position in ECEF coordinates.
Eigen::Vector3< typename DerivedA::Scalar > e_calcTimeDerivativeForVelocity(const Eigen::MatrixBase< DerivedA > &e_measuredForce, const Eigen::MatrixBase< DerivedB > &e_coriolisAcceleration, const Eigen::MatrixBase< DerivedC > &e_gravitation, const Eigen::MatrixBase< DerivedD > &e_centrifugalAcceleration)
Calculates the time derivative of the velocity in ECEF frame coordinates.
Eigen::Vector3< typename Derived::Scalar > e_calcTimeDerivativeForPosition(const Eigen::MatrixBase< Derived > &e_velocity)
Calculates the time derivative of the ECEF position.
Values needed to calculate the PosVelAttDerivative for the local-navigation frame.
bool centrifgalAccelerationCompensationEnabled
Apply the centrifugal acceleration compensation to the measured accelerations.
bool angularRateEarthRotationCompensationEnabled
Apply the Earth rotation rate compensation to the measured angular rates.
GravitationModel gravitationModel
Gravity Model to use.
bool coriolisAccelerationCompensationEnabled
Apply the Coriolis acceleration compensation to the measured accelerations.