Quadrotor Dynamics and Control Rev 0.1
These notes outline quadrotor dynamics, state estimation, and control using a camera as the primary sensor.
Quadrotor Dynamics and Control Randal W. Beard Brigham Young University February 19, 2008 1 Reference Frames This section describes the various reference frames and coordinate systems that are used to describe the position of orientation of aircraft, and the transformation between these coordinate systems. It is necessary to use several different coordinate systems for the following reasons: Newton s equations of motion are given the coordinate frame attached to the quadrotor. Aerodynamics forces and torques are applied in the body frame. On-board sensors like accelerometers and rate gyros measure information with respect to the body frame. Alternatively, GPS measures position, ground speed, and course angle with respect to the inertial frame. Most mission requirements like loiter points and flight trajectories, are specified in the inertial frame. In addition, map information is also given in an inertial frame. One coordinate frame is transformed into another through two basic operations: rotations and translations. Section 1.1 develops describes rotation matrices and their use in transforming between coordinate frames. Section 1.2 describes the specific coordinate frames used for micro air vehicle systems. In Section 1.3 we derive the Coriolis formula which is the basis for transformations between between between translating and rotating frames. 1 1.1 Rotation Matrices We begin by considering the two coordinate systems shown in Figure 1. The Figure 1: Rotation in 2D vector p can be expressed in both the F0 frame (specified by (^i0;^j 0; ^k0)) and in the F1 frame (specified by (^i1;^j1; ^k1)). In the F0 frame we have p = p0 x ^i 0 + p0 y ^j 0 + p0 z ^k0: Alternatively in the F1 frame we have p = p1 x ^i 1 + p1 y ^j 1 + p1 z ^k1: Setting these two expressions equal to each other gives p1 x ^i 1 + p1 y ^j 1 + p1 z ^k1 = p0 x ^i 0 + p0 y ^j 0 + p0 z ^k0: Taking the dot product of both sides with^i1,^j1, and ^k1 respectively, and stacking the result into matrix form gives p1 4= 0 @ p1 x p1 y p1 z 1 A = 0 @ ^i 1 ^i0 ^i1 ^j0 ^i1 ^k0 ^j 1 ^i0 ^j 1 ^j 0 ^j 1 ^k0 ^k1 ^i0 ^k1 ^j0 ^k1 ^k0 1 A 0 @ p0 x p0 y p0 z 1 A: From the geometry of Figure 1 we get p1 = R1 0p0; (1) 2 where R1 0 4= 0 @ cos( ) sin( ) 0 sin( ) cos( ) 0 0 0 1 1 A: The notation R1 0 is used to denote a rotation matrix from coordinate frame F0 to coordinate frame F1. Proceeding in a similar way, a right-handed rotation of the coordinate system about the y-axis gives R1 0 4= 0 @ cos( ) 0 sin( ) 0 1 0 sin( ) 0 cos( ) 1 A; and a right-handed rotation of the coordinate system about the x-axis resultes in R1 0 4= 0 @ 1 0 0 0 cos( ) sin( ) 0 sin( ) cos( ) 1 A: As pointed out in [1], the negative sign on the sin term appears above the line with only ones and zeros. The matrix R1 0 in the above equations are examples of a more general class of rotation matrices that have the following properties: P.1. (Rb a) 1 = (Rb a)T = Ra b . P.2. Rc bRb a = Rc a. P.3. detRb a = 1. In the derivation of Equation (1) note that the vector p remains constant and the new coordinate frame F1 was obtained by rotating F0 through a rightedhanded rotation of angle . We will now derive a formula, called the rotation formula that performs a left-handed rotation of a vector p about another vector ^n by an angle of . Our derivation follows that given in [1]. Consider Figure 2 which is similar to Figure 1.2-2 in [1]. The vector p is rotated, in a left-handed sense, about a unit vector ^n by an angle of to produce the vector q. The angle between p and ^n is . By geometry we have that q = O N + N W +W Q: (2) 3 Figure 2: Left-handed rotation of a vector p about the unit vector ^n by an angle of to obtain the vector q. The vector O N can be found by taking the projection of p on the unit vector n^ in the direction of ^n: O N = (p n^) n^: The vector N W is in the direction of p O N with a length of NQcos . Noting that the length NQ equals the length NP which is equal to p O N we get that N W = p (p ^n) ^n kp (p ^n) ^nk NQcos = (p (p ^n) ^n) cos : The vectorW Q is perpendicular to both p and n^ and has length NQsin . Noting that NQ = kpk sin we get W Q = p ^n kpk sin NQsin = ^n p sin : Therefore Equation (2) becomes q = (1 cos ) (p ^n) ^n + cos p sin (^n p) ; (3) 4 Figure 3: Rotation of p about the z-axis. which is called the rotation formula. As an example of the application of Equation (3) consider a left handed rotation of a vector p0 in frame F0 about the z-axis as shown in Figure 3. Using the rotation formula we get q0 = (1 cos )(p ^n)^n + cos p sin ^n p = (1 cos )p0 z 0 @ 0 0 1 1 A + cos 0 @ p0 x p0 y p0 z 1 A sin 0 @ p0 y p0 x 0 1 A = 0 @ cos sin 0 sin cos 0 0 0 1 1 Ap0 = R1 0p0: Note that the rotation matrix R1 0 can be interpreted in two different ways. The first interpretation is that it transforms the fixed vector p from an expression in frame F0 to an expression in frame F1 where F1 has been obtained from F0 by a right-handed rotation. The second interpretation is that it rotates a vector p though a left-handed rotation to a new vector q in the same reference frame. Right-handed rotations of vectors are obtained by using (R1 0)T . 5 1.2 Quadrotor Coordinate Frames For quadrotors there are several coordinate systems that are of interest. In this section we will define and describe the following coordinate frames: the inertial frame, the vehicle frame, the vehicle-1 frame, the vehicle-2 frame, and the body frame. Throughout the book we assume a flat, non-rotating earth: a valid assumption for quadrotors. 1.2.1 The inertial frame Fi. The inertial coordinate system is an earth fixed coordinate system with origin at the defined home location. As shown in Figure 4, the unit vector ^ii is directed North,^ji is directed East, and ^ki is directed toward the center of the earth. Figure 4: The inertial coordinate frame. The x-axis points North, the y-axis points East, and the z-axis points into the Earth. 1.2.2 The vehicle frame Fv. The origin of the vehicle frame is at the center of mass of the quadrotor. However, the axes of Fv are aligned with the axis of the inertial frame Fi. In other words, the unit vector^iv points North, ^j v points East, and ^kv points toward the center of the earth, as shown in Figure 5. 1.2.3 The vehicle-1 frame Fv1. The origin of the vehicle-1 frame is identical to the vehicle frame, i.e, the the center of gravity. However, Fv1 is positively rotated about ^kv by the yaw angle so that if the airframe is not rolling or pitching, then^iv1 would point out the nose 6 Figure 5: The vehicle coordinate frame. The x-axis points North, the y-axis points East, and the z-axis points into the Earth. Figure 6: The vehicle-1 frame. If the roll and pitch angels are zero, then the x-axis points out the nose of the airframe, the y-axis points out the right wing, and the z-axis points into the Earth. of the airframe,^jv1 points out the right wing, and ^kv1 is aligned with ^kv and points into the earth. The vehicle-1 frame is shown in Figure 6. The transformation from Fv to Fv1 is given by pv1 = Rv1 v ( )pv; where Rv1 v ( ) = 0 @ cos sin 0 sin cos 0 0 0 1 1 A: 7 1.2.4 The vehicle-2 frame Fv2. The origin of the vehicle-2 frame is again the center of gravity and is obtained by rotating the vehicle-1 frame in a right-handed rotation about the ^j v1 axis by the pitch angle . If the roll angle is zero, then^iv2 points out the nose of the airframe, ^j v2 points out the right wing, and ^kv2 points out the belly, a shown in Figure 7. Figure 7: The vehicle-2 frame. If the roll angel is zero, then the x-axis points out the nose of the airframe, the y-axis points out the right wing, and the z-axis points out the belly. The transformation from Fv1 to Fv2 is given by pv2 = Rv2 v1( )pv1; where Rv2 v1( ) = 0 @ cos 0 sin 0 1 0 sin 0 cos 1 A: 1.2.5 The body frame Fb. The body frame is obtained by rotating the vehicle-2 frame in a right handed rotation about^iv2 by the roll angle . Therefore, the origin is the center-of-gravity, ^i b points out the nose of the airframe, ^j b points out the right wing, and ^kb points out the belly. The body frame is shown in Figure 8. The transformation from Fv2 to Fb is given by pb = Rb v2( )pv2; 8 Figure 8: The body frame. The x-axis points out the nose of the airframe, the y-axis points out the right wing, and the z-axis points out the belly. where Rb v2( ) = 0 @ 1 0 0 0 cos sin 0 sin cos 1 A: The transformation from the vehicle frame to the body frame is given by Rb v ( ; ; ) = Rb v2( )Rv2 v1( )Rv1 v ( ) = 0 @ 1 0 0 0 cos sin 0 sin cos 1 A 0 @ cos 0 sin 0 1 0 sin 0 cos 1 A 0 @ cos sin 0 sin cos 0 0 0 1 1 A = 0 @ c c c s s s s c c s s s s + c c s c c s c + s s c s s s c c c 1 A; where c 4= cos and s 4= sin . 1.3 Equation of Coriolis In this section we provide a simple derivation of the famous equation of Coriolis. We will again follow the derivation given in [1]. Suppose that we are given two coordinate frames Fi and Fb as shown in Figure 9. For example, Fi might represent the inertial frame and Fb might represent the body frame of a quadrotor. Suppose that the vector p is moving in Fb and that Fb is rotating and translating with respect to Fi. Our objective is to find the time derivative of p as seen from frame Fi. We will derive the appropriate equation through two steps. Assume first that Fb is not rotating with respect to Fi. Denoting the time derivative of p in frame 9 Figure 9: Derivation of the equation of Coriolis. Fi as d dti p we get d dti p = d dtb p: (4) On the other hand, assume that p is fixed in Fb but that Fb is rotating with respect to Fi, and let ^s be the instantaneous axis of rotation and the (right-handed) rotation angle. Then the rotation formula (3) gives p + p = (1 cos( ))^s(^s p) + cos( )p sin( )^s p: Using a small angle approximation and dividing both sides by t gives p t t ^s p: Taking the limit as t ! 0 and defining the angular velocity of Fb with respect to Fi as !b=i 4= ^s_ we get d dti p = !b=i p: (5) Since differentiation is a linear operator we can combine Equations (4) and (5) to obtain d dti p = d dtb p + !b=i p; (6) which is the equation of Coriolis. 10 2 Kinematics and Dynamics In this chapter we derive the expressions for the kinematics and the dynamics of a rigid body. While the expressions derived in this chapter are general to any rigid body, we will use notation and coordinate frames that are typical in the aeronautics literature. In particular, in Section 2.1 we define the notation that will be used for the state variables of a quadrotor. In Section 2.2 we derive the expressions for the kinematics, and in Section 2.3 we derive the dynamics. 2.1 Quadrotor State Variables The state variables of the quadrotor are the following twelve quantities pn = the inertial (north) position of the quadrotor along^ii in Fi, pe = the inertial (east) position of the quadrotor along^j i in Fi, h = the altitude of the aircraft measured along ^ki in Fi, u = the body frame velocity measured along^ib in Fb, v = the body frame velocity measured along^j b in Fb, w = the body frame velocity measured along ^kb in Fb, = the roll angle defined with respect to Fv2, = the pitch angle defined with respect to Fv1, = the yaw angle defined with respect to Fv, p = the roll rate measured along^ib in Fb, q = the pitch rate measured along^jb in Fb, r = the yaw rate measured along ^kb in Fb. The state variables are shown schematically in Figure 10. The position (pn; pe; h) of the quadrotor is given in the inertial frame, with positive h defined along the negative Z axis in the inertial frame. The velocity (u; v;w) and the angular velocity (p; q; r) of the quadrotor are given with respect to the body frame. The Euler angles (roll , pitch , and yaw ) are given with respect to the vehicle 2-frame, the vehicle 1-frame, and the vehicle frame respectively. 11 Figure 10: Definition of Axes 2.2 Quadrotor Kinematics The state variables pn, pe, and h are inertial frame quantities, whereas the velocities u, v, and w are body frame quantities. Therefore the relationship between position and velocities is given by d dt 0 @ pn pe h 1 A = Rv b 0 @ u v w 1 A = (Rb v )T 0 @ u v w 1 A = 0 @ c c s s c c s c s c + s s c s s s s + c c c s s s c s s c c c 1 A 0 @ u v w 1 A: The relationship between absolute angles , , and , and the angular rates p, q, and r is also complicated by the fact that these quantities are defined in different coordinate frames. The angular rates are defined in the body frame Fb, whereas the roll angle is defined in Fv2, the pitch angle is defined in Fv1, and the yaw angle is defined in the vehicle frame Fv. We need to relate p, q, and r to _ , _ , and _ . Since _ , _ , _ are small and noting that Rb v2(_ ) = Rv2 v1( _) = Rv1 v ( _ ) = I; 12 we get 0 @ p q r 1 A = Rb v 2(_ ) 0 @ _ 0 0 1 A + Rb v2( )Rv2 v1( _) 0 @ 0 _ 0 1 A + Rb v2( )Rv2 v1( )Rv!v1( _ ) 0 @ 0 0 _ 1 A = 0 @ _ 0 0 1 A + 0 @ 1 0 0 0 cos sin 0 sin cos 1 A 0 @ 0 _ 0 1 A + 0 @ 1 0 0 0 cos sin 0 sin cos 1 A 0 @ cos 0 sin 0 1 0 sin 0 cos 1 A 0 @ 0 0 _ 1 A = 0 @ 1 0 s 0 c s c 0 s c c 1 A 0 @ _ _ _ 1 A: (7) Inverting we get 0 @ _ _ _ 1 A = 0 @ 1 sin( ) tan( ) cos( ) tan( ) 0 cos( ) sin( ) 0 sin( ) sec( ) cos( ) sec( ) 1 A 0 @ p q r 1 A: (8) 2.3 Rigid Body Dynamics Let v be the velocity vector of the quadrotor. Newton s laws only hold in inertial frames, therefore Newton s law applied to the translational motion is m dv dti = f ; where m is the mass of the quadrotor, f is the total applied to the quadrotor, and d dti is the time derivative in the inertial frame. From the equation of Coriolis we have m dv dti = m dv dtb + !b=i v = f ; (9) where !b=i is the angular velocity of the airframe with respect to the inertial frame. Since the control force is computed and applied in the body coordinate system, and since ! is measured in body coordinates, we will express Eq (9) in body coordinates, where vb 4= (u; v;w)T , and !b b=i 4= (p; q; r)T . Therefore, in body coordinates, Eq. (9) becomes 0 @ u_ v_ w_ 1 A = 0 @ rv qw pw ru qu pv 1 A + 1 m 0 @ fx fy fz 1 A; (10) 13 where f b 4= (fx; fy; fz)T . For rotational motion, Newton s second law states that dhb dti = m; where h is the angular momentum andmis the applied torque. Using the equation of Coriolis we have dh dti = dh dtb + !b=i h = m: (11) Again, Eq. (11) is most easily resolved in body coordinates where hb = J!b b=i where J is the constant inertia matrix given by J = 0 @ R (y2 + z2)dm R xy dm R xz dm R xy dm R (x2 + z2)dm R yz dm R xz dm R yz dm R (x2 + y2)dm 1 A 4= 0 @ Jx Jxy Jxz Jxy Jy Jyz Jxz Jyz Jz 1 A: Figure 11: The moments of inertia for the quadrotor are calculated assuming a spherical dense center with mass M and radius R, and point masses of mass m located at a distance of from the center. As shown in Figure 11, the quadrotor is essentially symmetric about all three axes, therefore Jxy = Jxz = Jyz = 0 which implies that J = 0 @ Jx 0 0 0 Jy 0 0 0 Jz 1 A: 14 Therefore J 1 = 0 @ 1 Jx 0 0 0 1 Jy 0 0 0 1 Jz 1 A: The inertia for a solid sphere is given by J = 2MR2=5[2]. Therefore Jx = 2MR2 5 + 2 2m Jy = 2MR2 5 + 2 2m Jz = 2MR2 5 + 4 2m: Defining mb 4= ( ; ; )T we can write Eq. (11) in body coordinates as 0 @ p_ q_ r_ 1 A = 0 @ 1 Jx 0 0 0 1 Jy 0 0 0 1 Jz 1 A 2 4 0 @ 0 r q r 0 p q p 0 1 A 0 @ Jx 0 0 0 Jy 0 0 0 Jz 1 A 0 @ p q r 1 A + 0 @ 1 A 3 5 = 0 B@ Jy Jz Jx qr Jz Jx Jy pr Jx Jy Jz pq 1 CA + 0 @ 1 Jx 1 Jy 1 Jz 1 A: The six degree of freedom model for the quadrotor kinematics and dynamics can be summarized as follows: 0 @ p_n p_e _h 1 A = 0 @ c c s s c c s c s c + s s c s s s s + c c c s s s c s s c c c 1 A 0 @ u v w 1 A (12) 0 @ u_ v_ w_ 1 A = 0 @ rv qw pw ru qu pv 1 A + 1 m 0 @ fx fy fz 1 A; (13) 0 @ _ _ _ 1 A = 0 @ 1 sin tan cos tan 0 cos sin 0 sin cos cos cos 1 A 0 @ p q r 1 A (14) 0 @ p_ q_ r_ 1 A = 0 B@ Jy Jz Jx qr Jz Jx Jy pr Jx Jy Jz pq 1 CA + 0 @ 1 Jx 1 Jy 1 Jz 1 A: (15) 15 3 Forces and Moments The objective of this section is to describe the forces and torques that act on the quadrotor. Since there are no aerodynamic lifting surfaces, we will assume that the aerodynamic forces and moments are negligible. The forces and moments are primarily due to gravity and the four propellers. front right back left Figure 12: The top view of the quadrotor. Each motor produces an upward force F and a torque . The front and back motors spin clockwise and the right and left motors spin counterclockwise. Figure 13: Definition of the forces and torques acting on the quadrotor. Figure 12 shows a top view of the quadrotor systems. As shown in Figure 13, each motor produces a force F and a torque . The total force acting on the 16 quadrotor is given by F = Ff + Fr + Fb + Fl: The rolling torque is produced by the forces of the right and left motors as = (Fl Fr): Similarly, the pitching torque is produced by the forces of the front and back motors as = (Ff Fb): Due to Newton s third law, the drag of the propellers produces a yawing torque on the body of the quadrotor. The direction of the torque will be in the oppositive direction of the motion of the propeller. Therefore the total yawing torque is given by = r + l f b: The lift and drag produced by the propellers is proportional to the square of the angular velocity. We will assume that the angular velocity is directly proportional to the pulse width modulation commend sent to the motor. Therefore, the force and torque of each motor can be expressed as F = k1 = k2 ; where k1 and k2 are constants that need to be determined experimentally, is the motor command signal, and represents f, r, b, and l. Therefore, the forces and torques on the quadrotor can be written in matrix form as 0 BB@ F 1 CCA = 0 BB@ k1 k1 k1 k1 0 k1 0 k1 k1 0 k1 0 k2 k2 k2 k2 1 CCA 0 BB@ f r b l 1 CCA 4= M 0 BB@ f r b l 1 CCA : The control strategies derived in subsequent sections will specify forces and torques. The actual motors commands can be found as 0 BB@ f r b l 1 CCA =M 1 0 BB@ F 1 CCA : 17 Note that the pulse width modulation commands are required to be between zero and one. In addition to the force exerted by the motor, gravity also exerts a force on the quadrotor. In the vehicle frame Fv, the gravity force acting on the center of mass is given by f v g = 0 @ 0 0 mg 1 A: However, since v in Equation (13) is expressed in Fb, we must transform to the body frame to give f b g = Rb v 0 @ 0 0 mg 1 A = 0 @ mg sin mg cos sin mg cos cos 1 A: Therefore, equations (12) (15) become 0 @ p_n p_e _h 1 A = 0 @ c c s s c c s c s c + s s c s s s s + c c c s s s c s s c c c 1 A 0 @ u v w 1 A (16) 0 @ u_ v_ w_ 1 A = 0 @ rv qw pw ru qu pv 1 A + 0 @ g sin g cos sin g cos cos 1 A + 1 m 0 @ 0 0 F 1 A; (17) 0 @ _ _ _ 1 A = 0 @ 1 sin tan cos tan 0 cos sin 0 sin cos cos cos 1 A 0 @ p q r 1 A; (18) 0 @ p_ q_ r_ 1 A = 0 B@ Jy Jz Jx qr Jz Jx Jy pr Jx Jy Jz pq 1 CA + 0 @ 1 Jx 1 Jy 1 Jz 1 A: (19) 4 Simplified Models Equations (16) (19) are the equations of motion to be used in our six degreeof- freedom simulator. However, they are not appropriate for control design for 18 several reasons. The first reason is that they are too complicated to gain significant insight into the motion. The second reason is that the position and orientation are relative to the inertial world fixed frame, whereas camera measurements will measure position and orientation of the target with respect to the camera frame. 4.1 Model for estimation For the quadrotor, we are not able to estimate the inertial position or the heading angle . Rather, we will be interested in the relative position and heading of the quadrotor with respect to a ground target. The relative position of the quadrotor will be measured in the vehicle-1 frame, i.e., the vehicle frame after it has been rotated by the heading vector . The vehicle-1 frame is convenient since x, y, and z positions are still measured relative to a flat earth, but they are vehicle centered quantities as opposed to inertial quantities. Let px, py, and pz denote the relative position vector between the target and the vehicle resolved in the v1 frame. Therefore Eq (16) becomes 0 @ p_x p_y p_z 1 A = 0 @ c s s c s 0 c s s s c c c 1 A 0 @ u v w 1 A: (20) 4.2 Model for control design Assuming that and are small, Equation (18) can be simplified as 0 @ _ _ _ 1 A = 0 @ p q r 1 A: (21) Similarly, Equation (19) is simplified by assuming that the Coriolis terms qr, pr, and pq, are small to obtain 0 @ p_ q_ r_ 1 A = 0 @ 1 Jx 1 Jy 1 Jz 1 A: (22) Combining Eq. (21) and (22) we get 0 @ 1 A = 0 @ 1 Jx 1 Jy 1 Jz 1 A: (23) 19 Differentiating Eq. (16) and neglecting _Rv b gives 0 @ pn pe pd 1 A = 0 @ c c s s c c s c s c + s s c s s s s + c c c s s s c s s c c c 1 A 0 @ u_ v_ w_ 1 A: (24) Neglecting the Coriolis terms and plugging Eq. (17) into Eq. (24) and simplifying gives 0 @ pn pe pd 1 A = 0 @ 0 0 g 1 A + 0 @ c s c s s c s s + s c c c 1 A F m : (25) Therefore, the simplified inertial model is given by pn = ( cos sin cos sin sin ) F m (26) pe = ( cos sin sin + sin cos ) F m (27) pd = g (cos cos ) F m (28) = 1 Jx (29) = 1 Jy (30) = 1 Jz : (31) (32) The dynamics given in Equations (26) (31) are expressed in the inertial frame. This is necessary for the simulator. However, we will be controlling position, altitude, and heading using camera frame measurements of a target position. In this setting heading is irrelevant. Therefore, instead of expressing the translational dynamics in the inertial frame, we will express them in the vehicle-1 frame Fv1, which is equivalent to the inertial frame after rotating by the heading angle. Differentiating Eq. (20) and neglecting _Rv1 b gives 0 @ px py pz 1 A = 0 @ c s s c s 0 c s s s c c c 1 A 0 @ u_ v_ w_ 1 A: (33) 20 Neglecting the Coriolis terms and plugging Eq. (17) into Eq. (33) and simplifying gives 0 @ px py pz 1 A = 0 @ 0 0 g 1 A + 0 @ c s s c c 1 A F m : (34) Therefore, the simplified model in the vehicle-1 frame is given by px = cos sin F m (35) py = sin F m (36) pz = g cos cos F m (37) = 1 Jx (38) = 1 Jy (39) = 1 Jz : (40) (41) 5 Sensors 5.1 Rate Gyros A MEMS rate gyro contains a small vibrating lever. When the lever undergoes an angular rotation, Coriolis effects change the frequency of the vibration, thus detecting the rotation. A brief description of the physics of rate gyros can be found at RateSensorAppNote.pdf. The output of the rate gyro is given by ygyro = kgyro + gyro + gyro; where ygyro is in Volts, kgyro is a gain, is the angular rate in radians per second, gyro is a bias term, and gyro is zero mean white noise. The gain kgyro should be given on the spec sheet of the sensor. However, due to variations in manufacturing it is imprecisely known. The bias term gyro is strongly dependent on temperature and should be calibrated prior to each flight. 21 If three rate gyros are aligned along the x, y, and z axes of the quadrotor, then the rate gyros measure the angular body rates p, q, and r as follows: ygyro;x = kgyro;xp + gyro;x + gyro;x ygyro;y = kgyro;yq + gyro;y + gyro;y ygyro;z = kgyro;zr + gyro;z + gyro;z: MEMS gyros are analog devices that are sampled by the on-board processer. We will assume that the sample rate is given by Ts. The Kestrel autopilot samples the gyros at approximately 120 Hz. 5.2 Accelerometers A MEMS accelerometer contains a small plate attached to torsion levers. The plate rotates under acceleration and changes the capacitance between the plate and the surrounding walls [3]. The output of the accelerometers is given by yacc = kaccA + acc + acc; where yacc is in Volts, kacc is a gain, A is the acceleration in meters per second, acc is a bias term, and acc is zero mean white noise. The gain kacc should be given on the spec sheet of the sensor. However, due to variations in manufacturing it is imprecisely known. The bias term acc is strongly dependent on temperature and should be calibrated prior to each flight. Accelerometers measure the specific force in the body frame of the vehicle. A physically intuitive explanation is given in [1, p. 13-15]. Additional explanation is given in [4, p. 27]. Mathematically we have 0 @ ax ay az 1 A = 1 m (F Fgravity) = v_ + !b b=i v 1 m Fgravity: In component form we have ax = u_ + qw rv + g sin ay = v_ + ru pw g cos sin az = w_ + pv qu g cos cos : 22 Using Eq (17) we get ax = 0 ay = 0 az = F=m; where F is the total thrust produced by the four motors. It is important to note that for the quadrotor, the output of the accelerometers is independent of angle. This is in contrast to the unaccelerated flight for fixed wing vehicles where the accelerometers are used to measure the gravity vector and thereby extract roll and pitch angles. MEMS accelerometers are analog devices that are sampled by the on-board processer. We will assume that the sample rate is given by Ts. The Kestrel autopilot samples the accelerometers at approximately 120 Hz. 5.3 Camera The control objective is to hold the position of the quadrotor over a ground based target that is detected using the vision sensor. In this section we will briefly describe how to estimate px and py in the vehicle 1-frame. We will assume that the camera is mounted so that the optical axis of the camera is aligned with the body frame z-axis and so that the x-axis of the camera points out the right of the quadrotor and the y-axis of the camera points to the back of the quadrotor. The camera model is shown in Figure 14. The position of the target in the vehicle-1 frame is (px; py; pz). The pixel location of the target in the image is ( x; y). The geometry for py is shown in Figure 15. From the geometry shown in Figure 15, we can see that py = pz tan x My ; (42) where is the camera field-of-view, and My is the number of pixels along the camera y-axis. In Figure 15, both py and x are negative. Positive values are toward the right rotor. A similar equation can be derived for px as px = pz tan y My : (43) 23 Figure 14: Camera model for the quadrotor. 6 State Estimation The objective of this section is to describe techniques for estimating the state of the quadrotor from sensor measurements. We need to estimate the following states: px, py, pz, u, v, w, , , , p, q, r. The angular rates p, q, and r can be obtained by low pass filtering the rate gyros. The remain states require a Kalman filter. Both are discussed below. 6.1 Low Pass Filters The Laplace transforms representation of a simple low-pass filter is given by Y (s) = a s + a U(s); 24 Target Optical Axis Figure 15: The geometry introduced by the vision system. The height above ground is given by pz, the lateral position error is py, the roll angle is , the field-of-view of the camera is , the lateral pixel location of the target in the image is x, and the total number of pixels along the lateral axis of the camera is Mx. were u(t) is the input of the filter and y(t) is the output. Inverse Laplace transforming we get y_ = ay + au: (44) Using a zeroth order approximation of the derivative we get y(t + T) y(t) T = ay(t) + au(t); where T is the sample rate. Solving for y(t + T) we get y(t + T) = (1 aT)y(t) + aTu(t): For the zeroth order approximation to be valid we need aT 1. If we let = aT then we get the simple form y(t + T) = (1 )y(t) + u(t): 25 Note that this equation has a nice physical interpretation: the new value of y (filtered value) is a weighted average of the old value of y and u (unfiltered value). If u is noisy, then 2 [0; 1] should be set to a small value. However, if u is relatively noise free, then should be close to unity. In the derivation of the discrete-time implementation of the low-pass filter, it is possible to be more precise. In particular, returning to Equation (44), from linear systems theory, it is well known that the solution is given by y(t + T) = e aT y(t) + a Z T 0 e a(T )u( ) d : Assuming that u(t) is constant between sample periods results in the expression y(t + T) = e aT y(t) + a Z T 0 e a(T ) d u(t)y(t + T) = e aT y(t) + (1 e aT )u(t): (45) Note that since ex = 1+x+ x2 2! +: : : we have that e aT 1 aT and 1 e aT aT . Therefore, if the sample rate is not fixed (common for micro controllers), and it is desired to have a fixed cut-off frequency, then Equation (45) is the preferable way to implement a low-pass filter in digital hardware. We will use the notation LPF( ) to represent the low-pass filter operator. Therefore ^x = LPF(x) is the low-pass filtered version of x. 6.2 Angular Rates p, q, and r. The angular rates p, q, and r can be estimated by low-pass filtering the rate gyro signals: ^p = LPF(ygyro,x) (46) ^q = LPF(ygyro,y) (47) ^r = LPF(ygyro,z): (48) 6.3 Dynamic Observer Theory The objective of this section is to briefly review observer theory. 26 Suppose that we have a linear time-invariant system modeled by the equations x_ = Ax + Bu y = Cx: A continuous-time observer for this system is given by the equation _^ x = |Ax^ {+zBu} + |L(y { zCx^}); (49) copy of the model correction due to sensor reading where ^x is the estimated value of x. Letting x = x ^x we observer that _ x = (A LC) x which implies that the observation error decays exponentially to zero if L is chosen such that the eigenvalues of A LC are in the open left half of the complex plane. In practice, the sensors are usually sampled and processed in digital hardware at some sample rate Ts. How do we modify the observer equation shown in Equation (49) to account for sampled sensor readings? The typical approach is to propagate the system model between samples using the equation _^ x = A^x + Bu (50) and then to update the estimate when a measurement is received using the equation ^x+ = ^x + L(y(tk) C^x ); where tk is the instant in time that the measurement is received and ^x is the state estimate produced by Equation (50) at time tk. Equation (50) is then reinstantiated with initial conditions given by ^x+. The continuous-discrete observer is summarized in Table 6.3. The observation process is shown graphically in Figure 16. Note that it is not necessary to have a fixed sample rate. The continuous-discrete observer can be implemented using Algorithm 1. Note that we did not use the fact that the process was linear. Suppose instead that we have a nonlinear system of the form x_ = f(x; u) (51) y = c(x) (52) , then the continuous discrete observer is given in table 6.3. The real question is how to pick the observer gain L. 27 Figure 16: sdf Algorithm 1 Continuous-Discrete Observer 1: Initialize: ^x = 0. 2: Pick an output sample rate Tout which is much less than the sample rates of the sensors. 3: At each sample time Tout: 4: for i = 1 to N do fPrediction: Propagate the state equation.g 5: ^x = ^x + Tout N (A^x + Bu) 6: end for 7: if A measurement has been received from sensor i then fCorrection: Measurement Updateg 8: ^x = ^x + Li (yi Ci^x) 9: end if 28 System model: x_ = Ax + Bu y(tk) = Cx(tk) Initial Condition x(0). Assumptions: Knowledge of A, B, C, u(t). No measurement noise. Prediction: In between measurements (t 2 [tk 1; tk)): Propagate _^x = A^x + Bu. Initial condition is ^x+(tk 1): Label the estimate at time tk as ^x (tk). Correction: At sensor measurement (t = tk): ^x+(tk) = ^x (tk) + L (y(tk) C^x (tk)) : Table 1: Continuous-Discrete Linear Observer. 6.4 Essentials from Probability Theory Let X = (x1; : : : ; xn)T be a random vector whose elements are random variables. The mean, or expected value of X is denoted by = 0 B@ 1 ... n 1 CA = 0 B@ Efx1g ... Efxng 1 CA = EfXg; where Efxig = Z fi( ) d ; and f( ) is the probability density function for xi. Given any pair of components xi and xj of X, we denote their covariance as cov(xi; xj) = ij = Ef(xi i)(xj j)g: The covariance of any component with itself is the variance, i.e., var(xi) = cov(xi; xi) = ii = Ef(xi i)( i)g: The standard deviation of xi is the square root of the variance: stdev(xi) = i = p ii: 29 System model: x_ = f(x; u) y(tk) = c(x(tk)) Initial Condition x(0). Assumptions: Knowledge of f, c, u(t). No measurement noise. Prediction: In between measurements (t 2 [tk 1; tk)): Propagate _^x = f(^x; u). Initial condition is ^x+(tk 1): Label the estimate at time tk as ^x (tk). Correction: At sensor measurement (t = tk): ^x+(tk) = ^x (tk) + L (y(tk) c(^x (tk))) : Table 2: Continuous-Discrete Nonlinear Observer. The covariances associated with a random vector X can be grouped into a matrix known as the covariance matrix: = 0 BBB@ 11 12 1n 21 22 2n ... . . . ... n1 n2 nn 1 CCCA = Ef(X )(X )T g = EfXXT g T : Note that = T so that is both symmetric and positive semi-definite, which implies that its eigenvalues are real and nonnegative. The probability density function for a Gaussian random variable is given by fx(x) = 1 p2 x e (x x)2 2 x ; where x is the mean of x and x is the standard deviation. The vector equivalent is given by fX(X) = 1 p2 det exp 1 2 (X )T 1(X ) ; in which case we write X N ( ; ) ; 30 Figure 17: Level curves for the pdf of a 2D Gaussian random variable. and say that X is normally distributed with mean and covariance . Figure 17 shows the level curves for a 2D Gaussian random variable with different covariance matrices. 6.5 Derivation of the Kalman Filter In this section we assume the following state model: x_ = Ax + Bu + G yk = Cxk + k; where yk = y(tk) is the kth sample of y, xk = x(tk) is the kth sample of x, k is the measurement noise at time tk, is a zero-mean Gaussian random process with covariance Q, and k is a zero-mean Gaussian random variable with covariance R. Note that the sample rate does not need to be be fixed. The observer will therefore have the following form: 31 Prediction: In between measurements: (t 2 [tk 1; tk]) Propagate _^x = A^x + Bu. Initial condition is ^x+(tk 1): Label the estimate at time tk as ^x (tk). Correction: At sensor measurement (t = tk): ^x+(tk) = ^x (tk) + L (y(tk) C^x (tk)) : Our objective is to pick L to minimize tr(P(t)). 6.5.1 Between Measurements. Differentiating x we get _ x = x_ _^x = Ax + Bu + G A^x Bu = A x + G : Therefore we have that x(t) = eAt x0 + Z t 0 eA(t )G ( ) d : We can therefore compute the evolution for P as _P = d dt Ef x xT g = Ef _ x xT + x_ xT g = E A x xT + G xT + x xTAT + x TGT = AP + PAT + GEf xT gT + Ef x T gGT : As in the previous section we get Ef xT g = E (t) x0eAT t + Z t 0 (t) T ( )GT eAT (t ) d = 1 2 QGT ; which implies that _P = AP + PAT + GQGT : 32 6.5.2 At Measurements. At a measurement we have that x+ = x ^x+ = x ^x L Cx + C^x = x LC x L : Therefore P+ = Ef x+ x+T g = E n x LC x L x LC x L T o = E x x T x x TCTLT x TLT LC x x T + LC x x TCTLT + LC x TLT = L x T + L x TCTLT + L TLT = P P CTLT LCP + LCP CTLT + LRLT : (53) Our objective is to pick L to minimize tr(P+). A necessary condition is @ @L tr(P+) = P CT P CT + 2LCP CT + 2LR = 0 =) 2L(R + CP CT ) = 2P CT =) L = P CT (R + CP CT ) 1: Plugging back into Equation (53) give P+ = P + P CT (R + CP CT ) 1CP P CT (R + CP CT ) 1CP + P CT (R + CP CT ) 1(CP CT + R)(R + CP CT ) 1CP = P P CT (R + CP CT ) 1CP = (I P CT (R + CP CT ) 1C)P = (I LC)P : For linear systems, the continuous-discrete Kalman filter is summarized in Table 6.5.2 If the system is nonlinear, then the Kalman filter can still be applied but we need to linearize the nonlinear equations in order to compute the error covariance matrix P and the Kalman gain L. The extended Kalman filter (EKF) is given in Table 6.5.2, and an algorithm to implement the EKF is given in Algorithm 2. 33 System model: x_ = Ax + Bu + yi(tk) = Cix(tk) + k Initial Condition x(0). Assumptions: Knowledge of A, B, Ci, u(t). Process noise satisfies N(0;Q). Measurement noise satisfies k N(0;R). Prediction: In between measurements (t 2 [tk 1; tk)): Propagate _^x = A^x + Bu. Propagate _P = AP + PAT + Q. Correction: At the ith sensor measurement (t = tk): Li = P CT i (Ri + CiPCT i ) 1; P+ = (I LiCi)P , ^x+(tk) = ^x (tk) + L (y(tk) C^x (tk)) : Table 3: Continuous-Discrete Kalman Filter. 6.6 Application to the quadrotor In this section we will discuss the application of Algorithm 2 to the quadrotor. We would like to estimate the state ^x = 0 BBBBBBBBBBBBB@ ^px ^py ^pz _ ^p x _ ^p y _ ^p z ^ ^ ^ 1 CCCCCCCCCCCCCA ; where the rate gyros and accelerometers will be used to drive the prediction step, and an ultrasonic altimeter and camera will be used in the correction step. 34 System model: x_ = f(x; u) + yi(tk) = ci(x(tk)) + k Initial Condition x(0). Assumptions: Knowledge of f, ci, u(t). Process noise satisfies N(0;Q). Measurement noise satisfies k N(0;R). Prediction: In between measurements (t 2 [tk 1; tk)): Propagate _^x = f(^x; u), Compute A = @f @x x=^x(t), Propagate _P = AP + PAT + Q. Correction: At the ith sensor measurement (t = tk): Ci = @ci @x x=^x Li = P CT i (Ri + CiPCT i ) 1; P+ = (I LiCi)P , ^x+(tk) = ^x (tk) + L (y(tk) ci(^x (tk))) : Table 4: Continuous-Discrete Extended Kalman Filter. The propagation model is obtained from Equations (35) (37), and (18) as f(x; u) = 0 BBBBBBBBBBBB@ p_x p_y p_z cos sin az sin az g + cos cos az p + q sin tan + r cos tan q cos r sin q sin cos + r cos cos 1 CCCCCCCCCCCC A ; where we have used the fact that the z-axis of the accelerometer measures az = 35 Algorithm 2 Continuous-Discrete Extended Kalman Filter 1: Initialize: ^x = 0. 2: Pick an output sample rate Tout which is much less than the sample rates of the sensors. 3: At each sample time Tout: 4: for i = 1 to N do fPrediction: Propagate the equations.g 5: ^x = ^x + Tout N (f(^x; u)) 6: A = @f @x 7: P = P + Tout N AP + PAT + GQGT 8: end for 9: if A measurement has been received from sensor i then fCorrection: Measurement Updateg 10: Ci = @ci @x 11: Li = PCT i (Ri + CiPCT i ) 1 12: P = (I LiCi)P 13: ^x = ^x + Li (yi ci(^x)). 14: end if F=m. Differentiating we obtain A = 0 BBBBBBBBBBBB@ 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 s s az c c az 0 0 0 0 0 0 0 c az 0 0 0 0 0 0 0 0 s c az c s az 0 0 0 0 0 0 0 qc t rs t (qs + rc )=c2 0 0 0 0 0 0 0 qs rc 0 0 0 0 0 0 0 0 qc rs c (qs + rc ) t c 0 1 CCCCCCCCCCCCA Note that it may be adequate (not sure) to use a small angle approximation in 36 the model resulting in f(x; u) = 0 BBBBBBBBBBBB@ p_x p_y p_z az az g + az p q r 1 CCCCCCCCCCCCA ; and A = 0 BBBBBBBBBBBB@ 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 az 0 0 0 0 0 0 0 az 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 CCCCCCCCCCCCA : If this form works, then the update equation for P can be coded by hand, significantly reducing the computational burden. Note also that f(x; u) does not take into account the motion of the target. A feedforward term can be added to f to account for the target motion as f(x; u) = 0 BBBBBBBBBBBB@ p_x m_ x p_y m_ y p_z az mx az my g + az p q r 1 CCCCCCCCCCCCA ; where (m_ x;m_ y) is the velocity of the target in the vehicle-1 frame, and (m x;m y) is the acceleration of the target in the vehicle-1 frame. 37 Let the relative position between the quadrotor and the target be denoted by pv1 = (px; py; pz)T . We can tranform to the camera frame as pc = Rc g Rg bRb v1pv1; where Rc g = 0 @ 0 1 0 0 0 1 1 0 0 1 A is the rotation matrix from gimbal coordinates to camera coordinates, Rg b = 0 @ 0 0 1 0 1 0 1 0 0 1 A transforms body coordinates to gimbal coordinates, and Rb v1 = 0 @ c c s s c c s c s c + s s c s s s s + c c c s s s c s s c c c 1 A transforms vehicle-1 coordinates to body coordinates. Using the small angle approximation for and we get pc = 0 @ px + py + pz px + pz px py + pz 1 A: The model for the pixel coordinates are therefore computed as x = c x(x) 4= f px + py + pz px py + pz 2 = c y (x) 4= f px + pz px py + pz : We therefore have the following sensors available to correct the state estimate: sensor 1: altimeter y1 = calt(x) 4= pz + 1[k] (54) sensor 2: vision x-pixel y2 = c x(x) + 2[k]: (55) sensor 3: vision y-pixel y3 = c y (x) + 3[k] (56) sensor 4: vision heading y4 = c (x) 4= =2 + + 4[k]: (57) 38 The linearization of the first and fourth output functions are given by C1 = 0 0 1 0 0 0 0 0 (58) C4 = 0 0 0 0 0 0 0 1 : (59) The linearization of the expression for the pixel coordinates is messy and can easily be computed numerically using the approximation @f(x1; ; xi; . . . ; xn) @xi f(x1; ; xi + ; ; xn) f(x1; ; xi; ; xn) ; where is a small constant. 7 Control Design The control design will be derived directly from Equations (35) (40). Equations (38) (40) are already linear. To simplify Equations (35) (37) define ux 4= cos sin F m (60) uy 4= sin F m (61) uz 4= g cos cos F m ; (62) to obtain px = ux (63) py = uy (64) pz = uz: (65) The control design proceeds by developing PID control strategies for ux, uy, and uz. After ux, uy, and uz have been computed, we can compute the desired force F, and the commanded roll angle c and the commanded pitch angle c from Equations (60) (62) as follows. From Equation (62) solve for F=m as F m = g uz cos cos : (66) 39 Substituting (66) into (61) gives uy = g uz cos tan : Solving for and letting this be the commanded roll angle gives c = tan 1 uy cos g uz : (67) Similarly, we can solve for the commanded pitch angle as c = tan 1 ux uz g : (68) 7.1 Vision Based Altitude Hold For the altitude hold we need to develop an expression for uz to drive pz to a desired altitude based on the size of the object in the image. We will assume that the camera returns the size of the object in the image plane in pixels, which is denoted by s. Figure 18 shows the geometry of the quadrotor hovering over a Target Figure 18: The size of the target is S in meters, and the size of the target in the image plane is denoted by s in pixels. The focal length is f, and the height above the target is pz. target of size S. From similar triangles we have that pz S = f s : 40 Solving for pz an differentiating we obtain p_z = fS _ s 2 s : (69) Differentiating again gives pz = fS s 2 s 2fS _ 2 s 3 s : Substituting uz = pz and solving for s gives s = 2 s fS uz + 2 _ 2 s s : Defining us 4= 2 s fS uz + 2 _ 2 s s ; (70) we get s = us: We can now derive a PID controller to drive s ! d s as us = kps( d s s) kds _ s + kis Z t 0 ( d s s) d : Solving (70) for uz we get uz = fS 2 s kps( d s s) kds fS 2 s + 2 fS _ s 3 s _ s + kis fS 2 s Z t 0 ( d s s) d : (71) The downside to this equation is that it requires knowledge of the target size S and the focal length f. This requirement can be removed by incorporating fS into the PID gains by defining ^kps 4=f S k p s ^kis 4= fSkis ^kds 4= fSkds ; and by noting from Equation (69) that fS _ s 2 s = w. Therefore Equation (71) becomes uz = ^kps ( d s s) 2 s ^kds 2 s + 2 w s ! _ s + ^kis 2 s Z t 0 ( d s s) d : (72) 41 7.2 Roll Attitude Hold The equation of motion for roll is given by Eq. (38) as = p=Jx. We will use a PID controller to regulate the roll attitude as p = kp ( c ) kd p + ki Z t 0 ( c ) d : A block diagram of the control structure is shown in Figure 19. Figure 19: A block diagram of the roll attitude hold loop. The gains kp , ki , and kd are selected one at a time using a method called successive loop closure. To pick kp note that if the input command c is a step of value A, then at time t = 0, before the integrator has time to begin winding up, x is given by x(0) = kp A: Therefore, setting kp = M=A will just saturate the actuators when a step value of A is placed on c. To select kd , let kp be fixed and let ki = 0, and solve for the characteristic equation in Evan s form verses kd to obtain 1 + kd 1 Jx s s2 + kp Jx = 0: The derivative gain kd is selected to achieve a damping ratio of = 0:9. The characteristic equation including ki in Evan s form verses ki is given by 1 + ki 1 Jx s3 + kd Jx s2 + kp Jx s = 0: 42 The integral gain ki can be selected so that damping ratio is not significantly changed. 7.3 Pitch Attitude Hold The equation of motion for pitch is given by Eq. (39) as = q=Jy. Similar to the roll attitude hold, we will use a PID controller to regulate pitch as q = kp ( c ) kd q + ki Z t 0 ( c ) d : 7.4 Vision Based Position Tracking From Equation 64 the lateral dynamics are given by py = uy, where py is the relative lateral position which we drive to zero using the PID strategy uy = kpypy kdyv + kiy Z t 0 py d : The relative position error py is given by Equation (42). From Equation 63 the longitudinal dynamics are given by px = ux, where px is the relative lateral position which we drive to zero using the PID strategy ux = kpxpx kdxu + kix Z t 0 px d : The relative position error px is given by Equation (43). 7.5 Relative Heading Hold The heading dynamics are given in Equation (40) as = r=Jz. Define d as the inertial heading of the target, and define 4= d as the relative heading. The camera directly measures . Assuming that d is constant we get = r=Jz. We regulate the relative heading with the PID strategy r = kp kd r ki Z t 0 d : 43 7.6 Feedforward When the quadrotor is tracking a ground robot, the motion of the robot will cause tracking errors due to the delayed response of the PID controller. If we can communicate with the robot, and we know its intended motion, we should be able to use that information to help the quadrotor predict where the robot is moving. To motivate our approach, lets consider a simple example to gain intuition. Consider the double integrator system y = u; where y is position, and u is commanded acceleration. If r is the reference signal then the standard PD loop is shown in Figure 20. Figure 20: Standard PD loop Let e = r y, then e = r y = r u = r kpe + kdy_ = r kpe kde_ + kdr_: In other words we have e + kde_ + kpe = r + kdr_: Therefore, the signal r + kdr_ drives the error transfer function 1 s2+kds+kp and if w is not zero, the tracking error will not go to zero. 44 From the expression e = r u we see that if instead of u = kpe kdy_, we instead use u = r + kpe + kde_; then we get e + kde_ + kpe = 0; which ensures that e(t) ! 0 independent of r(t). The associated block diagram is shown in Figure 21. Figure 21: Feedforward term is added to a standard PD loop. Now consider the lateral motion of the quadrotor in the cage where y = g tan ; where y is the position of the quadrotor, g is the gravity constant, and is the roll angle. Let m(t) be the position of the target and define y 4= m r as the relative position. Then y = m r = m g tan : If by some means we know the target velocity m_ and the target acceleration m , then we should pick such that g tan = m + kpy + kdy_; or = tan 1 m + kpy + kdy_ g : 45 Given the decoupling terms described at the beginning of this section, the motion model for the quadrotor in the vehicle-1 frame is given by px = ux py = uy = u : Let the position of the target in the vehicle-1 frame be given by (mx;my) and let it orientation be given by t, and define px = mx px py = my py = t : Differentiating twice we get p x = m x u x p y = m y u y = t u : Therefore, adding a feedforward term we have ux = mx + PIDx uy = my + PIDy u = t + PID where PID are the PID controllers derived earlier in this chapter. The motion model for the target is given by m_ n = Vt cos t m_ e = Vt sin t _V t = u1 _ t = u2; where (mn;me) is the inertial position of the target, Vt is the ground speed of the target, and u1 and u2 are robot control signals. In the vehicle-1 frame we have m_ x m_ y = cos sin sin cos Vt cos t Vt sin t = Vt cos Vt sin ; 46 where the camera measures the relative heading = t. Differentiating we obtain mx my = cos sin sin cos u1 u2 _ : To implement feedforward on the quadrotors, we must communicate with the robot to obtain Vt, u1, and u2. References [1] B. L. Stevens and F. L. Lewis, Aircraft Control and Simulation. Hoboken, New Jersey: John Wiley & Sons, Inc., 2nd ed., 2003. [2] D. Halliday and R. Resnick, Fundamentals of Physics. John Wiley & Sons, 3rd ed., 1988. [3] http://www.silicondesigns.com/tech.html. [4] M. Rauw, FDC 1.2 - A SIMULINK Toolbox for Flight Dynamics and Control Analysis, February 1998. Available at http://www.mathworks.com/. 47