1

QUADROTOR CONTROL, STABILIZATION & TELEOPERATION WITH KINECT

BUGRA TURAN | PHISIT JORPHOCHAUDOM | SARUNYU HEMHONG

AMIN ATRASH Ph.D.

University of Southern California

Viterbi School of Engineering

APRIL 2012

2

Table of Contents Table of Contents .............................................................................................................................................. 2 Figure List ........................................................................................................................................................... 3 1. Abstract ....................................................................................................................................................... 4

Introduction ................................................................................................................................................... 5 2. QUADROTOR MODELING ............................................................................................................... 5

Modeling Assumptions ................................................................................................................................. 7 Design Parameters ......................................................................................................................................... 7 Rhinoceros Quadrotor CAD Model Drawings ........................................................................................ 8

3. CONTROLLER DESIGN ................................................................................................................... 11 Controller Design &Performance ............................................................................................................. 11 MATLAB Simulink Controller Simulations ............................................................................................ 14

4. QUADROTOR SIMULATION .......................................................................................................... 16 5. QUADROTOR TELEOPERATION ................................................................................................ 18

Body Tracking .............................................................................................................................................. 18 Teleoperation via Kinect ............................................................................................................................ 19

6. Conclusions .............................................................................................................................................. 20 References ......................................................................................................................................................... 21 APPENDIX I- MATLAB & C++ SOURCE CODES ............................................................................ 22 Teleoperation Code Explanation ................................................................................................................... 25

Quadrotor Simulation Video Link

3

Figure List Figure 1 Hector_quadrotor Package Sample Quadrotor Model ................................................................. 5 Figure 2 Hector_quadrotor Package Sample Quadrotor Model ................................................................. 6 Figure 3 Hector_quadrotor Package Sample Quadrotor Model ................................................................. 6 Figure 4 Hector_quadrotor Package Sample Quadrotor Model ................................................................. 7 Figure 5 Custom Built Quadrotor Model Top Perspective ......................................................................... 8 Figure 6 Custom Built Quadrotor Model Cross Top Perspective .............................................................. 9 Figure 7 Custom Built Quadrotor Model Front Perspective ...................................................................... 9 Figure 8 Custom Built Quadrotor Top Perspective ................................................................................... 10 Figure 9 Custom Built Quadrotor Model Rendered Perspective ............................................................. 10 Figure 10 Hovering .......................................................................................................................................... 11 Figure 11 Thruster Movement ....................................................................................................................... 11 Figure 12 Roll Movement ............................................................................................................................... 11 Figure 13 Pitch Movement ............................................................................................................................. 12 Figure 14 Yaw Moment .................................................................................................................................. 12 Figure 15 Matlab Simulink Model ................................................................................................................. 14 Figure 16 Yaw,Pitch and Roll angle Transient Response Characteristics................................................ 15 Figure 17 Matlab 3D Display Simulation ..................................................................................................... 15 Figure 18 Outdoor Simulation ....................................................................................................................... 16 Figure 19 Simulation Nodes and Topics ...................................................................................................... 17 Figure 20 Body Movement Commands on Rviz with Skeleton_Tracker ................................................ 18

4

1. Abstract

This project explores the feasibility of Microsoft Kinect sensor usage for quadrotor teleoperation simulation on gazebo. Quadrotors are used for different purposes because of their simple structure and decent load capacity. When compared with helicopters, quadrotors are easy to stabilize and control. Quadrotors are useful for close area monitoring, bomb searches in difficult areas, search and rescue purposes when they carry camera. One of the main objective in quadrotor design is to increase its payload capacity while maintaining stability.

The goal of this project is to provide a new approach for teleoperation in order to test

stability of the quadrotor in simulation. Even though teleoperation with keyboard is possible, during simulation using keyboard for teleoperation limits user for value input into controllers. Teleoperation with Kinect provides advantage of using keyboard if any tuning is needed for controllers. Tools such as computer aided design, MATLAB Simulink real-time simulations, MATLAB transient response analysis in 3 Degree of freedom, Robotics Operating System are utilized to address different issues encountered throughout the project development. As final configuration teleoperation of Quadrotor with Microsoft Kinect camera was achieved successfully. Simulation results showed that controllers are well-tuned enough to maintain stability of the quadrotor even for fast direction command changes.

5

Introduction

