Equations of Motion with Quaternions

This post is mainly intended as a reference for my future self. For everyone else, this is yet another post with slightly different symbols in the equations of motion.

Every time I work on a project involving equations of motion, I find myself searching through my old notes and browsing the internet. Instead of collecting the pieces again and again, I will do it one last time here.

Rigid Body Dynamics

Inertia Matrix

A rigid body has a mass and an inertia matrix . The inertia matrix is:

The inertia matrix is symmetric, so , , and .

State Vector

To describe the full state of a rigid body, we need to know its linear velocity, angular velocity, orientation, and position.

The velocity is defined in the body-frame. This will come in handy later. The velocity vector is:

where is forward, is right, and is down.

The angular velocity is defined in the body-frame as well:

where is the roll rate, is the pitch rate, and is the yaw rate. Positive roll is to the right, positive pitch is up, and positive yaw is to the right.

The orientation of the body relative to the inertial frame is defined by a quaternion:

where is the scalar part and is the vector part. The quaternion is normalized such that

Position is defined in a local north-east-down (NED) frame. This describes the position of the center of mass in the local reference frame. The position vector is:

While is also used for roll rate, I think the (lack of) subscripts will make it clear which is which.

If we put everything together, we get one big state vector:

For the equations of motion, we need to find the derivative of the state vector.

Linear Acceleration

For the velocity derivative, the equation for the acceleration in body frame is:

is the applied force in the body frame. The notation means angular velocity of the body frame with respect to the inertial frame (subscript), expressed in the body frame (superscript).

where , , and are the forces in the body frame.

Angular Acceleration

The angular velocity derivative is:

where is the moment vector in the body frame. This can be expanded further, but there's no point in doing that since most linear algebra libraries can easily perform this calculation.

Quaternion Derivative

The derivative of the quaternion is:

where that fancy symbol is the quaternion multiplication operator. This can also be written as:

where is the quaternion multiplication matrix:

Position Derivative

The position derivative is just the velocity, but in the inertial frame:

Since the velocity is defined in the body frame, we need to rotate that velocity vector to the inertial frame:

where is the rotation matrix from the body frame to the inertial frame. The rotation matrix is constructed from the quaternion:

State Derivative

Now we can put everything together. The state derivative is:

Propagating state with Runge-Kutta

We can use the Runge-Kutta method to calculate the state at a next time step. It's a numerical approximation of the integral of the differential equations.

where is the time step, and are the derivatives of the state vector at different points in time:

is the function that calculates the derivative of the state vector. After each calculation of , you need to normalize the quaternion.

Roll, pitch, yaw angles

Quaternions are nice in the differential equations, but for inputs and outputs we prefer Euler angles. For the initial state, we can get the quaternion for a given roll, pitch, and yaw angle:

where is roll, is pitch, and is yaw.

To obtain the roll, pitch and yaw angles from the quaternion:

And now what?

These equations are pretty cool and easily implemented in code. Next, you can start applying forces and moments to the rigid body and see what happens. For example, you can apply aerodynamic forces and moments to simulate the behavior of a multi-rotor or fixed-wing UAV.

Continue Reading