Inertial Navigation Mechanization Functions in local navigation frame. More...

Functions
template<typename DerivedA, typename DerivedB>
Eigen::Vector4< typename DerivedA::Scalar >	NAV::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.

template<typename Derived>
Eigen::Vector3< typename Derived::Scalar >	NAV::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.

template<typename T>
Eigen::Vector< T, 10 >	NAV::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.

template<typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD>
Eigen::Vector3< typename DerivedA::Scalar >	NAV::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.

Detailed Description

Inertial Navigation Mechanization Functions in local navigation frame.

Author: T. Topp (topp@.nosp@m.ins..nosp@m.uni-s.nosp@m.tutt.nosp@m.gart..nosp@m.de)

Date: 2020-09-02

Function Documentation

◆ calcTimeDerivativeFor_n_Quat_b()

template<typename DerivedA, typename DerivedB>

Eigen::Vector4< typename DerivedA::Scalar > NAV::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.

$\begin{equation} \label{eq:eq-INS-Mechanization-n_Quat_b-dot} \mathbf{\dot{q}}_b^n = \begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{z} \\ \dot{w} \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & \omega_{nb,z}^b & -\omega_{nb,y}^b & \omega_{nb,x}^b \\ -\omega_{nb,z}^b & 0 & \omega_{nb,x}^b & \omega_{nb,y}^b \\ \omega_{nb,y}^b & -\omega_{nb,x}^b & 0 & \omega_{nb,z}^b \\ -\omega_{nb,x}^b & -\omega_{nb,y}^b & -\omega_{nb,z}^b & 0 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ w \end{bmatrix} \end{equation}$

Parameters

[in]	b_omega_nb	ω_nb_b Body rate with respect to the navigation frame, expressed in the body frame
[in]	n_Quat_b_coeffs	Coefficients of the quaternion n_Quat_b in order x, y, z, w (q = w + ix + jy + kz)

Returns: The time derivative of the coefficients of the quaternion n_Quat_b in order x, y, z, w (q = w + ix + jy + kz)

Note: See Propagation of quaternion with time equation $\eqref{eq:eq-ImuIntegrator-Mechanization-n-Attitude-Quaternion-matrix-Titterton}$

◆ lla_calcTimeDerivativeForPosition()

template<typename Derived>

Eigen::Vector3< typename Derived::Scalar > NAV::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.

$\begin{equation} \label{eq:eq-INS-Mechanization-p_lla-dot} \begin{aligned} \dot{\phi} &= \frac{v_N}{R_N + h} \\ \dot{\lambda} &= \frac{v_E}{(R_E + h) \cos{\phi}} \\ \dot{h} &= -v_D \end{aligned} \end{equation}$

Parameters

[in]	n_velocity	[v_N v_E v_D]^T Velocity with respect to the Earth in local-navigation frame coordinates [m/s]
[in]	phi	ϕ Latitude [rad]
[in]	h	Altitude above the ellipsoid [m]
[in]	R_N	North/South (meridian) earth radius [m]
[in]	R_E	East/West (prime vertical) earth radius [m]

Returns: The time derivative of the curvilinear position

Note: See Position equation $\eqref{eq:eq-ImuIntegrator-Mechanization-n-Position}$

◆ n_calcPosVelAttDerivative()

template<typename T>

Eigen::Vector< T, 10 > NAV::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.

Parameters

[in]	y	[ 𝜙, λ, h, v_N, v_E, v_D, n_q_bx, n_q_by, n_q_bz, n_q_bw]^T
[in]	z	[fx, fy, fz, ωx, ωy, ωz]^T
[in]	c	Constant values needed to calculate the derivatives

Returns: The derivative ∂/∂t [ 𝜙, λ, h, v_N, v_E, v_D, n_q_bx, n_q_by, n_q_bz, n_q_bw]^T

◆ n_calcTimeDerivativeForVelocity()

template<typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD>

Eigen::Vector3< typename DerivedA::Scalar > NAV::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.

$\begin{equation} \label{eq:eq-INS-Mechanization-v_n-dot} \boldsymbol{\dot{v}}^n = \overbrace{\boldsymbol{f}^n}^{\hidewidth\text{measured}\hidewidth} -\ \underbrace{(2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{v}^n}_{\text{coriolis acceleration}} +\ \overbrace{\mathbf{g}^n}^{\hidewidth\text{gravitation}\hidewidth} -\ \mathbf{C}_e^n \cdot \underbrace{\left(\boldsymbol{\omega}_{ie}^e \times [ \boldsymbol{\omega}_{ie}^e \times \mathbf{x}^e ] \right)}_{\text{centrifugal acceleration}} \end{equation}$

Parameters

[in]	n_measuredForce	f_n = [f_N f_E f_D]^T Specific force vector as measured by a triad of accelerometers and resolved into local-navigation frame coordinates
[in]	n_coriolisAcceleration	Coriolis acceleration in local-navigation coordinates in [m/s^2]
[in]	n_gravitation	Local gravitation vector (caused by effects of mass attraction) in local-navigation frame coordinates [m/s^2]
[in]	n_centrifugalAcceleration	Centrifugal acceleration in local-navigation coordinates in [m/s^2]

Returns: The time derivative of the velocity in local-navigation frame coordinates

Note: See Velocity equation $\eqref{eq:eq-ImuIntegrator-Mechanization-n-Velocity}$