AN INTEGRATOR BACKSTEPPING CONTROLLER FOR A STANDARD HELICOPTER YITAO LIU THESIS

Transcription

1 AN INEGRAOR BACKSEPPING CONROLLER FOR A SANDARD HELICOPER BY YIAO LIU HESIS Submitted in partial fulfillment of the requirements for the degree of Master of Science in Electrical and Computer Engineering in the Graduate College of the University of Illinois at Urbana-Champaign, 010 Urbana, Illinois Adviser: Professor Mark W. Spong

2 ABSRAC his thesis investigates the use of an integrator backstepping controller for a standard helicopter. First, a dynamic model of the helicopter in hovering condition is obtained through the use of Newton-Euler equations. Next, the idea of an integrator backstepping controller is examined followed by the derivation of the actual controller. Finally, simulation results from MALAB are analyzed and potential future work is proposed. ii

3 ACKNOWLEDGMENS I would not have completed this project without the support of many people. First, I would like to thank my adviser, Professor M. W. Spong, for providing support throughout my years as a graduate student. I would also like to thank my parents, who served as my inspiration and provided never-ending love and support. iii

4 ABLE OF CONENS CHAPER 1: INRODUCION... 1 CHAPER : HELICOPER MODEL... CHAPER : NONLINEAR CONROL...11 CHAPER 4: SIMULAION RESULS...19 CHAPER 5: CONCLUSION AND FUURE WORK...6 REFERENCES...7 APPENDIX A: SIMULINK MODEL AND CODE... 8 iv

5 CHAPER 1: INRODUCION In order to successfully control an unmanned aerial vehicle (UAV), one must first understand the behavior of the UAV, specifically a helicopter in this thesis. Once the behavior of the UAV is comprehended, a mathematical model is generally developed based on either Newton s laws of motion or the Euler- Lagrange equation for motion. Only then can a controller be derived. After Leonardo da Vinci s first helicopter-like design in 148, hundreds of failed attempts at taking flight in a helicopter occurred, until the first successful flight in 1907 [1]. Helicopters have become an interesting area of study due to their unique ability of vertical takeoff and landing (VOL). In addition to the ability of VOL, helicopters can also hover, fly forward, backwards, and laterally. hese abilities make a helicopter based UAV very important in missions and tasks where human intervention is considered dangerous. In this thesis, a model of the helicopter in hovering condition is presented first. While there are options in deriving the model, the one used in this thesis will be based on Newton s laws of motion. With the given model, a nonlinear control design strategy, namely the integrator backstepping technique, is used to produce a controller. Finally, the model and controller will be simulated in Simulink along with certain embedded functions that are coded in Matlab. his will evaluate the effectiveness of the controller and ensure that such a controller is plausible. 1

6 CHAPER : HELICOPER MODEL he behavior of a helicopter must be modeled before a controller is obtained. his chapter describes the model of a standard configuration helicopter in hover flight conditions obtained by using Newton laws [], []. A model of a helicopter based on the Euler-Lagrange equation for motion can also be found in [] and [4]. As seen in Figure.1, the coordinate frame on the lower left corner, { x, y, z} I = E E E, is the right-handed inertial frame. A second coordinate frame, {,, } C= E E E, is the right-hand fixed body frame, and it is fixed on the center 1 of mass of the helicopter. A x matrix R SO(), shown in equation (.1), is the orthogonal rotation matrix used to convert from the fixed body frame of the helicopter to the inertial frame. he matrix denotes the helicopter orientation with respect to I, and η= ( ψ, θ, φ) describes the yaw, pitch, and roll angles of the helicopter, respectively. cosθ cosψ sinφ sinθ cosψ cos sinψ cosφ sinθ cosψ + sinφ sinψ R( η) = cosθ sinψ sinφ sinθ sinψ cos cosψ cosφ sinθ sinψ sinφ cosψ + sinθ sinφ cosθ cosφ cosθ (.1) he symbol SO () denotes the special orthogonal group of order. For any x matrix in SO (), the following properties hold [5]: 1 R = R SO(). det R= 1. he columns and rows of R are mutually orthogonal.

