Aviumtechnologies Blog
  • Introduction
  • Topics
    • Initial sizing
    • Definition of axes and angles
    • Stability
      • Obtaining stability derivatives from forced oscillations
    • Equations of motion
    • PX4 Autopilot
      • Building and running PX4 Autopilot on BeagleBone® Blue
      • PX4 BeagleBone® Blue-based quadcopter
Powered by GitBook
On this page
  • Derivation
  • Non-linear state-space form

Was this helpful?

  1. Topics

Equations of motion

The aim of this topic is to explain how the six-degrees of freedom equations of motion with respect to rotating Earth are derived

PreviousObtaining stability derivatives from forced oscillationsNextPX4 Autopilot

Last updated 3 years ago

Was this helpful?

The flat-Earth aircraft equations of motion are used for short-distance flight only and for observation of the aircraft responses to control inputs. Here the aircraft equations of motion for spherical, rotating Earth are derived. These equations are widely used for aircraft simulation.

Derivation

Nomenclature

then the direction cosine matrix from the ECEF to the body-fixed coordinate system is:

Simplifying by gathering terms gives:

Some of the derivatives can be rewritten as:

If the above definitions are substituted in the acceleration equation the following equation is obtained:

Assuming the Earth's rotational velocity is constant:

This equation gives the derivative of the velocity vector in the body-fixed coordinate system with respect to the EFEC coordinate system. However, since the body-fixed coordinate system rotates with respect to the EFEC coordinate system the following equation is used:

Substitution of the above equation in the previous gives:

The above equation gives the derivative of the velocity vector in the body-fixed coordinate system with respect to the body-fixed coordinate system. The flat-Earth aircraft equations of motion can be identified. However, there are additional terms for the Coriolis and Centripetal acceleration due to the Earth's rotational velocity. Since angular velocities are additive, the angular velocity of the body-fixed coordinate system relative to the ECI the coordinate system in body coordinates is given by:

Further simplifying the above equation gives:

The above are the translational aircraft equations of motion. The rotational aircraft equations of motion are given by:

The angular velocity of the NED coordinate system with respect to the ECEF coordinate system is given by:

where

Then the angular velocity of the body-fixed relative to the NED coordinate system is given by:

Non-linear state-space form

[C]b/a[\mathbf{C}]_{b/a}[C]b/a​- Direction cosine matrix transformation from coordinate system aaa to bbb ()a()^{a}()a- Vector in coordinate system aaa b()˙a^b\dot{()}^{a}b()˙​a- Derivative of vector in coordinate system aaawith respect to coordinate system bbb ()b/a()_{b/a}()b/a​- bbb with respect to aaa μ\muμ- latitude lll- longitude hhh- altitude ReReRe- Earth radius

In order to derive the aircraft equations for spherical, rotating Earth four coordinate systems are defined. The Earth-centred inertial (ECI) coordinate system has its origin at the centre of the Earth with its xbx_{b}xb​-axis pointing towards the vernal equinox and its zbz_{b}zb​-axis pointing towards the Earth's north pole. The Earth-fixed centred coordinate system (EFEC) has its origin at the centre of the Earth with its xbx_{b}xb​-axis pointing towards the prime meridian and zbz_{b}zb​-axis towards the Earth's north pole. The north-east-down (NED) coordinate system has its origin at a particular longitude, latitude and altitude with respect to the EFEC coordinate system, its xbx_{b}xb​-axis is pointing north and its zbz_{b}zb​-axis is pointing toward the centre of the Earth. The last coordinate system is the body-fixed coordinate system. Its origin coincides with the NED coordinate system origin.

The direction cosine matrices between the EFEC, NED and body-fixed coordinate systems must be defined. The direction cosine matrix from the EFEC to NED coordinate system in terms of longitude (μ\muμ) and latitude (lll) is:

[C]f/e=[−ClSμ−SlSμCμ−SlCl0−ClCμ−CμSl−Sμ],[\mathbf{C}]_{f/e}= \begin{bmatrix} -C_{l}S_{\mu} &-S_{l}S_{\mu} &C_{\mu}\\ -S_{l} &C_{l} &0\\ -C_{l}C_{\mu} &-C_{\mu}S_{l} &-S_{\mu}\\ \end{bmatrix}\text{,}[C]f/e​=​−Cl​Sμ​−Sl​−Cl​Cμ​​−Sl​Sμ​Cl​−Cμ​Sl​​Cμ​0−Sμ​​​,

and the direction cosine matrix from the NED to the body-fixed coordinate system in terms of Euler angles (ϕ\phiϕ,θ\thetaθ,ψ\psiψ) is:

[C]b/f=[CθCψSϕSθCψ−CϕSψCϕSθCψ+SϕSψCθSψSϕSθSψ+CϕCψCϕSθSψ−SϕCψ−SθSϕCθCϕCθ]T,[\mathbf{C}]_{b/f}= \begin{bmatrix} C_{\theta}C{\psi} &S_{\phi}S_{\theta}C_{\psi}-C_{\phi}S_{\psi} &C_{\phi}S_{\theta}C_{\psi}+S_{\phi}S_{\psi}\\ C_{\theta}S{\psi} &S_{\phi}S_{\theta}S_{\psi}+C_{\phi}C_{\psi} &C_{\phi}S_{\theta}S_{\psi}-S_{\phi}C_{\psi}\\ -S_{\theta} &S_{\phi}C_{\theta} &C_{\phi}C_{\theta}\end{bmatrix}^T\text{,}[C]b/f​=​Cθ​CψCθ​Sψ−Sθ​​Sϕ​Sθ​Cψ​−Cϕ​Sψ​Sϕ​Sθ​Sψ​+Cϕ​Cψ​Sϕ​Cθ​​Cϕ​Sθ​Cψ​+Sϕ​Sψ​Cϕ​Sθ​Sψ​−Sϕ​Cψ​Cϕ​Cθ​​​T,

[C]b/e=[C]b/f[C]f/e.[\mathbf{C}]_{b/e}=[\mathbf{C}]_{b/f}[\mathbf{C}]_{f/e}\text{.}[C]b/e​=[C]b/f​[C]f/e​.

A position vector r\mathbf{r}r in the ECEF coordinate system gives the position of the NED and the body-fixed coordinate systems (the NED and body-fixed coordinate systems coincide, as mentioned earlier). The time derivative of r\mathbf{r}rwith respect to the ECI coordinate system is given by:

ir˙e=er˙e+ωe/ie×re,^i\dot{\mathbf{r}}^{e}=^e\dot{\mathbf{r}}^{e}+\mathbf{\omega}_{e/i}^{e}\times\mathbf{r}^{e}\text{,}ir˙e=er˙e+ωe/ie​×re,

where ωe/i\mathbf{\omega}_{e/i}ωe/i​is the earth rotational velocity. Differentiating again with respect to time gives:

ir¨e=er˙e+eω˙e/ie×re+ωe/ie×er˙e+ωe/ie×er˙e+ωe/ie×ωe/ie×re.^i\ddot{\mathbf{r}}^{e}=^e\dot{\mathbf{r}}^{e}+^e\dot{\mathbf{\omega}}_{e/i}^{e}\times\mathbf{r}^{e}+\mathbf{\omega}_{e/i}^{e}\times ^e\dot{\mathbf{r}}^{e}+\mathbf{\omega}_{e/i}^{e}\times ^e\dot{\mathbf{r}}^{e}+\mathbf{\omega}_{e/i}^{e}\times\mathbf{\omega}_{e/i}^{e}\times\mathbf{r}^{e}\text{.}ir¨e=er˙e+eω˙e/ie​×re+ωe/ie​×er˙e+ωe/ie​×er˙e+ωe/ie​×ωe/ie​×re.