The goal of the quadrotor is to maintain a hover at a user-defined altitude while minimizing lateral drift. Quadrotors are symmetrical vehicles with four equally sized rotors at the end of four equal length rods. Quadrotors make use of multiple rotors allowing for a greater amount of thrust and consequently a greater amount of maneuverability. Also, the quadrotors symmetrical design allows for easier control of the overall stability for the flight.

This project is organized as follows, in the second chapter custom quadrotor modeling achieved, in the third chapter controllers were built and tuned for quadrotor, quadrotor simulation was tested in the fourth chapter, finally quadrotor simulation with body tracking objective was realized in the 5th chapter.

2. QUADROTOR MODELING Even though hector_quadrotor simulation package comes with a basic quadrotor model ,so that the quadrotor dimension values are not practical and it was not clear whether external forces were taken into account or not for controller implementation. Thus custom model was needed for controller design, flight simulation and quadrotor teleoperation via kinect. Quadrotor is considered to be built with aluminium which has density of 2.8g/cm^3. ( 0.0000028 kilogram/cubic millimeter) Quadrotor model provided with hector_quadrotor package has following non-realistic dimensions; Total Length = 0.08mm Cumulative Area=0.5mm^2 Mass=0.14 gram

Simulation Package Given Quadrotor Models (Units in milimeter)

Figure 1 Hector_quadrotor Package Sample Quadrotor Model

6

Figure 2 Hector_quadrotor Package Sample Quadrotor Model

Figure 3 Hector_quadrotor Package Sample Quadrotor Model

7

Figure 4 Hector_quadrotor Package Sample Quadrotor Model

Modeling Assumptions In order to create practical quadrotor model following assumptions were made,

1)System input is considered as propeller speed, 2)System output is considered as position and attitude, 3)Center of Gravity and body frame origin are assumed to coincide, 4)Structure is assumed to be symmetric so diagonal inertia matrix is used.

Design Parameters Following parameters are used to model our quadrotor dynamics, g = 9.81; [m/s^2] Quadrotor body mass (m) : 0.4794; (kg) Arm length (l) : Planar distance between Center of Gravity and rotor plane, (20,33cm) Rotor height (h) :Height between center of gravity and rotor plane,(4.52cm) Thrust Constant (b) :In order to calculate thrust force with the formula Thrust Force = b* (propeller rotational speed)^2 (3.13e-5) Drag Constant(d) :In order to calculate drag moment with the formula Drag moment= d* (propeller rotational speed)^2 (9e-7)

8

Rotor Inertia (Jr) :Inertial counter torque moment is calculated as Jr*propeller rotational acceleration Inertia matrix (I) :Diagonal matrix with the following values used Ixx = 0.0086; [kg.m^2] Iyy = 0.0086; [kg.m^2] Izz = 0.0172; [kg.m^2] gravity vector offset = 0.0

Rhinoceros Quadrotor CAD Model Drawings

Quadrotor model built different perspective views according to practical values (units in centimeter )

Figure 5 Custom Built Quadrotor Model Top Perspective

9

Figure 6 Custom Built Quadrotor Model Cross Top Perspective

Figure 7 Custom Built Quadrotor Model Front Perspective

10

Figure 8 Custom Built Quadrotor Top Perspective

Figure 9 Custom Built Quadrotor Model Rendered Perspective

11

3. CONTROLLER DESIGN

Controller Design &Performance Basic Control Concept of Quadrotor is as follows,

Figure 10 Hovering

In hovering state (Fig 10), the front and the rear propellers rotate counter-clockwise, while the left and the right ones turn clockwise. All the propellers have the same speed.

Figure 11 Thruster Movement

In throttle state (Fig 11), this state is controlled by increasing (or decreasing) all the propeller speeds by the same amount.

Figure 12 Roll Movement

12

Roll state (Fig 12), is controlled by increasing (or decreasing) the left propeller speed and by

decreasing (or increasing) the right one. It leads to a torque with respect to the axis which makes the quadrotor turn

Figure 13 Pitch Movement

. Pitch state (Fig 13), is very similar to the roll and is controlled by increasing (or decreasing) the rear propeller speed and by decreasing (or increasing) the front one. It leads to a torque with

respect to the axis which makes the quadrotor turn.

Figure 14 Yaw Moment

Yaw state (Fig 14), is controlled by increasing (or decreasing) the front-rear propellers’ speed and by decreasing (or increasing) that of the left-right couple. It leads to a torque with respect to the

axis which makes the quadrotor turn. In order to control thrusters, the objective is to find each propeller’s speed.