7 Each column and row of R is a unit vector. Figure.1. Helicopter position displayed in both inertial frame and fixed body frame. I is the inertial frame. C is the fixed body frame. l m is the distance from the main rotor hub to the center of mass of the helicopter, while l t is the distance from the tail rotor hub to the center of mass. here are three types of forces applied to the fuselage of the helicopter. One of them is from the gravitation pull of the earth. he other two come from the two rotors of the helicopter. Let M and denote the thrust vectors of the main and tail rotors respectively, where, R M. hen M and can be defined in the following manner: = E + E + E (.) M M 1 1 M M

8 = E + E + E (.) 1 1 Since the tail rotor does not have a swashplate, the thrust vector will always have the same direction. hus the thrust vector equation for the tail rotor can be rewritten as = E (.4) he thrust vector of the main rotor is a function of the flapping angle, β, which is the tilt of the main rotor disk with respect to its initial rotational plane, caused by the swashplate. he flapping angle can be broken down into two parts, the longitudinal flapping angle, a, and the lateral flapping angle, b. hese two angles are assumed to be controllable through the cyclic control of a helicopter. Figure.. Arbitrary main rotor thrust, M, expressed in the fixed body frame. 4

9 By using Euclidean geometry and trigonometry, the flapping angle, β, can be expressed in terms of both the longitudinal flapping and the lateral flapping angles. cosβ = cos a cosb 1 sin a sin b (.5) he components of the thrust vector of the main rotor M can also be expressed in terms of the longitudinal flapping and the lateral flapping angles. Since M is the projection of the main rotor thrust vector onto the E axis, as shown in Figure., it is expressed as M = cosβ M (.6) M = cos a cosb 1 sin a sin b M (.7) From Figure. and through simple trigonometric relationships, the component of the main rotor thrust projected onto the E 1 axis, expressed as M1, is defined as = tan a (.8) M1 M M1 = sin a cosb 1 sin a sin b M (.9) 5

10 Figure.. An arbitrary main rotor thrust project on the E 1 -E plane. Similarly from Figure.4, M, the component of the main rotor thrust projected onto the E axis, is defined as = tan b (.10) M M M = cos a sin b 1 sin asin b M (.11) 6

11 Figure.4. An arbitrary main rotor thrust project on the E -E plane. From equations (.7), (.9), and (.11), main rotor thrust vector M can be rewritten as M 1 sin a cosb 1 = sin bcos a = M M M 1 sin a sin b M cos a cosb (.1) If F is the total external force exerted on the helicopter and expressed in the inertia frame I, then F is defined by the summation of the gravitation pull of the earth plus the two thrust vectors of the main and tail rotors, where R is the rotational matrix given in equation (.1), m is the mass of the helicopter, and g is the standard gravity. 7

12 sin a cosb 1 F = R sin b cos a + RE + mge 1 sin asin b cos a cosb M z (.1) For simplification purposes, the total external force can be defined as F = R G + RE + mge (.14) M z where sin a cosb 1 G= sin b cos a 1 sin asin b cos a cosb (.15) In addition to the translational forces generated by the two rotors and gravity effects acting on the helicopter, the fuselage is also affected by the torques. hese torques are generated by the two thrust vectors M and since there are separations from the center of mass of the helicopter to the rotors. he distances from the center of mass to the main and tail rotor are defined by l M and l, respectively. l = l E + l E + l E (.16) M M 1 1 M M l = l E + l E + l E (.17) 1 1 It should be noted that gravitational force does not generate a torque since that force is applied at the center of mass on the helicopter and thus no separation exists between the application of force and the axis of rotation. he torques caused by the thrust vector of the main and tail rotors are then defined as [ l ] τ = G (.18) M M M 8

13 τ cos a cosb lm sin b cos a lm 1 = cos a cosb l sin a cosb l 1 sin a sin b sin a cosb lm + sin b cos a l M 1 M M 1 M M (.19) [ ] τ = l E (.0) τ l = 0 l 1 (.1) In addition to the torques from the thrust vectors, it will be assumed that the aerodynamic drags on the rotors generate some pure torques, known as antitorques. hen, the total torque applied to the fuselage is given by where earlier. Q M and [ ] [ ] τ = l G + l E + Q E Q E (.) M M M Q are the anti-torques of the main and tail rotors, as mentioned Now the complete dynamic model of the helicopter can be put together. Equation (.) denotes the velocity of the helicopter expressed in the inertial frame, I. Equation (.4) is the total translation force applied to the center of mass of the helicopter. Equation (.6) shows the rotational component of motion in a non-inertial frame. he symbol Ω denotes the angular velocity of the helicopter expressed in the non-inertial frame. he full dynamic model is then represented in the inertial frame, given by & ξ = v (.) mv& = RG + RE + mge (.4) M z R& = RΩˆ (.5) 9