ir¨e=er¨e+eω˙e/ie×re+2ωe/ie×er˙e+ωe/ie×ωe/ie×re.^i\ddot{\mathbf{r}}^{e}=^e\ddot{\mathbf{r}}^{e}+^e\dot{\mathbf{\omega}}_{e/i}^{e}\times\mathbf{r}^{e}+2\mathbf{\omega}_{e/i}^{e}\times ^e\dot{\mathbf{r}}^{e}+\mathbf{\omega}_{e/i}^{e}\times\mathbf{\omega}_{e/i}^{e}\times\mathbf{r}^{e}\text{.}ir¨e=er¨e+eω˙e/ie​×re+2ωe/ie​×er˙e+ωe/ie​×ωe/ie​×re.

Using Newton's Second Law (often stated as F=maF=maF=ma) gives:

ir¨e=ae=Fem+Ge,^i\ddot{\mathbf{r}}^{e}=\mathbf{a}^{e}=\frac{\mathbf{F}^{e}}{m}+\mathbf{G}^{e}\text{,}ir¨e=ae=mFe​+Ge,

where Fe\mathbf{F}^{e}Feis the force vector and Ge\mathbf{G}^{e}Geis the gravitational vector in ECEF coordinates.

er¨e=eV˙e,er˙e=Ve.^e\ddot{\mathbf{r}}^{e}=^e\dot{\mathbf{V}}^{e}\text{,}\\ ^e\dot{\mathbf{r}}^{e}=\mathbf{V}^{e}\text{.} er¨e=eV˙e,er˙e=Ve.

Fem+Ge=eV˙e+eω˙e/ie×re+2ωe/ie×Ve+ωe/ie×ωe/ie×re.\frac{\mathbf{F}^{e}}{m}+\mathbf{G}^{e}=^e\dot{\mathbf{V}}^{e}+^e\dot{\mathbf{\omega}}_{e/i}^{e}\times\mathbf{r}^{e}+2\mathbf{\omega}_{e/i}^{e}\times \mathbf{V}^{e}+\mathbf{\omega}_{e/i}^{e}\times\mathbf{\omega}_{e/i}^{e}\times\mathbf{r}^{e}\text{.}mFe​+Ge=eV˙e+eω˙e/ie​×re+2ωe/ie​×Ve+ωe/ie​×ωe/ie​×re.

eω˙e/ie×re=0,^e\dot{\mathbf{\omega}}_{e/i}^{e}\times\mathbf{r}^{e}=0\text{,}eω˙e/ie​×re=0,

and multiplying by the direction cosine matrix [C]b/e[\mathbf{C}]_{b/e}[C]b/e​ gives the following equation:

Fbm+Gb=eV˙b+2[C]b/eωe/ie×Vb+[C]b/eωe/ie×ωe/ie×re.\frac{\mathbf{F}^{b}}{m}+\mathbf{G}^{b}=^e\dot{\mathbf{V}}^{b}+2[\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e}\times \mathbf{V}^{b}+[\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e}\times\mathbf{\omega}_{e/i}^{e}\times\mathbf{r}^{e}\text{.}mFb​+Gb=eV˙b+2[C]b/e​ωe/ie​×Vb+[C]b/e​ωe/ie​×ωe/ie​×re.

Rearranging for eV˙b^e\dot{\mathbf{V}}^{b}eV˙b gives:

eV˙b=Fbm−2[C]b/eωe/ie×Vb+Gb−[C]b/eωe/ie×ωe/ie×re,^e\dot{\mathbf{V}}^{b}=\frac{\mathbf{F}^{b}}{m}-2[\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e}\times \mathbf{V}^{b}+\mathbf{G}^{b}-[\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e}\times\mathbf{\omega}_{e/i}^{e}\times\mathbf{r}^{e}\text{,}eV˙b=mFb​−2[C]b/e​ωe/ie​×Vb+Gb−[C]b/e​ωe/ie​×ωe/ie​×re,

where re\mathbf{r}^{e}reis given by:

re={(Re+h)cos(μ)cos(l)(Re+h)cos(μ)sin(l)(Re+h)sin(μ)}.\mathbf{r}^{e}=\begin{Bmatrix}(Re+h)cos(\mu)cos(l)\\(Re+h)cos(\mu)sin(l)\\(Re+h)sin(\mu)\end{Bmatrix}\text{.}re=⎩⎨⎧​(Re+h)cos(μ)cos(l)(Re+h)cos(μ)sin(l)(Re+h)sin(μ)​⎭⎬⎫​.

