 |
0.2.0
|
|
0.2.0
|
The navigation equation for a local-navigation frame is (see [45] Titterton, ch. 3.7.1, eq. 3.69, p. 47f)
\begin{equation} \boldsymbol{\dot{v}}^n = \boldsymbol{f}^n - \underbrace{(2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{v}^n}_{\text{coriolis acceleration}} +\ \mathbf{g}_l^n \end{equation}
where
Therefore equation eq-ImuSimulator-VelocityNav-equation-pre becomes
\begin{equation} \underbrace{\boldsymbol{\dot{v}}^n}_{\text{trajectory}} = \overbrace{\boldsymbol{f}^n}^{\text{measured}} -\ \underbrace{(2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{v}^n}_{\text{coriolis acceleration}} +\ \overbrace{\mathbf{g}^n}^{\text{gravitation}} -\ \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}
which can be reordered to calculate the measured accelerations with
\begin{equation} \underbrace{\boldsymbol{f}^n}_{\text{measured}} = \overbrace{\boldsymbol{\dot{v}}^n}^{\text{trajectory}} + \underbrace{(2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{v}^n}_{\text{coriolis acceleration}} -\ \overbrace{\mathbf{g}^n}^{\text{gravitation}} +\ \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}
The acceleration from the trajectory is derived as
\begin{equation} \boldsymbol{\dot{v}}^n = \frac{\partial}{\partial t} \left( \boldsymbol{\dot{x}}^n \right) = \frac{\partial}{\partial t} \left( \boldsymbol{C}_e^n \cdot \boldsymbol{\dot{x}}^e \right) = \boldsymbol{\dot{C}}_e^n \cdot \boldsymbol{\dot{x}}^e + \boldsymbol{C}_e^n \cdot \boldsymbol{\ddot{x}}^e \end{equation}
\begin{equation} \boldsymbol{\dot{C}}_e^n = (\boldsymbol{\dot{C}}_n^e)^T = (\boldsymbol{C}_n^e \cdot \boldsymbol{\Omega}_{en}^n)^T \end{equation}
Gyroscopes measure the turn rate of the platform frame with respect to an inertial frame expressed in platform coordinates
\begin{equation} \begin{aligned} \boldsymbol{\omega}_{ip}^p &= \boldsymbol{\omega}_{in}^p + \boldsymbol{\omega}_{np}^p \\ &= \mathbf{C}_n^p \cdot [ \boldsymbol{\omega}_{in}^n + \boldsymbol{\omega}_{np}^n ] \\ &= \mathbf{C}_n^p \cdot [ ( \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n ) + ( \boldsymbol{\omega}_{nb}^n + \overbrace{\boldsymbol{\omega}_{bp}^n}^{= 0} ) ] \\ &= \mathbf{C}_n^p \cdot [ \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n + \boldsymbol{\omega}_{nb}^n ] \\ &= \mathbf{C}_n^p \cdot [ \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n + \mathbf{C}_b^n \boldsymbol{\omega}_{nb}^b ] \end{aligned} \end{equation}
The turn rate of the body frame with respect to the local-navigation frame expressed in body coordinates depends on the time derivation of the gimbal angles \( \phi \) (roll), \( \theta \) (pitch), \( \psi \) (yaw):
\begin{equation} \boldsymbol{\omega}_{nb}^b = \begin{bmatrix} \dot{R} \\ 0 \\ 0 \end{bmatrix} + \mathbf{C_3} \begin{bmatrix} 0 \\ \dot{P} \\ 0 \end{bmatrix} + \mathbf{C_3} \mathbf{C_2} \begin{bmatrix} 0 \\ 0 \\ \dot{Y} \end{bmatrix} \end{equation}
where
see
The time derivative of the gimbal angles are calculated via spline derivatives.
All trajectories are calculated with a cubic spline. The spline points are calculated by the position formulas below with a certain spacing and then the velocity and acceleration is derived from it.
The fixed trajectory describes a geostationary position on Earth.
\begin{equation} \mathbf{x}^n = \begin{pmatrix} \phi & \lambda & h \end{pmatrix}^T = \text{const} \end{equation}
\begin{equation} \boldsymbol{v}^n = \mathbf{\dot{x}}^n = 0 \end{equation}
\begin{equation} \mathbf{a}^n = \boldsymbol{\dot{v}}^n = 0 \end{equation}
The linear trajectory describes a vehicle moving with constant velocity in local-navigation coordinates.
Position
The time derivative of the curvilinear position (lat, lon, alt) is given by ([17] Groves, ch. 2.4.2, eq. 2.111, p. 61 or [45] Titterton, ch. 3.7, eq. 3.81,3.85,3.86, p. 48ff)
\begin{equation} \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}
As the velocity in local-navigation coordinates is constant and neglecting the change in non velocity terms, the position can be expressed as
\begin{equation} \renewcommand*{\arraystretch}{2.0} \mathbf{x}^n = \begin{pmatrix} \phi \\ \lambda \\ h \end{pmatrix} \approx \begin{pmatrix} \phi_0 \\ \lambda_0 \\ h_0 \end{pmatrix} + \begin{pmatrix} \dfrac{v_N}{R_N + h} \\ \dfrac{v_E}{(R_E + h) \cos{\phi}} \\ -v_D \end{pmatrix} \cdot t \end{equation}
\begin{equation} \boldsymbol{v}^n = \mathbf{\dot{x}}^n = const \end{equation}
\begin{equation} \mathbf{a}^n = \boldsymbol{\dot{v}}^n = 0 \end{equation}
The circular or helical trajectory describes a vehicle moving with constant horizontal speed on a circular shaped path.
\begin{equation} \begin{aligned} \left|\boldsymbol{v}_{\text{h}}^n\right| &= \text{const} \end{aligned} \end{equation}
The vertical velocity is
Position
In the local-navigation frame in a tangential plane to the Earth surface a circular path can be expressed as
\begin{equation} \mathbf{x}^n = \begin{pmatrix} x_N \\ x_E \end{pmatrix} = \mathbf{x}_M^n + \begin{pmatrix} r \cdot \sin{\left( \varphi \right)} \\ r \cdot \cos{\left( \varphi \right)} \end{pmatrix} \end{equation}
where \( x_M \) is the center of the circle.
The vertical position is \( x_D = x_{M,D} - v_{UP} \cdot t \).
The variable \( \varphi \) can be expressed due to the constant speed
\begin{equation} \left|\boldsymbol{v}_{\text{h}}^n\right| = \frac{s}{t} = \left( \frac{\tilde{\varphi}}{2\pi} \cdot U \right) \frac{1}{t} = \left( \frac{\tilde{\varphi}}{2\pi} \cdot 2\pi r \right) \frac{1}{t} = \frac{\tilde{\varphi} \cdot r}{t} \end{equation}
Also a starting angle \( \varphi_0 \) and direction term \( \text{dir} \) (-1 for CW and +1 for CCW) are added.
\begin{equation} \varphi = \frac{\left|\boldsymbol{v}_{\text{h}}^n\right| \cdot t}{r} \cdot \text{dir} + \varphi_0 \end{equation}
As perfekt circular trajectories are hard for Navigation algorithms to estimate, due to the lack of dynamic elements, a harmonic modulation is added to the position, which leads to
\begin{equation} \begin{aligned} \mathbf{x}^n = \begin{pmatrix} x_N \\ x_E \\ x_D\end{pmatrix} = \mathbf{x}_M^n + \begin{pmatrix} r \cdot \sin{\left( \varphi \right)} \cdot \left( 1 + a_h \cdot \sin{\left( \varphi \cdot \omega_h \right)} \right) \\ r \cdot \cos{\left( \varphi \right)} \cdot \left( 1 + a_h \cdot \sin{\left( \varphi \cdot \omega_h \right)} \right) \\ - v_{UP} \cdot t \end{pmatrix} \end{aligned} \end{equation}
where \( a_h \) is an amplitude factor which scales the harmonic oscillation in relation to the radius and \( \omega_h \) is the harmonic oscillation frequency in units [cycles/revolution]. Note that only unsigned integer values are allowed for \( \omega_h \).
The local position can then be added to the start position after rotating it into the Earth frame
\begin{equation} \renewcommand*{\arraystretch}{2.0} \mathbf{x}^e = \mathbf{x}_M^e + \mathbf{C}_n^e(\mathbf{x}_M) \cdot \mathbf{x}^n \end{equation}
Velocity
The velocity is the first derivative of the position spline.
Acceleration
The acceleration is the second derivative of the position spline.
When not flying on constant height, which is the case for our circular trajectory, there is a certain roll angle. The following image should explain this.
\begin{tikzpicture} \def\ellipseB{10.0}; \def\ellipseA{14.0}; \coordinate (origin) at (0.0, 0.0); \draw[thick, black] (0,0) [partial ellipse=45:120:\ellipseA cm and \ellipseB cm]; \node[left] at (-5.0, \ellipseB-0.5) {Earth surface}; \draw[black,thick] (8.0,\ellipseB) -- (5.0,\ellipseB); \draw[black,thick] (-5.0,\ellipseB) -- (-8.0,\ellipseB); \draw[blue,ultra thick] (5.0,\ellipseB) -- (-5.0,\ellipseB) node[left,yshift=10pt] {Flight plane}; \coordinate (xE) at (5.0,\ellipseB); \coordinate (xM) at (0,\ellipseB); \def\tangentLength{100}; \path let \p1=(xE) in coordinate (t1) at (\x1 + \tangentLength pt, \y1 - \tangentLength * \ellipseB/\ellipseA * \ellipseB/\ellipseA * \x1/\y1); \path let \p1=(xE) in coordinate (t2) at (\x1 - \tangentLength pt, \y1 + \tangentLength * \ellipseB/\ellipseA * \ellipseB/\ellipseA * \x1/\y1); \draw[brown,thick] (t1) -- (t2); \def\tangentLength{10}; \path let \p1=(xE) in coordinate (nE) at (\x1 + \tangentLength pt, \y1 + \tangentLength * \ellipseA/\ellipseB * \ellipseA/\ellipseB * \y1/\x1); \draw[->,brown,thick] (xE) -- (nE) node[above] {$\vec{\mathbf{n}}^e$}; \pgfmathanglebetweenpoints{\pgfpointanchor{xE}{center}}{\pgfpointanchor{t1}{center}} \edef\angleRoll{\pgfmathresult} \def\angleRadius{2.5}; \centerarc[black,thick,->](xE)(-(360-\angleRoll):0:\angleRadius); \node[left,xshift=-5pt,yshift=-7pt] at ($(xE)+(\angleRadius,0)$) {$R$}; \def\rightAngleRadius{0.5}; \centerarc[black,thick](xE)(90-(360-\angleRoll):180-(360-\angleRoll):\rightAngleRadius); \node[left,xshift=1pt,yshift=7pt] at (xE) {$\boldsymbol{\cdot}$}; \coordinate (nM) at ($(xM)+(0,1.5)$); \draw[->,brown,thick] (xM) -- (nM) node[left] {$\vec{\mathbf{n}}^e_M$}; \centerarc[black,thick](xM)(90:180:\rightAngleRadius); \node[left,xshift=-1pt,yshift=7pt] at (xM) {$\boldsymbol{\cdot}$}; \node[draw,cross=5pt,red,thick] (xMC) at (xM) {}; \node[draw,cross=5pt,red,thick] (xEC) at (xE) {}; \path [draw,->,thick] ($($(origin)!.8!(xEC)$)$) -- (xEC) node [pos=0.38,left] {$\vec{\mathbf{x}}^e$}; \path [draw, thick] ($($(origin)!.8!(xEC)$)$) --pic[rotate=-45]{ext} ($($(origin)!.8!(xEC)$)$); \path [draw,->,thick] ($($(origin)!.8!(xMC)$)$) -- (xMC) node [pos=0.5,right] {$\vec{\mathbf{x}}_M^e$}; \path [draw, thick] ($($(origin)!.8!(xMC)$)$) --pic{ext} ($($(origin)!.8!(xMC)$)$); \end{tikzpicture}
The simulated position is at the position \(\vec{x}^e\) and its velocity vector is facing out of the plane. When facing into the plane, the roll angle runs into the other direction resulting in a minus in the formula.
The roll angle is the angle between the tangential planes to the Earth surface in the current position and the center of the flight path.
\begin{equation} R = \text{dir} \cdot \arccos{\left(\frac{\vec{\mathbf{n}}_M^e \cdot \vec{\mathbf{n}}^e}{\left|\vec{\mathbf{n}}_M^e\right| \cdot \left|\vec{\mathbf{n}}^e\right|}\right)} \end{equation}
where \( \text{dir} \) is a sign (+1 for CW and -1 for CCW) determining the direction of the circle.
The normal of the tangential planes is however not the position vectors \(\vec{\mathbf{x}}^e\) and \(\vec{\mathbf{x}}^e_M\), because Earth is an ellipsoid, but can be calculated with the geodetic latitude and longitude as (see Wikipedia n-vector )
\begin{equation} \vec{\mathbf{n}}^e = \begin{pmatrix} \cos{\phi} \cdot \cos{\lambda} \\ \cos{\phi} \cdot \sin{\lambda} \\ \sin{\phi} \end{pmatrix} \end{equation}
Especially it can be seen, that this vector has length one
\begin{equation} \begin{aligned} \left|\vec{\mathbf{n}}^e\right| &= \sqrt{ \cos^2{\phi} \cdot \cos^2{\lambda} + \cos^2{\phi} \cdot \sin^2{\lambda} + \sin^2{\phi} } \\ &= \sqrt{ \cos^2{\phi} \cdot \underbrace{( \cos^2{\lambda} + \sin^2{\lambda})}_{=1} + \sin^2{\phi} } \\ &= \sqrt{ \cos^2{\phi} + \sin^2{\phi} } \\ &= 1 \end{aligned} \end{equation}
The roll angle therefore becomes
\begin{equation} R = \text{dir} \cdot \arccos{\left( \vec{\mathbf{n}}_M^e \cdot \vec{\mathbf{n}}^e \right)} \end{equation}
The pitch angle is calculated with the local navigation frame velocity after formula eq-INS-Mechanization-Pitch
\begin{equation} P = \tan^{-1}\left(\frac{-v_D}{\sqrt{v_N^2 + v_E^2}}\right) \end{equation}
The yaw angle is calculated with the local navigation frame velocity after formula eq-INS-Mechanization-Yaw
\begin{equation} Y = \tan^{-1}\left(\frac{v_E}{v_N}\right) \end{equation}
1.10.0