( PD control was used for altitude and attitude control.

13

14

MATLAB Simulink Controller Simulations

Figure 15 Matlab Simulink Model

Simulink model includes noise and motor dynamics switches, in order to provide robust controller tuning. Simulation desired values are selected as, Roll = 0.2 rad Pitch = 0 rad Yaw = 0 rad Altitude = 5 m

15

Figure 16 Yaw,Pitch and Roll angle Transient Response Characteristics

Figure 17 Matlab 3D Display Simulation

16

4. QUADROTOR SIMULATION

Figure 18 Outdoor Simulation

Hector_quadrotor_demo package is used for simulation purposes.Simulation package uses

sonar and pressure sensor for altitude control, gps_velocity for angular velocity measurement, imu includes accelerometer (linear acceleration measurement ) and gyroscope (rate of turn measurement). Tf is utilized for coordinate transformations cmd_vel is the subscribed topic, that controllers are attached to control velocity of thrusters. Below all the subscribed and published topics are listed.

Publications: * /camera/image/compressed/parameter_updates [dynamic_reconfigure/Config] * /gps_velocity [geometry_msgs/Vector3Stamped] * /gazebo/model_states [gazebo_msgs/ModelStates] * /gazebo/parameter_descriptions [dynamic_reconfigure/ConfigDescription] * /raw_imu [sensor_msgs/Imu] * /pressure_height [geometry_msgs/PointStamped] * /tf [tf/tfMessage] * /sonar_height [sensor_msgs/Range] * /gazebo/parameter_updates [dynamic_reconfigure/Config] * /magnetic [geometry_msgs/Vector3Stamped] Subscriptions:

17

* /set_update_rate [unknown type] * /clock [rosgraph_msgs/Clock] * /gazebo/set_model_state [unknown type] * /cmd_vel [unknown type]

Figure 19 Simulation Nodes and Topics

18

5. QUADROTOR TELEOPERATION

Body Tracking Openni_kinect stack that contains openni_camera and openni_tracker drivers used, in order to capture body movements to provide movements for quadrotor. Pi_tracker and skeleton_markers packages from Pi Robot repository was used to see how the robot tracks skeleton. Multiple tracking tests were done with changing the distance between body and kinect. It is assured that tracking of skeleton movements is guaranteed under 3.5 feet range. Rviz 3D visualization environment was used to visualize real time body moevements. Rviz screen captures for stop, up, left rotate, right rotate, left turn, right turn, down commands are depicted below.

Figure 20 Body Movement Commands on Rviz with Skeleton_Tracker

In order to define arm movements to control quadrotor movements, two different fixed

points in the skeleton was tested. Both neck and torso fixed points accuracy was analyzed. After running several simulations it is realized that using neck as fixed point gives not only faster response but also more accurate transition between states. During simulations it is concluded that, when torso

19

is defined as fixed point, kinect does not track fast transitions between arm movements. As an example for given rise up command (both arms are pointing upwards ), if the arm is immediately moved to left turn (left arm makes straight angle with torso) , quadrotor hesitates between stop and left turn.

Teleoperation via Kinect

As mentioned above, Kinect tracked body movements and published to ROS message system by openni_tracker. However, information about body movements was defined by translation vector. Translation vector define how particular object move in 3d space. Translation vectors are in 3d space. Thus, arm movements can be tracked using these translation vectors.

In order to successfully perform teleoperation, simple set of arm movements have been defined. Basically, translation vector value in y-axis increases when arm moves upward and vice versa. These movements hold true in x-axis(horizontal movement) and z-axis(distance from body to kinect) as well. After arm movements were tracked and translated into translation vectors, simple set of commands were defined according to those movements. In this project, arm movements were translated into commands as following.

Left Arm Right Arm Command

Move upward Move upward Increase height

Stretch to side Stretch to side Decrease height

No movement/Move to stand position

Stretch to side Go forward

Stretch to side No movement/Move to stand position

Go backward

No movement/Move to stand position

Move upward Rotate counter-clockwise

Move upward No movement/Move to stand position

Rotate clockwise

No movement/Move to stand position

No movement/Move to stand position

Stop

20

6. Conclusions

Quadrotor with practical dimensions was created in Rhinoceros CAD software platform. Area and thickness was calculated using CAD software. Quadrotor main frame assumed to be built from aluminium and density of quadrotor was considered as 2.8g/cm^3. As a result quadrotor mass is calculated to be 0.4794kg. When compared to the model provided by hector_quadrotor package, this model is almost 65% larger in size and 40% heavier in weight. Controllers for 3 degree of freedom built according to the model created. Even though first attempt was to implement PID controllers, so that desired command tracking obtained without integral gain, controllers selected as PD controllers. MATLAB Simulink simulation with transient response analysis in 3 degree of freedom considered for a robust PD controller in order to ensure stability for the noisy flight conditions. Proportional and derivative gains are tuned for optimum response and settling time.

Microsoft Kinect was used for teleoperation of the quadrotor. Different fixed points in skeleton were considered to obtain fastest and most accurate response. Movements with respect to neck is found to be the most accurate teleoperation method for quadrotor. Quadrotor stabilization, control and teleleoperation with Kinect goals are successfully achieved by the usage of hector_quadrotor_demo simulation package. Kinect teleoperation was used both indoor and outdoor simulations, with the PD controller implementations .Kinect teleoperation was found to be useful in order to evaluate quadrotor modeling accuracy and controller performance. Kinect teleoperation gives user more flexibility in terms of movements. Keyboard, x-box, ps3 teleoperation is possible for quadrotor simulation. If controller tuning is the objective, usage of these methods will limit user to run simulation then tune the parameters. With kinect teleoperation user is still able to run the simulation ,control movements while using keyboard to feed input for tuning controller parameters.

Video link is provided showing complete quadrotor simulation with kinect teleoperation.

Quadrotor Simulation Video Link

21

References

[1] http://www.grc.nasa.gov/WWW/K-12/airplane/short.html (general) [2] https://dspace.ist.utl.pt/bitstream/2295/574042/1/Tese_de_Mestrado.pdf [3] http://www.wpi.edu/Pubs/E-project/Available/E-project-031411-

140744/unrestricted/MQP_Rotorheads_Final_Report.pdf [4] http://www.ros.org/wiki/pi-robot-ros-pkg [5] http://www.ros.org/wiki/kinect [6] http://ros.org/wiki/openni_tracker

22

APPENDIX I- MATLAB & C++ SOURCE CODES

Source Code #include <iostream> #include <geometry_msgs/Twist.h> #include <ros/ros.h> #include <geometry_msgs/TransformStamped.h> #include <tf/transform_listener.h> #include "std_msgs/String.h" /* * numState: * Convert translation vector from Transform Stamp object * into position of arm. * */ int numState(geometry_msgs::TransformStamped &msg1){ float positionY = msg1.transform.translation.y; int num = 0; if (positionY < -0.4){ num = 1; } else if(positionY>-0.3 && positionY<0.3){ num = 2; } else if(positionY>0.4){ num = 3; } return num; } /* * checkState * Convert position of arm into machine state. */ int checkState(int right, int left){ int ret = 0; if(right==3 && left==3 ){ //up ret = 1; } else if(right==2 && left==2 ){ //down ret = 2;

23

} else if(right==1 && left==2 ){ //forward ret = 3; } else if(right==2 && left==1 ){ //back ret = 4; } else if(right==3 && left==1 ){ //rotate counter-clock ret = 5; } else if(right==1 && left==3 ){ //rotate clock |_ O (down) ret = 6; } else if(right==1 && left==1 ){ //stop stand ret = 7; } else { ret = 8; } return ret; } int main(int argc, char** argv) { //init the ROS node ros::init(argc, argv, "robot_driver"); // ROS node ros::NodeHandle nh; // Command publisher ros::Publisher cmd_vel_pub_; // Listener to listen to kinect publisher tf::TransformListener listener_; // Stamped transform tf::StampedTransform right_hand; tf::StampedTransform right_elbow; tf::StampedTransform left_hand; tf::StampedTransform left_elbow; // Message received from listener geometry_msgs::TransformStamped msg_right_hand; geometry_msgs::TransformStamped msg_left_hand; geometry_msgs::TransformStamped msg_right_elbow; geometry_msgs::TransformStamped msg_left_elbow; // Advertise to command publisher which connect to Gazebo cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1); bool check = true; geometry_msgs::Twist base_cmd; base_cmd.linear.y = base_cmd.angular.z = 0;

24

while(check) { try { // Listen to Kinect and retrieve transform object of // right hand of user 1 relative to neck of user 1 listener_.lookupTransform("neck_1", "right_hand_1", ros::Time(0), right_hand); // Listen to Kinect and retrieve transform object of // left hand of user 1 relative to neck of user 1 listener_.lookupTransform("neck_1", "left_hand_1", ros::Time(0), left_hand); // Convert transform stamped object to ROS message tf::transformStampedTFToMsg(right_hand, msg_right_hand); tf::transformStampedTFToMsg(left_hand, msg_left_hand); // Find position of right hand and left hand. int state_right = numState(msg_right_hand); int state_left = numState(msg_left_hand); // Translate into machine's state int state = checkState(state_right, state_left); // Output to console ROS_INFO("State is %d", state); // Select case for state machine. switch(state){ case 1: // increase height from the ground base_cmd.linear.z = 5.0; break; case 2: // decrease height base_cmd.linear.z = -5.0; break; case 3: // Move to positive X-axis base_cmd.linear.x = 5.0; break; case 4: // Move to negative X-axis base_cmd.linear.x = -5.0; break; case 5: // Rotate counter-clockwise base_cmd.angular.z = 5.0; break; case 6: // Rotate clockwise base_cmd.angular.z = -5.0; break; case 7: // Stop base_cmd.linear.x = 0.0;

25

base_cmd.linear.y = 0.0; base_cmd.linear.z = 0.0; base_cmd.angular.x = 0.0; base_cmd.angular.y = 0.0; base_cmd.angular.z = 0.0; break; case 8: // Stop base_cmd.linear.x = 0.0; base_cmd.linear.y = 0.0; base_cmd.linear.z = 0.0; base_cmd.angular.x = 0.0; base_cmd.angular.y = 0.0; base_cmd.angular.z = 0.0; break; } // publish command to topic "cmd_vel" cmd_vel_pub_.publish(base_cmd); } catch (tf::TransformException ex) { ROS_ERROR("%s",ex.what()); } } }

Teleoperation Code Explanation

// ROS node

ros::NodeHandle nh;

Node handle object handles communication between program and ROS.

// Command publisher

ros::Publisher cmd_vel_pub_; ROS publisher publishes data to specified topic. In this program, topic named cmd_vel is being used.

// Listener to listen to kinect publisher

tf::TransformListener listener_;

Topic listener object listens to ROS. In our implementation, this object is being used to listen to Kinect commands. // Stamped transform tf::StampedTransform right_hand; tf::StampedTransform right_elbow;

26

tf::StampedTransform left_hand; tf::StampedTransform left_elbow;

Stamped transform object is a message format received from transform listener.

// Message received from listener

geometry_msgs::TransformStamped msg_right_hand; geometry_msgs::TransformStamped msg_left_hand; geometry_msgs::TransformStamped msg_right_elbow; geometry_msgs::TransformStamped msg_left_elbow;

geometry_msgs is a data structure which is being used to hold information from Kinect. // Advertise to command publisher which connect to Gazebo cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);

Tell ROS that cmd_vel_pub is publishing data to topic named “cmd_vel”. geometry_msgs::Twist base_cmd; base_cmd.linear.y = base_cmd.angular.z = 0;

base_cmd is a geometry_msgs. This variable is being used to send command to cmd_vel_pub. The second line initialize velocity to zero.

// Listen to Kinect and retrieve transform object of // right hand of user 1 relative to neck of user 1 listener_.lookupTransform("neck_1", "right_hand_1", ros::Time(0), right_hand);

// Listen to Kinect and retrieve transform object of // left hand of user 1 relative to neck of user 1 listener_.lookupTransform("neck_1", "left_hand_1", ros::Time(0), left_hand);

Listener look up for Kinect's command and use stamped transform object to contain data.

// Convert transform stamped object to ROS message

tf::transformStampedTFToMsg(right_hand, msg_right_hand); tf::transformStampedTFToMsg(left_hand, msg_left_hand);

Convert stamped transform object into ROS message. int numState(geometry_msgs::TransformStamped &msg1){ float positionY = msg1.transform.translation.y; int num = 0; if (positionY < -0.4){ num = 1;

27

} else if(positionY>-0.3 && positionY<0.3){ num = 2; } else if(positionY>0.4){ num = 3; } return num; }

According to empirical data, translation vector from transform stamped message can be translated into hand position. int checkState(int right, int left){ int ret = 0; if(right==3 && left==3 ){ //up ret = 1; } else if(right==2 && left==2 ){ //down ret = 2; } else if(right==1 && left==2 ){ //forward ret = 3; } else if(right==2 && left==1 ){ //back ret = 4; } else if(right==3 && left==1 ){ //rotate counter-clock ret = 5; } else if(right==1 && left==3 ){ //rotate clock |_ O (down) ret = 6; } else if(right==1 && left==1 ){ //stop stand ret = 7; } else { ret = 8; } return ret; }

This function translates hands' position into state. Each state represents different movement for robot.

// Output to console ROS_INFO("State is %d", state);

ROS_INFO prints desired text to console.

28

// Select case for state machine. switch(state){ case 1: // increase height from the ground base_cmd.linear.z = 5.0; break; case 2: // decrease height base_cmd.linear.z = -5.0; break; case 3: // Move to positive X-axis base_cmd.linear.x = 5.0; break; case 4: // Move to negative X-axis base_cmd.linear.x = -5.0; break; case 5: // Rotate counter-clockwise base_cmd.angular.z = 5.0; break; case 6: // Rotate clockwise base_cmd.angular.z = -5.0; break; case 7: // Stop base_cmd.linear.x = 0.0; base_cmd.linear.y = 0.0; base_cmd.linear.z = 0.0; base_cmd.angular.x = 0.0; base_cmd.angular.y = 0.0; base_cmd.angular.z = 0.0; break; case 8: // Stop base_cmd.linear.x = 0.0; base_cmd.linear.y = 0.0; base_cmd.linear.z = 0.0; base_cmd.angular.x = 0.0; base_cmd.angular.y = 0.0; base_cmd.angular.z = 0.0; break; }

This switch-case statement checks for state and assign a command to base_cmd.

// publish command to topic "cmd_vel"

cmd_vel_pub_.publish(base_cmd);

Publish command to cmd_vel_pub. As mentioned above, cmd_vel_pub_ is being advertised to cmd_vel topic. Topic:

29

In this program, topic named cmd_vel has been used to fulfill project implementation. cmd_vel is the topic which control velocity of the quadrotor. MATLAB System_Dynamics.m In the matlab file system_dynamics forces, states, system inputs and output defined as follows, x1 = in(1); x position x2 = in(2); xdot velocity x3 = in(3); y position x4 = in(4); ydot velocity x5 = in(5); z position x6 = in(6); zdot velocity x7 = in(7); phi roll angle x8 = in(8); body x angular velocity x9 = in(9); theta pitch angle x10 = in(10); body y angular velocity x11 = in(11); psi yaw angle x12 = in(12); body z angular velocity system inputs Om_1 = in(13); Om_ stands for propeller rotational speeds Om_2 = in(14); Om_3 = in(15); Om_4 = in(16); U1 = b*(Om_1^2 + Om_2^2 + Om_3^2 + Om_4^2); U2 = b*(-Om_2^2 + Om_4^2); U3 = b*(Om_1^2 - Om_3^2); U4 = d*(-Om_1^2 + Om_2^2 - Om_3^2 + Om_4^2); omega = - Om_1 + Om_2 - Om_3 + Om_4; - first order differential equations ------------------------------ xdot = x2; xddot = -1/m * (cos(x7)*sin(x9)*cos(x11) + sin(x7)*sin(x11)) * U1; ydot = x4; yddot = -1/m * (cos(x7)*sin(x9)*sin(x11) - sin(x7)*cos(x11)) * U1; zdot = x6; zddot = g - (1/m)*cos(x7)*cos(x9) * U1; phidot = x8+sin(x7)*sin(x9)/cos(x9)*x10+cos(x7)*sin(x9)/cos(x9)*x12; omegaxdot = x10*x12*(Iyy-Izz)/Ixx + Jr/Ixx*x10*omega + L/Ixx * U2; thetadot = cos(x7)*x10+sin(x7)*x12; omegaydot = x8*x12*(Izz-Ixx)/Iyy - Jr/Iyy*x8*omega + L/Iyy * U3 - gOffset*m*g*cos(x9); psidot = sin(x7)/cos(x9)*x10+cos(x7)/cos(x9)*x12; omegazdot = x8*x10*(Ixx-Iyy)/Izz + U4/Izz; function output - state variable derivatives -------------------- out(1) = xdot; xdot velocity out(2) = xddot; xddot acceleration out(3) = ydot; ydot velocity out(4) = yddot; yddot acceleration out(5) = zdot; zdot velocity

30

out(6) = zddot; zddot acceleration out(7) = phidot; phidot roll velocity out(8) = omegaxdot; phiddot acceleration out(9) = thetadot; thetadot roll velocity out(10) = omegaydot; thetaddot acceleration out(11) = psidot; psidot roll velocity out(12) = omegazdot; psiddot acceleration