eV˙b=bV˙b+ωb/eb×Vb.^e\dot{\mathbf{V}}^{b}=^b\dot{\mathbf{V}}^{b}+\mathbf{\omega}_{b/e}^{b}\times\mathbf{V}^{b}\text{.}eV˙b=bV˙b+ωb/eb​×Vb.

bV˙b=Fbm−ωb/eb×Vb−2[C]b/eωe/ie×Vb+Gb−[C]b/eωe/ie×ωe/ie×re.^b\dot{\mathbf{V}}^{b}=\frac{\mathbf{F}^{b}}{m}-\mathbf{\omega}_{b/e}^{b}\times\mathbf{V}^{b}-2[\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e}\times \mathbf{V}^{b}+\mathbf{G}^{b}-[\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e}\times\mathbf{\omega}_{e/i}^{e}\times\mathbf{r}^{e} \text{.}bV˙b=mFb​−ωb/eb​×Vb−2[C]b/e​ωe/ie​×Vb+Gb−[C]b/e​ωe/ie​×ωe/ie​×re.

ωb/ib=ωb/eb+[C]b/eωe/ie.\mathbf{\omega}_{b/i}^{b}= \mathbf{\omega}_{b/e}^{b}+ [\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e}\text{.}ωb/ib​=ωb/eb​+[C]b/e​ωe/ie​.

Rearranging the above equation for ωb/eb\mathbf{\omega}_{b/e}^{b}ωb/eb​and substituting in the previous equation gives:

bV˙b=Fbm−(ωb/ib−[C]b/eωe/ie)×Vb−2[C]b/eωe/ie×Vb+Gb−[C]b/eωe/ie×ωe/ie×re.^b\dot{\mathbf{V}}^{b}=\frac{\mathbf{F}^{b}}{m}-(\mathbf{\omega}_{b/i}^{b}-[\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e})\times\mathbf{V}^{b}-2[\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e}\times \mathbf{V}^{b}+\mathbf{G}^{b}-[\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e}\times\mathbf{\omega}_{e/i}^{e}\times\mathbf{r}^{e}\text{.}bV˙b=mFb​−(ωb/ib​−[C]b/e​ωe/ie​)×Vb−2[C]b/e​ωe/ie​×Vb+Gb−[C]b/e​ωe/ie​×ωe/ie​×re.

bV˙b=Fbm−ωb/ib×Vb−[C]b/eωe/ie×Vb+Gb−[C]b/eωe/ie×ωe/ie×re.^b\dot{\mathbf{V}}^{b}=\frac{\mathbf{F}^{b}}{m}-\mathbf{\omega}_{b/i}^{b}\times\mathbf{V}^{b}-[\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e}\times \mathbf{V}^{b}+\mathbf{G}^{b}-[\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e}\times\mathbf{\omega}_{e/i}^{e}\times\mathbf{r}^{e}\text{.}bV˙b=mFb​−ωb/ib​×Vb−[C]b/e​ωe/ie​×Vb+Gb−[C]b/e​ωe/ie​×ωe/ie​×re.

ω˙b/ib=[I]−1(M−ωb/ib×([I]ωb/ib)),\dot{\mathbf{\omega}}_{b/i}^{b}=[\mathbf{I}]^{-1}(\mathbf{M}-\mathbf{\omega}_{b/i}^{b}\times([\mathbf{I}]\mathbf{\omega}_{b/i}^{b}))\text{,}ω˙b/ib​=[I]−1(M−ωb/ib​×([I]ωb/ib​)),

