51template<
typename DerivedA,
typename DerivedB>
53 const Eigen::MatrixBase<DerivedB>& n_Quat_b_coeffs)
55 using T =
typename DerivedA::Scalar;
61 A << T(0.0) , b_omega_nb(2), -b_omega_nb(1), b_omega_nb(0),
62 -b_omega_nb(2), T(0.0) , b_omega_nb(0), b_omega_nb(1),
63 b_omega_nb(1), -b_omega_nb(0), T(0.0) , b_omega_nb(2),
64 -b_omega_nb(0), -b_omega_nb(1), -b_omega_nb(2), T(0.0) ;
68 return 0.5 * A * n_Quat_b_coeffs;
88template<
typename DerivedA,
typename DerivedB,
typename DerivedC,
typename DerivedD>
90 const Eigen::MatrixBase<DerivedB>& n_coriolisAcceleration,
91 const Eigen::MatrixBase<DerivedC>& n_gravitation,
92 const Eigen::MatrixBase<DerivedD>& n_centrifugalAcceleration)
94 return n_measuredForce
95 - n_coriolisAcceleration
97 - n_centrifugalAcceleration;
118template<
typename Derived>
120 const auto& phi,
const auto& h,
121 const auto& R_N,
const auto& R_E)
124 const auto& v_N = n_velocity(0);
126 const auto& v_E = n_velocity(1);
128 const auto& v_D = n_velocity(2);
130 return { v_N / (R_N + h),
131 v_E / ((R_E + h) * std::cos(phi)),
145 Eigen::Vector<T, 10> y_dot = Eigen::Vector<T, 10>::Zero();
147 Eigen::Quaternion<T> n_Quat_b{ y.template segment<4>(6) };
148 n_Quat_b.normalize();
152 LOG_DATA(
"n_velocity = {} [m/s]", y.template segment<3>(3).transpose());
162 LOG_DATA(
"n_omega_ie = {} [rad/s]", n_omega_ie.transpose());
164 Eigen::Vector3<T> n_omega_en =
n_calcTransportRate(y.template segment<3>(0), y.template segment<3>(3), R_N, R_E);
165 LOG_DATA(
"n_omega_en = {} [rad/s]", n_omega_en.transpose());
167 Eigen::Vector3<T> b_omega_nb = z.template segment<3>(3)
168 - n_Quat_b.conjugate()
171 LOG_DATA(
"b_omega_nb = {} [rad/s]", b_omega_nb.transpose());
176 : Eigen::Vector3<T>::Zero();
177 LOG_DATA(
"n_coriolisAcceleration = {} [m/s^2]", n_coriolisAcceleration.transpose());
181 : Eigen::Vector3<T>::Zero();
182 LOG_DATA(
"n_centrifugalAcceleration = {} [m/s^2]", n_centrifugalAcceleration.transpose());
194 n_coriolisAcceleration,
196 n_centrifugalAcceleration);
199 y.template segment<4>(6));
201 LOG_DATA(
"lla_position_dot = {} [rad/s, rad/s, m/s]", y_dot.template segment<3>(0).transpose());
202 LOG_DATA(
"n_velocity_dot = {} [m/s^2]", y_dot.template segment<3>(3).transpose());
203 LOG_DATA(
"n_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.
Provides Numerical integration methods.
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::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 > quat2eulerZYX(const Eigen::QuaternionBase< Derived > &q)
Converts the quaternion to Euler rotation angles with rotation sequence ZYX.
Eigen::Vector3< typename Derived::Scalar > lla2ecef_WGS84(const Eigen::MatrixBase< Derived > &lla_position)
Converts latitude, longitude and altitude into Earth-centered-Earth-fixed coordinates using WGS84.
Eigen::Vector3< typename DerivedA::Scalar > n_calcTimeDerivativeForVelocity(const Eigen::MatrixBase< DerivedA > &n_measuredForce, const Eigen::MatrixBase< DerivedB > &n_coriolisAcceleration, const Eigen::MatrixBase< DerivedC > &n_gravitation, const Eigen::MatrixBase< DerivedD > &n_centrifugalAcceleration)
Calculates the time derivative of the velocity in local-navigation frame coordinates.
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::Vector3< typename DerivedA::Scalar > n_calcCoriolisAcceleration(const Eigen::MatrixBase< DerivedA > &n_omega_ie, const Eigen::MatrixBase< DerivedB > &n_omega_en, const Eigen::MatrixBase< DerivedC > &n_velocity)
Calculates the coriolis acceleration in [m/s^2] (acceleration due to motion in rotating reference fra...
Scalar calcEarthRadius_N(const Scalar &latitude, const Scalar &a=InsConst::WGS84::a, const Scalar &e_squared=InsConst::WGS84::e_squared)
Calculates the North/South (meridian) earth radius.
const char * to_string(gui::widgets::PositionWithFrame::ReferenceFrame refFrame)
Converts the enum to a string.
Eigen::Vector3< typename DerivedA::Scalar > n_calcTransportRate(const Eigen::MatrixBase< DerivedA > &lla_position, const Eigen::MatrixBase< DerivedB > &n_velocity, const auto &R_N, const auto &R_E)
Calculates the transport rate ω_en_n (rotation rate of the Earth frame relative to the navigation fra...
Eigen::Vector4< typename DerivedA::Scalar > calcTimeDerivativeFor_n_Quat_b(const Eigen::MatrixBase< DerivedA > &b_omega_nb, const Eigen::MatrixBase< DerivedB > &n_Quat_b_coeffs)
Calculates the time derivative of the quaternion n_Quat_b.
Eigen::Vector< T, 10 > n_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 curvilinear position.
Eigen::Vector3< typename Derived::Scalar > lla_calcTimeDerivativeForPosition(const Eigen::MatrixBase< Derived > &n_velocity, const auto &phi, const auto &h, const auto &R_N, const auto &R_E)
Calculates the time derivative of the curvilinear position.
constexpr auto rad2deg(const T &rad)
Convert Radians to Degree.
Scalar calcEarthRadius_E(const Scalar &latitude, const Scalar &a=InsConst::WGS84::a, const Scalar &e_squared=InsConst::WGS84::e_squared)
Calculates the East/West (prime vertical) earth radius.
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.
bool angularRateTransportRateCompensationEnabled
Apply the transport rate compensation to the measured angular rates.
GravitationModel gravitationModel
Gravity Model to use.
bool coriolisAccelerationCompensationEnabled
Apply the Coriolis acceleration compensation to the measured accelerations.