14 [ ] [ ] IΩ= Ω & IΩ+ l G + l E + Q E Q E (.6) where ˆΩ is a skew symmetric matrix M M M 0 Ω Ω Ω= ˆ 0 Ω Ω1 Ω Ω1 0 (.7) Upon examination, equations (.4) and (.6) can be rewritten as mv& = ure + mge + RσΩ & (.8) z IΩ= Ω & IΩ+ Q E Q E +Ω+ & k u (.9) M 0 where u and Ω & are control inputs and σ and k 0 are defined as σ l l M = l 0 0 M lm lm 1 l 1 1 (.0) k l = l 0 M 0 M 1 (.1) 10

15 CHAPER : NONLINEAR CONROL In this chapter, a nonlinear control strategy, namely the integrator backstepping technique, will be used to control the model of a helicopter developed in the previous chapter. However, in order to use the integrator backstepping technique, the system must be in a particular cascaded structure called lower triangular feedback [6]. he model obtained earlier has two coupling terms, RσΩ & and k0u, that destroy the pure cascade structure. o complete the design of a nonlinear control, these coupling terms will not be included in the design steps..1 Motivation he integrator backstepping technique is a unique nonlinear control strategy. Unlike the popular nonlinear control strategy, feedback linearization, the backstepping technique allows the design to be more flexible. It avoids wasteful cancellation of nonlinear terms that happens with feedback linearization; in fact, it can even introduce additional nonlinear terms to improve the transient performance of the system [7]. Backstepping designs a controller recursively by taking some state variables as virtual controls and using them as intermediate control laws during each stage of the entire system. 11

16 . Integrator Backstepping Controller Let ξ d : R R be the desired position trajectory for the helicopter and ψ d : R R be the desired yaw trajectory. It will be assumed that ξ d ( t) and ψ d ( t) are both smooth trajectories, and therefore any arbitrary number of time derivatives are also smooth trajectories. hen, the objective is to find a control law ( u, Ω1, Ω, Ω ), such that the tracking error, ε, is asymptotically stable for all initial conditions, where ε : ( ξ ( ) ξ ( ), ψ ( ) ψ ( )) d d 4 = t t t t R (.1) defined as For the first subsystem, a partial error and the first Lyapunov function is z d 1= ξ ξ (.) 1 1 V1 = z1 z1 = z1 (.) he Lyapunov function, V 1, is positive definite since V (0) = 0 and 1 V1 ( z 1) > 0 for z1 0 [8]. he time derivative for the first Lyapunov function is then where ( d V& ) 1= z1 z& 1= z1 v v (.4) d v is the velocity of the desired position trajectory, also known as the first time derivative of the smooth trajectory, ξ d ( t). Consider the velocity variable, v, as a virtual control. A velocity stabilization signal, v ref subsystem., is introduced in this d vref v k1z1 = (.5) 1

17 By introducing equations (.5) into (.4), the time derivative of our first Lyapunov function becomes & 1 1= (.6) V k z z z m where z is a new partial error signal defined as z = mv mvref (.7) It should be noted that the time derivative of the first Lyapunov function, V & 1, is negative definite once the partial error signal z is driven to zero. It should also be noted that the constant k 1 is greater than zero in order for V & 1 to be negative definite. Now a second Lyapunov function, V, associated with the previous partial error, z, is defined for the second subsystem. Again, this Lyapunov function is positive definite. he time derivative of the second Lyapunov is defined by 1 V = z z (.8) V& = z z& = z ( ure + mge mv& ) (.9) z ref where z& is obtained by taking the time derivative of equation (.7). In this subsystem, consider the virtual control signal as ure. If a new stabilization control signal associated with the angular position of the helicopter is introduced, 1 Rref = ( ure) ref = mgez mv& ref + z1+ kz (.10) m then the time derivative of V can be rewritten as 1