and are similar to the flat-Earth rotational equations of motion with the difference that ωb/ib\mathbf{\omega}_{b/i}^{b}ωb/ib​ is the aircraft angular velocity vector with respect to the ECI coordinate system. The components of this angular velocity vector are not directly equal to ppp, qqq, and rrr which is the case for the flat-Earth equations of motion. Here they are given by ωb/fb\mathbf{\omega}_{b/f}^{b}ωb/fb​. In order to determine ppp, qqq and rrr, the rotation of the NED with respect to the EFEC coordinate system must be taken into account. Why is rotation present between the NED and EFEC coordinate systems? The NED coordinate system is always tangent to the Earth. If the aircraft starts its journey at l=0l=0l=0 deg and ends its journey at l=90l=90l=90 deg, the NED coordinate system would have rotated in order to stay tangent to the Earth. So the angular velocity of the body-fixed to the ECEF coordinate system can be represented as:

ωb/eb=ωb/fb+[C]b/fωf/ef,\mathbf{\omega}_{b/e}^{b}=\mathbf{\omega}_{b/f}^{b}+[\mathbf{C}]_{b/f}\mathbf{\omega}_{f/e}^{f}\text{,}ωb/eb​=ωb/fb​+[C]b/f​ωf/ef​,

where ωb/fb\mathbf{\omega}_{b/f}^{b}ωb/fb​is the angular velocity of the body-fixed with respect to the NED coordinate system and ωf/ef\mathbf{\omega}_{f/e}^{f}ωf/ef​is the angular velocity of the NED with respect to the ECEF coordinate system.

ωf/ef={l˙cos(μ)−μ˙−l˙sin(μ)},\mathbf{\omega}_{f/e}^{f}=\begin{Bmatrix}\dot{l}cos(\mu)\\-\dot{\mu}\\-\dot{l}sin(\mu)\end{Bmatrix}\text{,}ωf/ef​=⎩⎨⎧​l˙cos(μ)−μ˙​−l˙sin(μ)​⎭⎬⎫​,

μ˙=1Re+hVfx,l˙=1Re+hcos(μ)Vfy,h˙=−Vfz,Vf=[C]b/fTVb.\dot{\mu}=\frac{1}{Re+h}V_{f_{x}}\text{,}\\ \dot{l}=\frac{1}{Re+h}cos(\mu)V_{f_{y}}\text{,}\\ \dot{h}=-V_{f_{z}}\text{,}\\ \mathbf{V_{f}}=[\mathbf{C}]_{b/f}^T\mathbf{V_{b}}\text{.}μ˙​=Re+h1​Vfx​​,l˙=Re+h1​cos(μ)Vfy​​,h˙=−Vfz​​,Vf​=[C]b/fT​Vb​.

ωb/fb=ωb/eb−[C]b/fωf/ef,\mathbf{\omega}_{b/f}^{b}=\mathbf{\omega}_{b/e}^{b}-[\mathbf{C}]_{b/f}\mathbf{\omega}_{f/e}^{f}\text{,}ωb/fb​=ωb/eb​−[C]b/f​ωf/ef​,

which can be further simplified if using the previously derived definition for ωb/eb\mathbf{\omega}_{b/e}^{b}ωb/eb​:

ωb/fb=ωb/ib−[C]b/fωf/ef−[C]b/eωe/ie.\mathbf{\omega}_{b/f}^{b}=\mathbf{\omega}_{b/i}^{b}-[\mathbf{C}]_{b/f}\mathbf{\omega}_{f/e}^{f}-[\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e}\text{.}ωb/fb​=ωb/ib​−[C]b/f​ωf/ef​−[C]b/e​ωe/ie​.

