# Modeling Vehicle Dynamics – Quadcopter Equations of Motion

In this post we will see how we can describe motion of the quadcopter – or any vehicle – as a set of differential equations.

This post is the 2nd in a series on modeling and simulation of a quadcopter’s vehicle dynamics. The full series will include all of the following posts:

1. Modeling Vehicle Dynamics – Euler Angles
2. Modeling Vehicle Dynamics – Quadcopter Equations of Motion
3. Modeling Vehicle Dynamics – 6DOF Nonlinear Simulation

The contents of this post will build on the concepts of multiple reference frames and Euler angles. If you aren’t familiar with those, you may want to read the first post in this series.

# Describing Motion

Before going into any details about equations of motion, we should identify what we are trying to describe. There are two questions we should answer:

What variables are necessary to describe motion?

In the first post we already discussed the concept of 6 Degrees of Freedom (6DOF), where we want to know 3-D position in addition to 3-D orientation of the vehicle. But to describe the motion of a body what we really want to know is how these change over time.

Therefore we will need variables which represent both the linear and angular velocity of the vehicle. The linear velocity in the body-frame of the vehicle is defined as:

$\textbf{v}^{b} = \begin{bmatrix} u\\v\\w \end{bmatrix} = \begin{bmatrix} \text{longitudinal velocity} \\ \text{lateral velocity} \\ \text{normal velocity} \end{bmatrix}$

The angular velocity of the rotating body-fixed reference frame is defined as:

$\boldsymbol{\omega} = \begin{bmatrix} p\\q\\r \end{bmatrix} = \begin{bmatrix} \text{roll rate} \\ \text{pitch rate} \\ \text{yaw rate} \end{bmatrix}$

We also need to know the forces which influence motion of the vehicle. Again we define this for both linear translation force and angular moment:

$\text{Force} = \begin{bmatrix} F_{x}\\F_{y}\\F_{z} \end{bmatrix}$

$\text{Moment} = \begin{bmatrix} L\\M\\N \end{bmatrix}$

Finally, some influences may be in the inertial frame (such as gravity) while some may be attached to the body (such as the vehicle’s propulsion). Therefore, the Euler angles must also be tracked in order to convert between the inertial and body frames.

$\boldsymbol{\Phi} = \begin{bmatrix} \phi \\ \theta \\ \psi \end{bmatrix} = \begin{bmatrix} \text{bank angle} \\ \text{pitch attitude} \\ \text{heading} \end{bmatrix}$

The above variables are sufficient to describe all of the dynamics of the quadcopter vehicle, however they do not specify the actual position in space of the vehicle. This is because the dynamics in one location should be the same as any other location (things that change with location such as aerodynamics and gravity we will consider constant, and only enter the equations through the force and moment variables).

This is desirable since we expect the quadcopter to behave the same no matter where it starts from. But given a starting point, the equations of motion will allow us to propagate that starting location forward in time to a new position if we know all of the “state” variables defined above.

With that being said, sometimes it is useful to include additional state variables to observe. For navigation or motion planning, it is generally necessary to track the inertial position as well as it changes over time. Therefore we will define three more variables for position in the inertial reference frame:

$\text{Navigation Coordinates} = \begin{bmatrix} x\\y\\h \end{bmatrix} = \begin{bmatrix} \text{longitudinal position} \\ \text{lateral position} \\ \text{height} \end{bmatrix}$

I will treat these navigation coordinates as in a Flat-Earth model, where the coordinates are assumed to represent a rectilinear grid. However, one could extend these equations to a spherical coordinate frame in order to express position in latitude and longitude, and even account for rotation of the Earth.

## Sign Conventions

All dynamics equations will use right-handed coordinate frames. In flight controls it is standard for the X component to be aligned with the forward direction of the vehicle. And the forward direction is assumed to be at a heading of 0 degrees when it is aligned with North in the inertial frame. This results in the somewhat odd convention of [X, Y, Z] being a [North, East, Down] reference frame, in order to maintain right-handedness.

However, for navigation it makes sense to define height as positive in the up direction. Therefore, our navigation frame, which we will refer to with (E) for Earth, in relation to the inertial (n) frame will have the convention:

$\begin{bmatrix} x^{E}\\y^{E}\\h^{E} \end{bmatrix}_{\text{(Nav)}} = \begin{bmatrix} x^{n}\\y^{n}\\-z^{n} \end{bmatrix}_{\text{(Inertial)}}$

# Reference Frames and Rotation

One thing that must be kept in mind while developing these expressions is what reference frame are the variables expressed in, and with respect to what reference frame are they changing? See Reference [2] for a more thorough explanation if desired.

## Inertial motion expressed in body frame components

