cosine of angle in radians

sine of angle in radians

Euler rotations from body-frame to global inertial frame angle 0 = roll (x-axis, phi) angle 1 = pitch (y-axis, theta)

tangent of angle in radians

rotate body angular velocity (Euler_dot) to inertial angular velocity (omega)

Returns the error between actual angle and reference angle

Returns the error between angular velocity position and reference angular velocity

Returns the error between actual position and reference position

Returns the error between actual velocity and reference velocity

finds speeds of motors to achieve a desired thrust and torque

Find linear acceleration in m/s

Find the angular acceleration in the inertial frame in rad/s

Euler rotations from inertial to body frame (Transpose of body-to-internal rotation)

rotate inertial angular velocity (omega) to body angular velocity (Euler_dot)

This is a full plot of the results. It will plot the 3D flight path, vertical and lateral positions, lateral velocity, motor thrusts, body to

The program initializes with a random perturbation in angular velocity on the vehicle. This simulates a wind disturbace. This is a disp

Plots to the magnitude of the position error vector (m)

Plots the laterial position, vertical position, and Euler angles over time. This is a quick plot for trouble shooting