18 = V& z z k z z z (.11) m A third partial error signal, z, is introduced in equation (.11), where z = R ure (.1) ref Once again, the time derivative of the second Lyapunov function, V &, is negative definite when the third partial error signal is driven to zero. he constant, k, must also be positive in order for V & to be negative definite. Continuing with the procedure of integrator backstepping, a third Lyapunov function, V, associated with the previous partial error, z, is introduced to the third subsystem. A new partial error, e 1, that penalizes the error in the yaw component, is also introduced. 1 1 V = z + e1 (.1) e d 1= ψ ψ (.14) he time derivative of the third Lyapunov function is defined as ( d V& ) = z z& + e1 ψ& ψ& (.15) where z& is the time derivative of the third partial error, given as z& = R& ure & ure & (.16) ref hen, by equations (.5) and (.16), V & becomes ( ˆ d V& = z R& ure & urω E ) + e ( ψ& ψ& ) (.17) ref 1 14

19 uω d V& = z ( R& ref R uω 1 ) + e1 ( ψ& ψ& ) u& (.18) At this point, consider the virtual control signal as uω u Ω1. Another 0 stabilization control signal and new partial errors are defined to rewrite equation (.18). First, the new reference signal associated with angular velocity in the fixed body frame will be defined as uω Ω ref = uω 1 = I EE ( R& ref + z+ kz) (.19) 0 ref Also, a fourth partial error variable is introduced as z =Ω urω ˆ E (.0) 4 ref By introducing equations (.19) and (.0), the time derivative of V becomes & = + + & & d (.1) V z z k ( ) z z z4 e1 ψ ψ Similar to previous steps, consider another reference signal and an error signal associated with the yaw velocity: ψ& & k e (.) d ref = ψ 4 1 e = ψ& ψ& (.) ref he time derivative of V can be rewritten again as V& z z k z z z e e k e (.4) =

20 By analyzing V &, it should be noted that equation (.4) is negative definite when the two new error signals, z 4 and e, are driven to zero. he two constants, k and k 4, must be positive for V & to be negative definite. Finally, a fourth Lyapunov function, V 4, is defined as 1 1 V4 = z4 + e (.5) his function, like the previous Lyapunov functions, is also positive definite. aking the time derivative of the fourth Lyapunov function, equation (.5) becomes V& 4 = z 4 z& 4+ e( && ψ && ψ ref ) (.6) he time derivative of the fourth error signal, z 4, is defined by z& =Ω& ( ure & ure Ω& ) (.7) 4 ref hen, equation (.6) is rewritten as V& = z ( Ω& ( ure & ureˆ Ω & )) + e (&& ψ && ψ ) (.8) 4 4 ref ref where Ω & is the control input and Ê is the skew symmetric matrix defined as the following x matrix: Eˆ = (.9). o simplify the Lyapunov equation, two more equations are introduced: && ψ = && ψ ref e k e (.0) 1 6 ure & ureˆ Ω=Ω & & + z + k z (.1) ref

21 Substituting equations (.0) and (.1) into equation (.9), the derivative of the fourth Lyapunov equation becomes & 4 = (.) V z z k z k e e e Once again, this particular derivative is also negative definite. he constants associated with equation (.), similar to the previous constants in the Lyapunov equations, must be positive. Due to the absence of Ω in the control input design, it will be obtained from the second derivative of η. he first derivative of η is given as & η= Ω (.) 1 W η 0 sinφ cosφ 1 & η= 0 cosθ cosφ cosθ sinφ cosθ Ω (.4) cosθ sinθ sinφ sinθ cosφ where the x matrix W η is defined by W η sinθ 0 1 = cosθ sinφ cosφ 0 cosθ cosφ sinφ 0 (.5) he second derivative of η is then given by && η= W W& W Ω+ W Ω& (.6) η η η η Obtaining the second derivative of the yaw, the equation yields 1 1 sinφ cosφ && ψ = E1 Wη W& ηwη Ω+ Ω & + Ω& (.7) cosθ cosθ Now the equations for the control laws can be obtained. u& = E R ( R& + z + k z ) (.8) ref 17

22 E R Ω & 1= ( Ω& ref u& Ω 1+ z+ k5z4) (.9) u E1 R Ω & = ( Ω& ref u& Ω + z+ k5z4) u (.40) cosθ 1 1 sinφ Ω & = (&& ψ ref e1 k6e+ E1 Wη W& ηwη Ω Ω& ) cosφ cosθ (.41) A final Lyapunov function is defined to be the sum of the first four functions. his Lyapunov function is positive definite. Its time derivative is negative definite. hen by Lyapunov s stability theorem [8], the control law is asymptotically stable for all initial conditions. V = V1 + V+ V + V4 (.4) & = (.4) V k z k z k z k e k z k e It can be directly verified that V & is negative definite for all constants, ki > 0, 1 i 6. 18