The motion we want to classify for translation as well as rotation is all with respect to the inertial frame (e.g. the ground). In fact, we assume a rigid body which means there can be no motion with respect to the body frame. The body frame is fixed to the vehicle, and any motion of the vehicle with respect to that origin would imply flexible bending or movement of the vehicle components.

The body frame is useful though, because our vehicle sensors are attached to it, as well as our propulsion (the propellers). Therefore, the equations of motion we derive will be inertial motion (change in position and orientation with respect to the ground), but expressed within the coordinate system of our body axes $\langle\hat{b}_{1}, \hat{b}_{2}, \hat{b}_{3}\rangle$.

You should notice once we derive the final set of equations however, that inertial position is not necessary for these equations of dynamics. The equations of motion only represent the change in these values from the vehicle’s point of view. In that sense, they are very useful as they may be applied to solve for the future motion of a vehicle from any inertial starting point.

## Simultaneous Translation and Rotation – The Chain Rule

Now that we have established we want to represent inertial motion in the vehicle coordinate frame, we must understand what that means mathematically. In our derivation of the equations of motion we will want to take the derivative of vectors which are expressed in the body frame. Using the rules of calculus, we must use the chain rule of derivation to account for both the change due to the time derivative of the vector within the coordinate frame, as well as the time derivative of the coordinate frame’s rotation. Let me show this with a scalar expression of the time differentiation of the body-fixed velocity vector:

$\textbf{v}^{b} = \begin{bmatrix} u\\v\\w \end{bmatrix}^{b} = u\hat{b}_1 + v\hat{b}_2 + w\hat{b}_3$

$(\dfrac{d\textbf{v}}{dt})_{\text{inertial}} = (\dfrac{du}{dt}\hat{b}_1 + \dfrac{dv}{dt}\hat{b}_2 + \dfrac{dw}{dt}\hat{b}_3) + (u\dfrac{d\hat{b}_1}{dt} + v\dfrac{d\hat{b}_2}{dt} + w\dfrac{d\hat{b}_3}{dt})$

The components in the first set of parentheses represent the change in inertial velocity within the body frame coordinate system. The components in the second set of parentheses represent perceived velocity change due to the rotation of the coordinate frame. We can rewrite this equation as Coriolis’ Theorem, where the frame rotation is represented by the angular velocity crossed with the velocity vector:

$\dot{\textbf{v}}_{\text{inertial}} = \dot{\textbf{v}}^{b} + \boldsymbol{\omega}_{n}^{b} \times \textbf{v}^{b}$

The cross-product $\boldsymbol{\omega} \times \textbf{v}$ can be rewritten as the skew-symmetric cross-product matrix, $[\boldsymbol{\omega} \times]$, premultiplying the velocity vector.

$\boldsymbol{\omega} \times \textbf{v} = [\boldsymbol{\omega} \times] \textbf{v} = \begin{bmatrix} 0 & -\omega_{z} & \omega_{y} \\ \omega_{z} & 0 & -\omega_{x} \\ -\omega_{y} & \omega_{x} & 0 \end{bmatrix} \textbf{v}$

# Newton’s Second Law of Motion

We will begin with Newton’s 2nd Law which is commonly summarized as force $(F)$ equals change in momentum $(p)$, or for systems with constant mass, force equals mass $(m)$ times acceleration $(a)$ — the derivative of velocity $(v)$:

$\textbf{F} = \frac{d\textbf{p}}{dt} = m\frac{d\textbf{v}}{dt} = m\textbf{a}$

This equation defines the change in linear momentum, however we can also express the rotational analogue. Crossing both sides of the equation with a position vector $(r)$ from some origin $O$ we get the rotational law of motion about the origin $O$. We can simplify the equation to moment $(M)$ equals the change in angular momentum $(H)$, or for systems with constant mass distribution, moment equals moment of inertia $(I)$ times angular acceleration $(\Omega)$ — the derivative of angular velocity $(\omega)$.

$\vec{\textbf{r}} \times \textbf{F} = \vec{\textbf{r}} \times m\frac{d\textbf{v}}{dt}$

$\textbf{M} = \frac{d\textbf{H}}{dt} = \textbf{I}\frac{d\boldsymbol{\omega}}{dt} = \textbf{I}\boldsymbol{\Omega}$

We will use both the linear and rotational forms of this law to derive the total vehicle equations of motion. But first, a note on what reference frame we will be working in.

# Force, Mass, and Acceleration

We will be deriving the equations for translation and rotation separately, but viewing the equations side by side can help illustrate the similarities between the two.

### Translation

$\textbf{F} = m\frac{d\textbf{v}}{dt}$

Inertial velocity in the body-fixed coordinate system will be expressed as:
$\textbf{v}^{b} = [u, v, w]^{T}$

Using the chain rule for a rotating body frame, the law of motion becomes:

$\textbf{F} = m ( \dot{\textbf{v}}^{b} + \boldsymbol{\omega}_{n}^{b} \times \textbf{v}^{b})$

### Rotation

$M = \frac{d\textbf{H}}{dt}$

Angular velocity from inertial ($n$) to body-fixed ($b$) frame will be expressed as:
$\boldsymbol{\omega}_{n}^{b} = [p, q, r]^{T}$

Using the chain rule for a rotating body frame, the rotational law of motion becomes:

$\textbf{M} = \textbf{I}^{b}\dot{\boldsymbol{\omega}}_{n}^{b} + \boldsymbol{\omega}_{n}^{b} \times \textbf{I}^{b}\boldsymbol{\omega}_{n}^{b}$

## External Forces and Moments

To start formulating the equations of motion, the left hand side of Newton’s 2nd Law requires all forces which act on the body. For the quadcopter this comes down to our propulsion (thrust created by the propellers) and gravity.

### Propeller Thrust

For this post we will not go into propeller dynamics, but simply assume each of the four quadcopter propellers provides a thrust force perpendicular to the propeller and a torque about the center of gravity of the vehicle.

The thrust force is represented for each of the 4 propellers as $F_1, F_2, F_3, \text{and} F_4$. And all are aligned parallel with the vertical axis of the quadcopter, $\hat{b}_{3}$.

The force vector within the body frame coordinate system can then be represented by:

$\begin{bmatrix} F_{x}\\F_{y}\\F_{z} \end{bmatrix} = \begin{bmatrix} 0\\ 0\\ -F_1 - F_2 - F_3 - F_4 \end{bmatrix}$

Note that since our body frame convention has positive $\hat{b}_{3}$ in the down direction, thrust from the propellers is negative.

The moments are represented with components L, M, N for rotations about the $\hat{b}_{1}, \hat{b}_{2}, \hat{b}_{3}$ axes, respectively. As a moment is measured by force times distance, we’ll need the motor distances from the center of gravity, as shown in the image above. For pitch and roll, half the motors provide positive moment, and the other half provide a negative moment, depending on which side of the axis they are located.

$L = F_1 d_{1y} - F_2 d_{2y} - F_3 d_{3y} + F_4 d_{4y}$

$M = -F_1 d_{1x} + F_2 d_{2x} - F_3 d_{3x} + F_4 d_{4x}$