Vb={uvw},ωb/f={pqr}bV˙b={u˙xv˙yw˙z}=Fbm−ωb/ib×Vb−[C]b/eωe/ie×Vb+Gb−[C]b/eωe/ie×ωe/ie×reω˙b/ib={ω˙xω˙yω˙z}=[I]−1(M−ωb/ib×([I]ωb/ib))ωb/fb=ωb/ib−[C]b/fωf/ef−[C]b/eωe/ie\mathbf{V^{b}}= \begin{Bmatrix} u\\ v\\ w \end{Bmatrix}, \mathbf{\omega}_{b/f}= \begin{Bmatrix} p\\ q\\ r \end{Bmatrix}\\\\ ^b\dot{\mathbf{V}}^{b}=\begin{Bmatrix} \dot{u}_{x}\\ \dot{v}_{y}\\ \dot{w}_{z}\\ \end{Bmatrix}=\frac{\mathbf{F}^{b}}{m}-\mathbf{\omega}_{b/i}^{b}\times\mathbf{V}^{b}-[\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e}\times \mathbf{V}^{b}+\mathbf{G}^{b}-[\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e}\times\mathbf{\omega}_{e/i}^{e}\times\mathbf{r}^{e}\\\dot{\mathbf{\omega}}_{b/i}^{b}=\begin{Bmatrix} \dot{\omega}_{x}\\ \dot{\omega}_{y}\\ \dot{\omega}_{z}\\ \end{Bmatrix}=[\mathbf{I}]^{-1}(\mathbf{M}-\mathbf{\omega}_{b/i}^{b}\times([\mathbf{I}]\mathbf{\omega}_{b/i}^{b}))\\ \mathbf{\omega}_{b/f}^{b}=\mathbf{\omega}_{b/i}^{b}-[\mathbf{C}]_{b/f}\mathbf{\omega}_{f/e}^{f}-[\mathbf{C}]_{b/e}\mathbf{\omega}_{e/i}^{e}Vb=⎩⎨⎧​uvw​⎭⎬⎫​,ωb/f​=⎩⎨⎧​pqr​⎭⎬⎫​bV˙b=⎩⎨⎧​u˙x​v˙y​w˙z​​⎭⎬⎫​=mFb​−ωb/ib​×Vb−[C]b/e​ωe/ie​×Vb+Gb−[C]b/e​ωe/ie​×ωe/ie​×reω˙b/ib​=⎩⎨⎧​ω˙x​ω˙y​ω˙z​​⎭⎬⎫​=[I]−1(M−ωb/ib​×([I]ωb/ib​))ωb/fb​=ωb/ib​−[C]b/f​ωf/ef​−[C]b/e​ωe/ie​

x˙={x˙1x˙2x˙3x˙4x˙5x˙6x˙7x˙8x˙9x˙10x˙11⋮x˙18x˙19x˙20⋮x˙27}={u˙xv˙yw˙zω˙xω˙yω˙zμ˙l˙h˙C11˙f/eC12˙f/e⋮C33˙f/eC11˙b/fC12˙b/f⋮C33˙b/f}\dot{\mathbf{x}}= \begin{Bmatrix} \dot{x}_{1} \\ \dot{x}_{2} \\ \dot{x}_{3} \\ \dot{x}_{4} \\ \dot{x}_{5} \\ \dot{x}_{6} \\ \dot{x}_{7} \\ \dot{x}_{8} \\ \dot{x}_{9} \\ \dot{x}_{10} \\ \dot{x}_{11} \\ \vdots \\ \dot{x}_{18} \\ \dot{x}_{19} \\ \dot{x}_{20} \\ \vdots \\ \dot{x}_{27} \\ \end{Bmatrix} =\begin{Bmatrix} \dot{u}_{x}\\ \dot{v}_{y}\\ \dot{w}_{z}\\ \dot{\omega}_{x}\\ \dot{\omega}_{y}\\ \dot{\omega}_{z}\\ \dot{\mu}\\ \dot{l}\\ \dot{h}\\ \dot{C11}_{f/e}\\ \dot{C12}_{f/e}\\ \vdots \\ \dot{C33}_{f/e}\\ \dot{C11}_{b/f}\\ \dot{C12}_{b/f}\\ \vdots \\ \dot{C33}_{b/f}\\ \end{Bmatrix}x˙=⎩⎨⎧​x˙1​x˙2​x˙3​x˙4​x˙5​x˙6​x˙7​x˙8​x˙9​x˙10​x˙11​⋮x˙18​x˙19​x˙20​⋮x˙27​​⎭⎬⎫​=⎩⎨⎧​u˙x​v˙y​w˙z​ω˙x​ω˙y​ω˙z​μ˙​l˙h˙C11˙f/e​C12˙f/e​⋮C33˙f/e​C11˙b/f​C12˙b/f​⋮C33˙b/f​​⎭⎬⎫​

Coordinate systems definitions