23 CHAPER 4: SIMULAION RESULS he following simulation results were obtained in Simulink, shown in Figures he controller, although derived from a simplified model of the helicopter, was implemented on the complete model. he initial and desired positions, with respect to the inertial frame I, are defined as ξ o ξ d = 0 5 = 5 10 (4.1) (4.) he initial and desired orientation of the helicopter with respect to the inertial frame is η o η d 0 = 0 0 o 50 = 0 0 o (4.) (4.4) While the pitch and roll orientation is included in equations (4.) and (4.4), it should be noted that the direction in which principle translational force, u, acts will determine those orientations. 19

24 Figure 4.1. he position trajectory for the helicopter along the E x axis of the inertial frame. Figure 4.. he position trajectory for the helicopter along the E y axis of the inertial frame. 0

25 Figure 4.. he position trajectory for the helicopter along the E z axis of the inertial frame. Figure 4.4. he yaw orientation trajectory for the helicopter with respect to the inertial frame. 1

26 Figure 4.5. he pitch orientation trajectory for the helicopter with respect to the inertial frame. Figure 4.6. he roll orientation trajectory for the helicopter with respect to the inertial frame.

27 he four control signals, ( u, Ω1, Ω, Ω ), are shown in Figures It should be noted that the values on the control signals are plausible. he control signal, u, is associated with translation forces. he control signals, Ω 1, Ω, and Ω, are associated with the torque applied to the fuselage. hus, the design technique has created a reasonable controller for a standard helicopter. Figure 4.7. he control signal, u, for the simulation. his control signal is associated with translation dynamics, and is in units of N.

28 Figure 4.8. he control signal, Ω 1, for the simulation. his control signal is associated with rotation dynamics, and is in units of N m. Figure 4.9. he control signal, Ω, for the simulation. his control signal is associated with rotation dynamics, and is in units of N m. 4

29 Figure he control signal, Ω, for the simulation. his control signal is associated with rotation dynamics, and is in units of N m. It should be noted that the controller produced from the integrator backstepping technique requires full-state feedback. In other words, the controller requires the states, ( ξ, & ξ, η, & η), to be measurable and available in order to derive the four control signals, ( u, Ω1, Ω, Ω ). While it is possible to measure the four states mentioned above, it is often extremely difficult to do so, due the fact that many expensive sensors are required in order to measure some of the states. When full-state feed back is not available, a dynamical observer, based on the control input and output, can be used to estimate the value of unavailable states. 5

30 CHAPER 5: CONCLUSION AND FUURE WORK In this thesis, a nonlinear controller is created for a standard helicopter through the technique of integrator backstepping. Before designing a controller, a model of the helicopter is established through Newton s law of motion. o verify that the controller does indeed work, simulations are done in MALAB. One particular set of results is then displayed. From Chapter 4, it can be seen that the position and orientation of the helicopter do indeed converge to the desired position and orientation. More specifically, the x, y, and z positions all converge to the desired values of [ ] ξ = in about 0 to 5 seconds while the yaw orientation, ψ, d converges to the desired value ψ = 50 o in about half the time. he roll and pitch d orientations, θ and φ respectively, experienced a small and unnoticeable disturbance, but stayed in their original orientation throughout the simulation. It should be noted that the system is stable due to the use of Lyapunov functions during the design process of the controller. As for continuation of this project, it will be great to actually implement the controller in a real-life UAV such as a remote control helicopter. Simulation in MALAB is a great way to verify the controller design and whether the system is stable or not. However, such simulation does not accurately encompass all the real-life variables, such as wind, that an UAV would encounter. 6

31 REFERENCES [1] H. Hellman, Helicopters and Other VOL s. Garden City, NY: Doubleday & Company, Inc., [] P. Castillo, R. Lozano, and A. E. Dzul, Modelling and Control of Mini- Flying Machines. London, England: Springer, 005. []. J. Koo, Y. Ma, and S. S. Sastry, Nonlinear control of a helicopter based unmanned aerial vehicle model, unpublished. [4] J. C. A. Vilchis, B. Brogliato, A. Dzul, and R. Lozano, Nonlinear modelling and control of helicopters, Automatica, vol. 9, pp , 00. [5] M. Spong, S. Hutchinson, and M. Vidyasagar, Robot Modeling and Control. Hoboken, NJ: John Wiley & Sons, Inc., 006. [6] K. B. Ngo, R. Mahony, and Z. P. Jiang, Integrator backstepping design for motion systems with velocity constraint, in 5 th Asian Control Conference, 004, vol. 1, pp [7] M. Krstic, I. Kanellakopoulos, and P. Kokotovic, Nonlinear and Adaptive Control Design. New York, NY: John Wiley & Sons, Inc., [8] H. K. Khalil, Nonlinear Systems. Upper Saddle River, NJ: Prentice-Hall, Inc., 00. 7