For yaw, the quadcopter takes advantage of its four rotating propellers. Half of the propellers are installed to rotate clockwise (#3 and #4) and the other half counter-clockwise (#1 and #2). Each propeller as it pushes against the air will also apply a torque about the cg of the quadcopter in the direction of its rotation as a function of its thrust, propeller radius, and distance from the cg. We will define this torque as a function $T(F,d_{x},d_{y})$, and can express the yaw moment of the vehicle as:

$N = -T(F_1,d_{1x},d_{1y}) - T(F_2,d_{2x},d_{2y}) + T(F_3,d_{3x},d_{3y}) + T(F_4,d_{4x},d_{4y})$

### Gravity Force in Body Coordinates

Gravity always acts towards the center of the Earth, and is expressed in the inertial frame as:

$\textbf{F}_{grav}^{n} =\begin{bmatrix} 0\\0\\mg \end{bmatrix}$

Where g = 9.81 m/s2, the gravitational constant. Remember that our z-inertial coordinate is in the Down direction, so gravity is a positive acceleration.

But all of our other forces are defined within the body frame of the vehicle. To include gravity in our equations of motion we must convert it into body frame components as well. Using our Euler angle transformation matrix from inertial to body frame, $\textbf{C}_{n}^{b}$, this is easy:

$\textbf{F}_{grav}^{b} = \textbf{C}_{n}^{b}\textbf{F}_{grav}^{n} = \begin{bmatrix} c_{\theta}c_{\psi} & c_{\theta}s_{\psi} & -s_{\theta} \\ -c_{\theta}s_{\psi}+s_{\phi}s_{\theta}c_{\psi} & c_{\phi}c_{\psi}+s_{\phi}s_{\theta}s_{\psi} & s_{\phi}c_{\theta} \\ s_{\phi}s_{\psi}+c_{\phi}s_{\theta}c_{\psi} & -s_{\phi}c_{\psi}+c_{\phi}s_{\theta}s_{\psi} & c_{\phi}c_{\theta} \end{bmatrix} \begin{bmatrix} 0\\0\\mg \end{bmatrix}$

$\textbf{F}_{grav}^{b} = \begin{bmatrix} -mg\sin(\theta)\\mg\sin(\phi)\cos(\theta)\\mg\cos(\phi)\cos(\theta) \end{bmatrix}$

## Mass and Moment of Inertia

Moment of inertia is the rotational analogue of mass, or “angular mass.” An object’s mass will inform you about how much force it will take to accelerate a still object, or stop a moving one (the property of inertia). The moment of inertia will similarly describe how large of a moment is required to turn a still object, or stop rotation of one about an axis. Because the rotational inertia depends on the axis it is rotating about, this property is represented with a matrix for our 3-dimensional world.

The moment of inertia is defined in physics for scalars as:

$I=mr^2$

### The Moment of Inertia Matrix (I)

A more general representation where the mass is integrated over the entire body of the object is:

$\textbf{I} = \int_{body} \textbf{r}^2 dm$

To represent this in matrix form, we will assume $\textbf{r}$ is some coordinate with components $[x, y, z]$ in relation to the reference frame origin. Then using the cross-product matrix form of $[\textbf{r} \times]$ we can evaluate:

$\textbf{I} = \int_{body} \textbf{r}^2 dm = \int_{body} [\textbf{r} \times \textbf{r} \times dm]$

$\textbf{I} = \int_{body} \begin{bmatrix} 0 & -z & y \\ z & 0 & -x \\ -y & x & 0 \end{bmatrix} \begin{bmatrix} 0 & -z & y \\ z & 0 & -x \\ -y & x & 0 \end{bmatrix} dm$

$\textbf{I} = \begin{bmatrix} \int(y^2+z^2)dm & -\int(xy)dm & -\int(xz)dm \\ -\int(xy)dm & \int(x^2+z^2)dm & -\int(yz)dm \\ -\int(xz)dm & -\int(yz)dm & \int(x^2 + y^2)dm \end{bmatrix}$

$\textbf{I} = \begin{bmatrix} I_{xx} & -I_{xy} & -I_{xz} \\ -I_{xy} & I_{yy} & -I_{yz} \\ -I_{xz} & -I_{yz} & I_{zz} \end{bmatrix}$

Let’s try to understand the physical meaning of these mathematical expressions. The diagonal terms of this matrix $I_{xx}, I_{yy}, \text{and} I_{zz}$ are referred to as the moments of inertia. The off-diagonal terms $I_{xy}, I_{xz}, \text{and} I_{yz}$ are referred to as the products of inertia.

If instead of a continuous body of mass, we represent the body as a collection of point-masses, then we can rewrite $I_{xx}$ as a summation of all mass points $i$:

$I_{xx} = \sum\limits_{i} (y_{i}^2 + z_{i}^2)m_{i}$

This means that every point of mass in the body is multiplied by the square of its distance from the x-axis — just as in the scalar form I = mr2. So the further the mass is away from the axis, the greater the angular inertia is about that axis.

### Symmetry

The products of inertia can be thought of as the cross-coupling of different axes’ inertias on each other. But, if the vehicle is symmetric about an axis, then we will see that some of the products of inertia will equal zero, and make our equations easier!

If we again think of them in the discrete form, the $I_{xy}$ product of inertia is the sum of all mass points $i$:

$I_{xy} = \sum\limits_{i} x_{i}y_{i}m_{i}$

With the moments of inertia the distance from the axis is squared, and therefore always results in a positive value. However here we can have both positive and negative values, since the distances $x_{i}$ and $y_{i}$ are from the center of the vehicle.

If the vehicle is symmetric across an axis, that would mean all the mass one side is equal to the other side and all at the same distance. For example, if the vehicle’s mass distribution is symmetric about the x-axis, then the sum of mass points in the negative direction equals the sum of mass points in the positive direction, and the positive and negative x distances all cancel out.

$I_{xy} = \sum\limits_{i} x_{i}y_{i}m_{i} = \sum\limits_{i}^{\{x_{i}<0\}} x_{i}y_{i}m_{i} + \sum\limits_{i}^{\{x_{i}>0\}} x_{i}y_{i}m_{i}$

$= -(\sum\limits_{i}^{\{x_{i}>0\}} x_{i}y_{i}m_{i}) + \sum\limits_{i}^{\{x_{i}>0\}} x_{i}y_{i}m_{i} = 0$

If the quadcopter is built so it is symmetric about both the x and y axes, then the inertia matrix would become:

$\textbf{I} = \begin{bmatrix} I_{xx} &0 & 0 \\ 0 & I_{yy} & 0 \\0 & 0 & I_{zz} \end{bmatrix}$

## Acceleration

The final component we must express to complete our equations of motion is the acceleration. As acceleration is the time derivative of velocity, we will represent acceleration as the derivative of our defined inertial velocity terms for linear and rotational motion.

### Linear Acceleration

We have already learned that we must use Coriolis’ Theorem to represent the inertial velocity derivative in the rotating body frame. And we came up with the formula:

$\dot{\textbf{v}}_{\text{inertial}} = \dot{\textbf{v}}^{b} + \boldsymbol{\omega}_{n}^{b} \times \textbf{v}^{b} = \dot{\textbf{v}}^{b} + \begin{bmatrix} 0 & -\omega_{z} & \omega_{y} \\ \omega_{z} & 0 & -\omega_{x} \\ -\omega_{y} & \omega_{x} & 0 \end{bmatrix} \textbf{v}$

If we plug in our nomenclature for $\textbf{v}^{b} = [u, v, w]^{T}$ and $\boldsymbol{\omega}^{b} = [p, q, r]^{T}$, we get:

$\dot{\textbf{v}}^{b} = \dot{\textbf{v}}^{b} + \boldsymbol{\omega}_{n}^{b} \times \textbf{v}^{b} = \begin{bmatrix} \dot{u}\\\dot{v}\\\dot{w} \end{bmatrix}^{b} + \begin{bmatrix} 0 & -r & q \\ r & 0 & -p \\ -q & p & 0 \end{bmatrix} \begin{bmatrix} u\\v\\w \end{bmatrix}^{b} = \begin{bmatrix} \dot{u} + qw - rv \\ \dot{v} + ru - pw \\ \dot{w} + pv - qu \end{bmatrix}$

### Translational Motion

We have now derived representations for all components of our system’s force, mass, and acceleration in translation. Plugging these into Newton’s formula provides us with our first set of equations of motion.

$\textbf{F} = m\textbf{a}$

$\textbf{F}_{prop} + \textbf{F}_{grav} = m\dot{\textbf{v}}_{inertial}^{b}$

$\begin{bmatrix} -mg\sin(\theta) \\ mg\sin(\phi)\cos(\theta) \\ -F_1 - F_2 - F_3 - F_4 + mg\cos(\phi)\cos(\theta) \end{bmatrix} = m \begin{bmatrix} \dot{u} + qw - rv \\ \dot{v} + ru - pw \\ \dot{w} + pv - qu \end{bmatrix}$

### Angular Acceleration and Rotational Motion

We follow a similar procedure to solve for the angular acceleration and rotational law of motion. Using Coriolis’ Theorem again for the time derivative of angular velocity, we can evaluate the rotational motion formula as follows:

$\textbf{M} = \textbf{I}^{b}\dot{\boldsymbol{\omega}}_{n}^{b} + \boldsymbol{\omega}_{n}^{b} \times \textbf{I}^{b}\boldsymbol{\omega}_{n}^{b}$

$\begin{bmatrix} L\\M\\N \end{bmatrix} = \begin{bmatrix} I_{xx} & -I_{xy} & -I_{xz} \\ -I_{xy} & I_{yy} & -I_{yz} \\ -I_{xz} & -I_{yz} & I_{zz} \end{bmatrix} \begin{bmatrix} \dot{p}\\ \dot{q}\\ \dot{r} \end{bmatrix} + \begin{bmatrix} 0 & -r & q \\ r & 0 & -p \\ -q & p & 0 \end{bmatrix} \begin{bmatrix} I_{xx} & -I_{xy} & -I_{xz} \\ -I_{xy} & I_{yy} & -I_{yz} \\ -I_{xz} & -I_{yz} & I_{zz} \end{bmatrix} \begin{bmatrix} p\\q\\r \end{bmatrix}$

But if we assume our quadcopter is symmetric about both the x and y axes of the body frame, then the products of inertia all go to zero and this becomes:

$\begin{bmatrix} L\\M\\N \end{bmatrix} = \begin{bmatrix} I_{xx} & 0 & 0 \\ 0& I_{yy} &0 \\ 0 & 0& I_{zz} \end{bmatrix} \begin{bmatrix} \dot{p}\\ \dot{q}\\ \dot{r} \end{bmatrix} + \begin{bmatrix} 0 & -r & q \\ r & 0 & -p \\ -q & p & 0 \end{bmatrix} \begin{bmatrix} I_{xx} & 0 & 0 \\ 0& I_{yy} &0 \\ 0 & 0& I_{zz} \end{bmatrix} \begin{bmatrix} p\\q\\r \end{bmatrix}$

$\begin{bmatrix} L\\M\\N \end{bmatrix} = \begin{bmatrix} I_{xx}\dot{p}\\ I_{yy}\dot{q}\\ I_{zz}\dot{r} \end{bmatrix} + \begin{bmatrix} -I_{yy}qr + I_{zz}qr\\ I_{xx}pr - I_{zz}pr\\ -I_{xx}pq + I_{yy}pq \end{bmatrix}$

### Angular Velocity vs. Euler Angle Rates

We now have an equation for how the angular velocity $[p, q, r]$ changes over time. These are the values which a gyroscope sensor would measure on-board a quadcopter. One caveat though, is that the angular velocity about the body-fixed frame is not the same as the rate of change of the Euler angles $[\phi, \theta, \psi]$.

This is because the Euler angles represent a sequence of rotations each in their own intermediate frame of reference, whereas the angular velocity is the instantaneous angular rate about each of the body-fixed axes. We can use coordinate transformations to express the angular velocity as a function of the Euler angle derivatives by transforming them all into the body frame.

To help make this clear I’ve labeled the intermediate reference frames in the sequence of Euler rotations below as b” and b’. So if we want to know how $\dot{\psi}$ would be represented in the body frame (b), we must transform it from its intermediate frame. Here we see $\dot{\psi}$ is the rotation rate about unit vector $\hat{b_{3}}$ in the b” frame, so we will refer to it as $\hat{b''_{3}}$. However in the final body-fixed frame, b, that unit vector has been rotated by the second and third Euler angles in the sequence.

Therefore, the rotation rates measured in the body-fixed frame $(p, q, r)$ due to the rate of change of the first Euler angle will equal the $\dot{\psi}$ rotation transformed by the $\theta$ and $\phi$ rotations. (Since its rotation angle is also parallel with the inertial axis $\hat{z}$, you could get the same answer by multiplying it by all three Euler angle rotations with the $\textbf{C}_{n}^{b}$ inertial-to-body coordinate transformation derived in Modeling Vehicle Dynamics – Euler Angles).

The second Euler rotation, $\dot{\theta}$ is treated similarly, and $\dot{\phi}$ needs no rotation as the unit vector it rotates around, $\hat{b'_{1}} = \hat{b_{1}}$, is a unit vector in the body frame, b.

The relationship between measured gyro rates and Euler angle rates can be expressed in scalar form multiplied by unit vectors as:

$p(\hat{b_{1}}) + q(\hat{b_{2}}) + r(\hat{b_{3}}) = \dot{\psi}(\hat{b''_{3}}) + \dot{\theta}(\hat{b'_{2}}) + \dot{\phi}(\hat{b_{1}})$

If we express it in matrix form however, we can take advantage of our rotation matrices to handle the disparate reference frames:

$\begin{bmatrix} p\\q\\r \end{bmatrix}^{b} = \textbf{R}(\phi)\textbf{R}(\theta) \begin{bmatrix} 0 \\ 0 \\ \dot{\psi} \end{bmatrix}^{b''} + \textbf{R}(\phi) \begin{bmatrix} 0 \\ \dot{\theta} \\ 0 \end{bmatrix}^{b'} + \begin{bmatrix} \dot{\phi} \\ 0 \\ 0 \end{bmatrix}^{b}$

To make things a little easier to read and fit on the page, I’m going to use a shorthand for the trigonometric functions: $c_{\theta} = \cos(\theta)$ and $s_{\theta} = \sin(\theta)$. The above expression can be simplified by multiplying the matrices.

$\begin{bmatrix} p\\q\\r \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & c_{\phi} & s_{\phi} \\ 0 & -s_{\phi} & c_{\phi} \end{bmatrix} \begin{bmatrix} c_{\theta} & 0 & -s_{\theta} \\ 0 & 1 & 0 \\ s_{\theta} & 0 & c_{\theta} \end{bmatrix} \begin{bmatrix} 0 \\ 0 \\ \dot{\psi} \end{bmatrix} + \begin{bmatrix} 1 & 0 & 0 \\ 0 & c_{\phi} & s_{\phi} \\ 0 & -s_{\phi} & c_{\phi} \end{bmatrix} \begin{bmatrix} 0 \\ \dot{\theta} \\ 0 \end{bmatrix} + \begin{bmatrix} \dot{\phi} \\ 0 \\ 0 \end{bmatrix}$

$\begin{bmatrix} p\\q\\r \end{bmatrix} = \begin{bmatrix} -\dot{\psi}s_{\theta} \\ \dot{\psi}c_{\theta}s_{\phi} \\ \dot{\psi}c_{\phi}c_{\theta} \end{bmatrix} + \begin{bmatrix} 0 \\ \dot{\theta}c_{\phi} \\ -\dot{\theta}s_{\phi} \end{bmatrix} + \begin{bmatrix} \dot{\phi} \\ 0 \\ 0 \end{bmatrix}$

So finally we arrive at the set of equations:

$p = \dot{\phi} - \dot{\psi}\sin\theta \\ q = \dot{\theta}\cos\phi + \dot{\psi}\cos\theta \sin\phi \\ r = \dot{\psi}\cos\phi \cos\theta - \dot{\theta}\sin\phi$

These equations can also be rewritten for the Euler angle derivatives:

$\dot{\phi} = p + (q \sin\phi + r \cos\phi) \tan\theta \\ \dot{\theta} = q \cos\phi - r \sin\phi \\ \dot{\psi} = (q \sin\phi + r \cos\phi) \sec\theta$

To determine the motion of the vehicle within our navigation coordinate frame, we simply transform our body frame velocity states with our Euler angle transformation matrix. Also we define our height variable, h, as the negative of our North-East-Down inertial frame to get a North-East-Up navigation frame. So below I’ve made the substitution $z=-h$.

$\begin{bmatrix} \dot{x}^{E}\\\dot{y}^{E}\\-\dot{h}^{E} \end{bmatrix} = \textbf{C}_{b}^{n} \begin{bmatrix} u\\v\\w \end{bmatrix}^{b} = \begin{bmatrix} c_{\theta}c_{\psi} & -c_{\theta}s_{\psi}+s_{\phi}s_{\theta}c_{\psi} & s_{\phi}s_{\psi}+c_{\phi}s_{\theta}c_{\psi}\\ c_{\theta}s_{\psi} & c_{\phi}c_{\psi}+s_{\phi}s_{\theta}s_{\psi} & -s_{\phi}c_{\psi}+c_{\phi}s_{\theta}s_{\psi} \\ -s_{\theta} & s_{\phi}c_{\theta} & c_{\phi}c_{\theta} \end{bmatrix} \begin{bmatrix} u\\v\\w \end{bmatrix}^{b}$

## The Quadcopter Equations of Motion

Finally we have collected all the pieces to fully describe the motion of the quadcopter. Collecting all of the equations into one set and rearranging them we arrive at:

$\dot{u} = -g\sin(\theta) + rv - qw \\ \dot{v} = g\sin(\phi)\cos(\theta) -ru + pw \\ \dot{w} = \frac{1}{m}(-F_{z}) + g\cos(\phi)\cos(\theta) + qu - pv \\ \dot{p} = \frac{1}{I_{xx}}(L + (I_{yy}-I_{zz})qr) \\ \dot{q} = \frac{1}{I_{yy}}(M + (I_{zz}-I_{xx})pr) \\ \dot{r} = \frac{1}{I_{zz}}(N + (I_{xx}-I_{yy})pq) \\ \dot{\phi} = p + (q \sin\phi + r \cos\phi) \tan\theta \\ \dot{\theta} = q \cos\phi - r \sin\phi \\ \dot{\psi} = (q \sin\phi + r \cos\phi) \sec\theta \\ \dot{x}^{E} = c_{\theta}c_{\psi}u^{b} + (-c_{\phi}s_{\psi}+s_{\phi}s_{\theta}c_{\psi})v^{b} + (s_{\phi}s_{\psi}+c_{\phi}s_{\theta}c_{\psi})w^{b} \\ \dot{y}^{E} = c_{\theta}s_{\psi}u^{b} + (c_{\phi}c_{\psi}+s_{\phi}s_{\theta}s_{\psi})v^{b} + (-s_{\phi}c_{\psi}+c_{\phi}s_{\theta}s_{\psi})w^{b} \\ \dot{h}^{E} = -1*(-s_{\theta}u^{b} + s_{\phi}c_{\theta}v^{b} + c_{\phi}c_{\theta}w^{b})$

where

$F_{z} = F_1 + F_2 + F_3 + F_4 \\ L = F_1 d_{1y} - F_2 d_{2y} - F_3 d_{3y} + F_4 d_{4y} \\ M = F_1 d_{1x} - F_2 d_{2x} + F_3 d_{3x} - F_4 d_{4x} \\ N = -T(F_1,d_{1x},d_{1y}) - T(F_2,d_{2x},d_{2y}) + T(F_3,d_{3x},d_{3y}) + T(F_4,d_{4x},d_{4y})$

and $h^E$ is positive in the up direction.

## Up next

The above set of equations describes the motion of the quadcopter as a set of nonlinear differential equations. Still needed are models for the propeller force and torque functions and values plugged in for the mass and moment of inertia variables. But simple estimates and approximations would be good enough to start simulating vehicle dynamics.

In the next post we will look at how we can program a simulation which can solve for a time history of this motion by numerically integrating these equations over time from a given initial condition.

[1] Nelson, Robert, “Flight Stability and Automatic Control”
A good starting point with straight-forward text on deriving the basic rigid body equations of motion for an airplane. Books by Etkin and Roskam are also common resources for this.

[2] Schmidt, David K., “Modern Flight Dynamics”

This is a good reference for a thorough description of the fundamentals in deriving these equations within the multiple frames of reference. It also provides in other chapters derivations of the equations of motion for elastic bodies as well as for the general linearized equations of motion. The mathematical notation may take a little getting used to in comparison to some of the more classic texts, as all equations are described as the integration of all point masses over the volume of the vehicle. However, the text descriptions are easy to understand.

[3] Stengel, Robert F., “Flight Dynamics”

This author provides very clear formulations of the equations and also provides useful information on the numerical integration of them. Also includes model dynamics for various lift and thrust sources, including propellers through actuator disk theory.

[4] Lewis, Frank L., and Stevens, Brian L., “Aircraft Control and Simulation”

This book is a great resource for practical examples of the equations in use for simulation of vehicles.

## 13 thoughts on “Modeling Vehicle Dynamics – Quadcopter Equations of Motion”

1. Nice work :). I’ve spotted a small error though, in the rotation matrix from the inertial- to the body reference frame.
The term in the second row of the first column should be: -cos(phi) * sin(psi) + sin(phi)*sin(theta)*cos(psi)
This mistake is repeated in the rotation matrix from body to inertial in the x_dot, y_dot, z_dot (navigation coordinates) step, but then it occurs in the first row of the second column (due to the transpose operation).

Also, I have a question regarding the u, v, w values in your work. You state these are the velocities as seen from the Inertial frame, in body-frame coordinates. However, in the calculation of v_dot you actually throw away part of (vdot)_inertial by subtracting the cross-product of omega and v, which leaves you with vdot as seen from the body-frame and not the inertial frame, so (vdot)_body. This means that the [u,v,w] vector, which is simply the integral of vdot, is actually a non-inertial velocity. You subsequently express the [u,v,w] vector in inertial reference frame coordinates to obtain your navigation coordinates [x, y, z]. I would argue these navigation coordinates should be obtained by integrating the velocities w.r.t. the inertial frame (vdot)_inertial, and not integrating velocity w.r.t. the body frame (vdot)_body, as is happening now. What are your thoughts on this?

Kind regards,
Lenard

2. hi , it is a great explanation I ever seeing in web
if introduce a nonlinar control , that will be great job

3. Hi Charlie!
Is there anyway you could help me modify the code in order to implement its own control?
(i.e. make a pre-determined trajectory and give it control law to follow the trajectory?)
Any help with this would be immensely helpful.
My contact is dmachatsch@gmail.com

Thank you!!

4. I think sign of L and M (Rolling moment and pitching moment) should be changed.

F1 and F4 generates roll left moment (-), and F2 and F3 generates roll right moment (+).
and F1 and F3 generates pitch up moment (+), and F2 and F4 generates pirch down moment (-).
but in this article, sign of L and M is opposite to my opinion.

Can you check the sign of L and M?
or please explainn what am I thinking wrong

5. I have a doubt as why you transformed forces in inertial frame where F= ma is valid for inertial frame . Here x, xdot , xdouble dot are defined wrt to inertial frame then shouldn’t the forced be transformed to the inertial frame.

6. Nicely summarized. I really enjoyed while reading and I have one or two questions if you don’t mind. In Yaw moment calculation you said, ” Each propeller as it pushes against the air will also apply a torque about the cg of the quadcopter in the direction of its rotation as a function of its thrust, propeller radius, and distance from the cg” but for yaw motion, propeller of quadrotor uses Counter angular momentum that will try to change the quadrotor’s attitude in the opposite direction of propeller rotation. Am I wrong?

For Moment Calculations, why did you represent the L and M like this
L= F1d1y-F2d2y-F3d3y+F4d4y
M=-F1d1x+F2d2x-F3d3x+F4d4x , is it because of the forces in z direction taken as minus ? Otherwise It would have been like this when we consider the BFF and Right Hand Rule
L= -F1d1y+F2d2y+F3d3y-F4d4y
M=+F1d1x-F2d2x+F3d3x-F4d4x

Final Question
Which Body Fixed Frame representation is correct ? The one you showed here or here: https://www.researchgate.net/figure/Inertial-and-body-fixed-frame-of-quadrotor_fig1_270163654
Because Moment equations will change when you apply second BFF coordination.

I will be appriciated ,if you reply
Thanks

7. Outstanding work! The combination of skillful, patient description and runnable Python code are going to make these pages the standard reference for teaching and research with quadcopters and other multiotor vehicles. Kudos, and thanks.

8. Hi there! Is there a small mistake just above the list of equations of motion in the “Navigation Coordinates” section? The matrix should be time derivatives of x_e, y_e, -z_e
on the left-hand-side shouldn’t they?
And if so shouldn’t that mean that the time derivative of h_e in the equations of motion list be negated?

Really great post though!
Thanks 🙂

1. Alex, you’re right! Thanks for pointing out the error, I’ve updated the content with the correction in both the Navigation Coordinates and Quadcopter Equations of Motion sections.
As a sanity check, for a scenario where the vehicle is pitched up (sine of theta will be positive) and there is positive x-direction velocity with respect to the body (u is positive), then this will contribute to an increase of altitude (h-dot receives a positive contribution) in the inertial/Earth frame, which can now be seen in the final EOM.