32 APPENDIX A: SIMULINK MODEL AND CODE he Simulink model of the helicopter is shown in Figure A.1. he controller, shown as the subsystem in Figure A.1, is shown in Figure A.. he Matlab code used in the embedded functions is given after the Simulink models. Figure A.1: he Simulink model used to obtain simulation results. 8

33 Figure A.: he Simulink model for the controller. 9

34 he following Matlab code was used in the embedded functions from the Simulink model. function omegadot = fcn(u, gamma, omega) % his function calculates the first time derivative of omega, % which is the angular velocity of helicopter, expressed in the % fixed body frame. I=[ ; ; ]; a=-omega; b=i*omega; c=cross(a,b); omegadot=(i^-1)*(c+gamma+u); function nudot = Wn_inv(omega, nu) % his function calculates the first time derivative of nu, which % is the angular velocity of the orientation of the helicopter % expressed in the inertial frame, I wnb=[0 sind(nu()) cosd(nu()); 0 cosd(nu())*cosd(nu()) - cosd(nu())*sind(nu()); cosd(nu()) sind(nu())*sind(nu()) sind(nu())*cosd(nu())]; wn=wnb/(cosd(nu())); nudot=wn*omega; function vdot = fcn(u, nu, gamma) % his function calculates the first time derivative of v, which is the % velocity of the helicopter, expressed in the inertial frame. r11=cosd(nu())*cosd(nu(1)); r1=sind(nu())*sind(nu())*cosd(nu(1))-cosd(nu())*sind(nu(1)); r1=cosd(nu())*sind(nu())*cosd(nu(1))-sind(nu())*sind(nu(1)); r1=cosd(nu())*sind(nu(1)); r=sind(nu())*sind(nu())*sind(nu(1))-cosd(nu())*cosd(nu(1)); r=cosd(nu())*sind(nu())*sind(nu(1))-sind(nu())*cosd(nu(1)); r1=-sind(nu()); r=sind(nu())*cosd(nu()); r=cosd(nu())*cosd(nu()); R=[r11 r1 r1; r1 r r; r1 r r]; 0

35 g=9.8; m=7.5; E=[0;0;1]; Ez=[0;0;1]; L=[1 0 0; 0 1 1; 0 0 0]; K=[ ; ; ]; sigma=l*(k^-1); vdot=(-u*r*e+m*g*ez+r*sigma*gamma)/m; function [R,Rt,Wn,Wni] = fcn(nu) % his block will calculate the R matrix based on the current angles % It will also find Wn and Wn^-1 r11=cosd(nu())*cosd(nu(1)); r1=sind(nu())*sind(nu())*cosd(nu(1))-cosd(nu())*sind(nu(1)); r1=cosd(nu())*sind(nu())*cosd(nu(1))-sind(nu())*sind(nu(1)); r1=cosd(nu())*sind(nu(1)); r=sind(nu())*sind(nu())*sind(nu(1))-cosd(nu())*cosd(nu(1)); r=cosd(nu())*sind(nu())*sind(nu(1))-sind(nu())*cosd(nu(1)); r1=-sind(nu()); r=sind(nu())*cosd(nu()); r=cosd(nu())*cosd(nu()); R1=[r11 r1 r1; r1 r r; r1 r r]; wnb=[0 sind(nu()) cosd(nu()); 0 cosd(nu())*cosd(nu()) - cosd(nu())*sind(nu()); cosd(nu()) sind(nu())*sind(nu()) sind(nu())*cosd(nu())]; wni=wnb/(cosd(nu())); R = R1; Rt= R1'; Wn=wni^-1; Wni=wni; function y = fcn(omega) % his block finds omega_hat, which is the skew symmetric % matrix of omega. 1

36 u=[0 -omega() omega(); omega() 0 omega(1); -omega() omega(1) 0]; y = u;