Patent Publication Number: US-11029709-B1

Title: Adaptive wind estimation, trajectory generation, and flight control for aerial systems using motion data

Description:
CROSS-REFERENCE TO RELATED APPLICATION 
     This application claims the benefit of U.S. Provisional Patent Application No. 62/611,319 filed Dec. 28, 2017. The subject matter of this earlier filed application is hereby incorporated by reference in its entirety. 
    
    
     ORIGIN OF THE INVENTION 
     The invention described herein was made by employees of the United States Government and may be manufactured and used by or for the Government of the United States of America for governmental purposes without the payment of any royalties thereon or therefore. 
    
    
     FIELD 
     The present invention generally pertains to aerial system control, and more particularly, to adaptive wind estimation, trajectory generation, and flight control for aerial systems using motion data. 
     BACKGROUND 
     Small unmanned aerial systems (sUASs) are becoming increasingly popular for research, commercial use, and military use. This is in part because of their relative affordability as compared to other aircraft due to their small size, low cost, and less complex hardware. One of the critical aspects of sUAS usage is reliability while maintaining relative simplicity in design. 
     However, designs with lightweight structure and limited power are vulnerable to wind disturbances. Hence, it is conventionally difficult to provide accurate navigation and control outdoors, especially in urban environments where the wind field tends to be more complex and have more uncertainties. Most existing approaches are related to adaptive and robust control strategies to follow desired trajectories by considering the wind as an external disturbance. These approaches assume that the desired trajectories have already been provided. Few approaches consider trajectory generation problems in the wind, assuming that wind field is known and is steady and uniform. 
     Limitations of conventional approaches can be grouped into two categories: (1) assumptions on the available hardware/sensors; and (2) assumptions on the controller capabilities. It is assumed that the dynamic pressure or the three-axis velocity relative to the surrounding air can be measured onboard in real-time. It is also assumed that the accurate measurements of the linear and angular accelerations are available through IMU units. The available adaptive and robust controllers, which are designed to directly compensate for the wind effect in the tracking of desired trajectories, are not able to handle the limits on the required thrust. 
     Accordingly, an improved navigation approach for sUASs and other aircraft that facilitates safe and effective navigation may be beneficial. 
     SUMMARY 
     Certain embodiments of the present invention may provide solutions to the problems and needs in the art that have not yet been fully identified, appreciated, or solved by conventional aerial system control technologies. For example, some embodiments of the present invention pertain to adaptive wind estimation, trajectory generation, and flight control for aerial systems using motion data. Some embodiments may provide real-time estimation of atmospheric drag and wind components using data from onboard sensors including, but not limited to, an inertial measurement unit (IMU), rate gyros, etc. The estimation approach may be implemented using onboard computing power, may rapidly converge to true values, may be relatively computationally inexpensive, and/or may not require any specific hardware or specific vehicle maneuvers for the convergence in some embodiments. 
     In an embodiment, a computer program is embodied on a non-transitory computer-readable medium. The program configured to cause at least one processor to perform adaptive wind estimation by identifying aerodynamic drag coefficients in still-air flight of an aircraft and estimating wind components in moving and variable air based on the identified aerodynamic drag coefficients. The program is also configured to cause the at least one processor to generate a feasible trajectory that takes the estimated wind components into account and generate motor and thrust commands based on the generated feasible trajectory, thereby compensating for wind disturbances. The computer program is further configured to cause the at least one processor to control the aircraft based on the generated motor and thrust commands. 
     In another embodiment, A computer-implemented method includes performing adaptive wind estimation, by a computing system, by identifying aerodynamic drag coefficients in still-air flight of an aircraft and estimating wind components in moving and variable air based on the identified aerodynamic drag coefficients. The estimated wind components include wind velocities and accelerations. The computer-implemented method also includes generating a feasible trajectory, by the computing system, that takes the estimated wind components into account. The computer-implemented method further includes generating motor and thrust commands, by the computing system, based on the generated feasible trajectory by determining center of gravity (CG) control of the aircraft and determining attitude control of the aircraft, thereby compensating for wind disturbances. Additionally, the computer-implemented method includes controlling the aircraft based on the generated motor and thrust commands, by the computing system. No information from one or more wind sensors is available to the computing system. 
     In yet another embodiment, a computer-implemented method includes performing adaptive wind estimation for an aircraft, by a computing system, and generating a feasible trajectory, by the computing system, that takes the estimated wind components into account. The computer-implemented method also includes generating motor and thrust commands, by the computing system, based on the generated feasible trajectory and controlling the aircraft based on the generated motor and thrust commands, by the computing system. 
    
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
       In order that the advantages of certain embodiments of the invention will be readily understood, a more particular description of the invention briefly described above will be rendered by reference to specific embodiments that are illustrated in the appended drawings. While it should be understood that these drawings depict only typical embodiments of the invention and are not therefore to be considered to be limiting of its scope, the invention will be described and explained with additional specificity and detail through the use of the accompanying drawings, in which: 
         FIG. 1  is a perspective view illustrating a sUAS, according to an embodiment of the present invention. 
         FIG. 2A  is a flowchart illustrating a process for performing adaptive wind estimation and flight control for aerial systems using motion data, according to an embodiment of the present invention. 
         FIG. 2B  is a flowchart illustrating a process for performing adaptive estimation, according to an embodiment of the present invention. 
         FIG. 2C  is a flowchart illustrating a process for generating a trajectory based on the adaptive estimation of  FIG. 2B , according to an embodiment of the present invention. 
         FIG. 2D  is a flowchart illustrating a process for controlling an aircraft to track to the trajectory generated in  FIG. 2C , according to an embodiment of the present invention. 
         FIG. 3  is a graph illustrating a generated 3D trajectory and corresponding waypoints, according to an embodiment of the present invention. 
         FIG. 4  is a block diagram illustrating a computing system configured to perform adaptive wind estimation, trajectory generation, and flight control for aerial systems using motion data, according to an embodiment of the present invention. 
     
    
    
     DETAILED DESCRIPTION OF THE EMBODIMENTS 
     Some embodiments of the present invention pertain to adaptive wind estimation, trajectory generation, and flight control for aerial systems using motion data. The estimation approach of some embodiments is implemented using onboard computing power, rapidly converges to true values, is computationally inexpensive, and does not require any specific hardware or specific vehicle maneuvers for the convergence. Some embodiments are particularly effective for sUASs, which are more affected by wind disturbances in complex wind fields, such as those of urban environments. Furthermore, rotorcraft create large amounts of local airflow, which makes wind measurements more difficult or even impossible with the typically less power hardware of sUASs as compared to military drones, for example. There is no prior knowledge of the wind field in some embodiments, using the motion of the aircraft itself rather than wind sensors, which requires less computing power, complexity, and cost than conventional approaches. It should be noted that the approaches of some embodiments may be applied to rotorcraft, fixed wing aircraft, manned aircraft, unmanned aircraft, small aircraft, large aircraft, or any combination thereof without deviating from the scope of the invention. 
     Certain embodiments have three components: (1) an identification/estimation block; (2) a navigation block; and (3) a control block. The identification/estimation block identifies aerodynamic drag coefficients in still air (e.g., indoor) flight and estimates the wind components in moving and variable air (e.g., outdoor) flight. The navigation block generates feasible trajectories taking into account the estimated wind field. The control block generates motor/engine thrust (e.g., spin rate for rotorcraft) commands necessary to track the generated trajectories while compensating for the wind disturbance. These blocks may not use information from wind sensors in some embodiments. Indeed, wind sensors may not be present in some aircraft. 
     Such embodiments enable safe navigation in windy environments while continuously estimating the wind field along the trajectory, which may be broadcast for air traffic management in some embodiments. It should be noted that while some embodiments are applicable to sUASs and other rotorcraft, embodiments may be applied to any aircraft regardless of size, design, propulsion type, and control type (e.g., whether manned or unmanned) without deviating from the scope of the invention. Characteristics of some embodiments include, but are not limited to, fast and reliable estimation of the wind and related aerodynamic drag components without requiring expensive measurement units and significant computational power, generating feasible trajectories in real-time that take into account the estimated wind field, and designing controllers capable of compensating for the wind effects in tracking of the designed trajectories. 
       FIG. 1  is a perspective view illustrating a sUAS  100 , according to an embodiment of the present invention. While a sUAS is shown here, certain embodiments may be used for any type and/or size of aircraft without deviating from the scope of the invention. In this embodiment, sUAS  100  is a rotorcraft that includes four rotors  110  that are spaced equally about a body  120 . Each rotor  110  includes a propeller  112 , a motor  114  that drives propeller  112 , and a shaft  116  that connects motor  114  to body  120 . Motor  114 , and thus propeller  112 , can rotate about shaft  116 . For instance, by rotating two opposite rotors  110  by a same amount, lateral movement is possible. Other orientations would cause at least some degree of rotation. In some embodiments, only two opposing rotors are rotatable. 
     A computing system  130 , such as computing system  400  of  FIG. 4 , controls operation of sUAS  100 . sUAS  100  also includes sensors  140 , which provide data to computing system  130  that can be used for flight control. Sensors  140  may include, but are not limited to, IMUs, rate gyros, accelerometers, spin rate sensors, and/or any other suitable sensor without deviating from the scope of the invention. 
     I. Rotorcraft Dynamic Model 
     An example dynamic model for a sUAS rotorcraft is discussed below. 
     A. Equations of Motion 
     The dynamics of the center of mass of a multi-rotor vehicle, such as sUAS  100  of  FIG. 1 , in the East-North-Up Earth (inertial) frame (F E ) are given by:
 
 {dot over (r)} ( t )= v ( t )  (1)
 
 m{dot over (v)} ( t )= R   B/E ( t ) e   3   B   f   T ( t )+ f   D ( t )+ mg   (2)
 
     where r(t)=[x(t) y(t) z(t)] T  is the position of the center of mass in F E , v(t)=[v x (t)v y (t)v z (t)] T  is the inertial velocity, m is the mass, f T (t) is the total thrust generated by the rotors, R B/E (t) is the rotation matrix from the body frame F B  (Forward-Left-Up) to F E , e 3   B =[0 0 1] T  is the third unit vector of F B , f D (t) is the aerodynamic drag force, and g=[0 0 −g] T  is the acceleration of gravity. 
     The rotational dynamics of the vehicle about the center of mass are given in the frame F B  by:
 
 {dot over (R)}   B/E ( t )= R   B/E ( t )ω x ( t )  (3)
 
 J {dot over (ω)}( t )=−ω( t )× J ω( t )+ J   m ω m ( t ) ω ( t )+τ( t )+τ D ( t )  (4)
 
     where ω(t)=[p(t) q(t) r(t)] T  is the angular rate of F B  with respect to the inertial frame F E  expressed in F B , J=diag(J 1 ,J 2 ,J 3 ) is the inertia matrix of the vehicle (the body frame is aligned with the principal axes of inertia), J m  is the rotor inertia about the axis of rotation (assuming this is identical for all rotors),  ω (t)=[−q(t) p(t) 0] T , 
                   ω   m     ⁡     (   t   )       =       ∑     i   =   1     n     ⁢         (     -   1     )     i     ⁢       Ω   i     ⁡     (   t   )             ,       Ω   i     ⁡     (   t   )             
is the angular rate of the i th  rotor about its axis of rotation, τ(t) is the torque generated by the rotors, and τ D (t) is the aerodynamic rotational drag torque.
 
     It is assumed that all motors generate thrust in the positive z-direction in F B  frame e 3   B , and 
                   f   T     ⁡     (   t   )       =       ∑     i   =   1     n     ⁢       f   i     ⁡     (   t   )           ,         
where f i (t) is the thrust generated by the i th  rotor at time t.
 
     B. Aerodynamic Drag 
     The quadratic model 
             D   =       -     1   2       ⁢   ρ   ⁢     v   a   2     ⁢   S   ⁢     C   D             
for the translational drag (i.e., drag force) is used herein, where ρ is the air density, v a  is the speed of the body relative to the air, S is the cross sectional area, and C D  is the drag coefficient. It can reasonably be assumed that the air density is constant at the altitude corresponding to an urban environment. However, S and C D  depend on the body configuration and orientation with respect to air speed. In other words, S and C D  are constants in the body frame. Therefore, the drag force can be modeled in the body frame as f D   B =[−v a     x     B |v a     x     B |c D     x    −v a     y     B |v a     y     B |c D     y    −v a     z     B |v a     z     B |c D     z   ] T , where superscript B indicates body frame quantities, v a     x     B , v a     y     B , v a     z     B  are the components of the body relative to the air velocity in the body frame, and the coefficients c D     i   =½ρS i C D     i    are constant for each axis i=x, y, z. The drag force f D   B  should be translated in the inertial frame in order to apply to the translational dynamics. In other words, f D =R B/E f D   B , which can be written in vector-matrix form as:
 
     
       
         
           
             
               
                 
                   
                     f 
                     D 
                   
                   = 
                   
                     
                       
                         R 
                         
                           B 
                           / 
                           E 
                         
                       
                       ⁡ 
                       
                         [ 
                         
                           
                             
                               
                                 
                                   - 
                                   
                                     v 
                                     
                                       a 
                                       x 
                                     
                                     B 
                                   
                                 
                                 ⁢ 
                                 
                                    
                                   
                                     v 
                                     
                                       a 
                                       x 
                                     
                                     B 
                                   
                                    
                                 
                                 ⁢ 
                                 
                                   c 
                                   
                                     D 
                                     X 
                                   
                                 
                               
                             
                           
                           
                             
                               
                                 
                                   - 
                                   
                                     v 
                                     
                                       a 
                                       y 
                                     
                                     B 
                                   
                                 
                                 ⁢ 
                                 
                                    
                                   
                                     v 
                                     
                                       a 
                                       y 
                                     
                                     B 
                                   
                                    
                                 
                                 ⁢ 
                                 
                                   c 
                                   
                                     D 
                                     y 
                                   
                                 
                               
                             
                           
                           
                             
                               
                                 
                                   - 
                                   
                                     v 
                                     
                                       a 
                                       z 
                                     
                                     B 
                                   
                                 
                                 ⁢ 
                                 
                                    
                                   
                                     v 
                                     
                                       a 
                                       z 
                                     
                                     B 
                                   
                                    
                                 
                                 ⁢ 
                                 
                                   c 
                                   
                                     D 
                                     z 
                                   
                                 
                               
                             
                           
                         
                         ] 
                       
                     
                     = 
                     
                       
                         
                           - 
                           
                             R 
                             
                               B 
                               / 
                               E 
                             
                           
                         
                         ⁢ 
                         
                           Φ 
                           ⁡ 
                           
                             ( 
                             
                               
                                 ν 
                                 a 
                                 B 
                               
                               | 
                               
                                 ν 
                                 a 
                                 B 
                               
                               | 
                             
                             ) 
                           
                         
                         ⁢ 
                         
                           c 
                           D 
                         
                       
                       = 
                       
                         
                           - 
                           
                             R 
                             
                               B 
                               / 
                               E 
                             
                           
                         
                         ⁢ 
                         
                           Φ 
                           ⁡ 
                           
                             ( 
                             
                               c 
                               D 
                             
                             ) 
                           
                         
                         ⁢ 
                         
                           ν 
                           a 
                           B 
                         
                         ⁢ 
                         
                            
                           
                             ν 
                             a 
                             B 
                           
                            
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   5 
                   ) 
                 
               
             
           
         
       
     
     where the notation Φ(b)=diag (b x , b y , b z ) has been introduced for a vector b. 
     A similar model is adopted herein for rotational drag (i.e., drag torque) using the body angular velocity ω a  with respect to the air. In the body frame, the rotational drag can be expressed as τ D   B =[−ω a     x     B |ω a     x     B |c τ     x    −ω a     y     B |ω a     y     B |c τ     y    −ω a     z     B |ω a     z     B |c τ     z   ] T , where ω a     x     B , ω a     y     B , ω a     z     B  are the components of the body relative to the air angular velocity and c τ     x   , c τ     y   , c τ     z    are rotational drag coefficients, which are constant in the body frame. In this case, there is no need to express the rotational drag in the inertial frame. The following expressions for the translational and rotational drag are used herein:
 
 f   D   =−R   B/E Φ( v   a   B   |v   a   B |) c   D   =−R   B/E Φ( c   D ) v   a   B   |v   a   B |  (6)
 
τ D =−Φ(ω a   B |ω a   B |) c   τ =−Φ( c   τ )ω a   B |ω a   B |  (7)
 
     where c D  and c τ  are constant vectors of translational and rotational drag coefficients, respectively. 
     Adaptive Estimation, Trajectory Generation, and Flight Control 
       FIG. 2A  is a flowchart illustrating a process  200  for performing adaptive wind estimation and flight control for aerial systems using motion data, according to an embodiment of the present invention. The process includes performing adaptive estimation at  210  and generating a trajectory at  220  based on the adaptive estimation. The process also includes controlling an aircraft to track to the generated trajectory at  230 . In some embodiments, steps  210 ,  220 , and  230  may include part or all of the steps shown in  FIGS. 2B, 2C, and 2D , respectively. 
     II. Adaptive Estimation 
       FIG. 2B  is a flowchart illustrating a process  210  for performing adaptive estimation, according to an embodiment of the present invention. 
     A. Drag Estimation 
     First, the translational drag coefficient c D  is estimated at 211 when the airspeed and the inertial velocity are equal, i.e., when the rotorcraft flies in still air (such as indoors). To this end, the inertial velocity and orientation angle measurements of the rotorcraft are used, which are available from the sensors of the rotorcraft. In addition, it is assumed here that the total thrust generated by the rotors is available from the rotor models and spin rate measurements. By representing the translational dynamics in the form:
 
 m{dot over (v)} ( t )= f   T ( t ) R   B/E ( t ) e   3   B   +mg−R   B/E ( t )Φ( v   a   B ( t )| v   a   B ( t )|) c   D   (8)
 
     the following prediction model results:
 
 m {circumflex over ({dot over (v)})} ( t )= f   T ( t ) R   B/E ( t ) e   3   B   +mg−R   B/E ( t )Φ( v   a   B ( t )| v   a   B ( t )|) ĉ   D ( t )+λ l   {tilde over (v)} ( t )  (9)
 
     where {circumflex over (v)}(t) is the velocity prediction, ĉ D (t) is the translational drag coefficient estimate, λ l  is the error feedback gain, and {tilde over (v)}(t)=v(t)−{circumflex over (v)}(t) is the prediction error. The adaptive law for ĉ D (t) is derived from the Lyapunov stability analysis for the prediction error dynamics:
 
 m {circumflex over ({dot over (v)})} ( t )=−λ l   {tilde over (v)} ( t )− R   B/E ( t )Φ( v   a   B ( t )| v   a   B ( t )|) {tilde over (c)}   D ( t )  (10)
 
     where {tilde over (c)} D (t)=c D −ĉ D (t) is the estimation error. The Lyapunov function is chosen as: 
     
       
         
           
             
               
                 
                   
                     L 
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       
                         m 
                         2 
                       
                       ⁢ 
                       
                         
                           
                             ν 
                             ~ 
                           
                           T 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                         
                           ν 
                           ~ 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                     
                     + 
                     
                       
                         1 
                         
                           2 
                           ⁢ 
                           
                             γ 
                             l 
                           
                         
                       
                       ⁢ 
                       
                         
                           
                             c 
                             ˜ 
                           
                           D 
                           T 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                         
                           
                             c 
                             ˜ 
                           
                           D 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   11 
                   ) 
                 
               
             
           
         
       
     
     where γ l &gt;0 is the adaptation rate. It is straightforward that: 
     
       
         
           
             
               
                 
                   
                     
                       L 
                       . 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       
                         - 
                         
                           λ 
                           l 
                         
                       
                       ⁢ 
                       
                         
                           
                             ν 
                             ~ 
                           
                           T 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                         
                           ν 
                           ~ 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                     
                     + 
                     
                       
                         
                           
                             c 
                             ˜ 
                           
                           D 
                           T 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁡ 
                       
                         [ 
                         
                           
                             
                               - 
                               
                                 Φ 
                                 ⁡ 
                                 
                                   ( 
                                   
                                     
                                       
                                         ν 
                                         a 
                                         B 
                                       
                                       ⁡ 
                                       
                                         ( 
                                         t 
                                         ) 
                                       
                                     
                                     ⁢ 
                                     
                                        
                                       
                                         
                                           ν 
                                           a 
                                           B 
                                         
                                         ⁡ 
                                         
                                           ( 
                                           t 
                                           ) 
                                         
                                       
                                        
                                     
                                   
                                   ) 
                                 
                               
                             
                             ⁢ 
                             
                               
                                 R 
                                 
                                   E 
                                   / 
                                   B 
                                 
                               
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                             ⁢ 
                             
                               
                                 ν 
                                 ~ 
                               
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                           
                           + 
                           
                             
                               1 
                               
                                 γ 
                                 l 
                               
                             
                             ⁢ 
                             
                               
                                 
                                   
                                     c 
                                     ˜ 
                                   
                                   . 
                                 
                                 D 
                               
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                           
                         
                         ] 
                       
                     
                   
                 
               
               
                 
                   ( 
                   12 
                   ) 
                 
               
             
           
         
       
     
     Therefore, defining the adaptive law as:
 
{circumflex over ( ċ )} D ( t )=−γ l Φ( v   a   B ( t )| v   a   B ( t )|) R   E/B ( t ) {tilde over (v)} ( t )  (13)
 
     renders {dot over (L)}(t) negative semidefinite, implying that {tilde over (v)}(t) and {tilde over (c)} D (t) are globally bounded. In addition, application of Barbalat&#39;s lemma ensures that {tilde over (v)}(t)→0 as t→∞ when v(t) is bounded. Since Eq. (10) is a linear time invariant (LTI) system with the input R B/E (t)Φ(v a   B (t)|v a   B (t)|)c D (t), it follows that R B/E (t)Φ(v a   B (t)|v a   B (t)|){tilde over (c)} D (t)→0 as t→∞ if Φ(v a   B (t)|v a   B (t)|) is nonsingular (i.e., if the airspeed components v x   B (t), v y   B (t), v z   B (t) are nonzero). In fact, in this case, the convergence is exponential. 
     Next, the rotational drag coefficient is estimated at 212 using the prediction of the angular rate dynamics:
 
 J {circumflex over ({dot over (ω)})}( t )=−ω( t )× J ω( t )+ J   m ω m ( t ) ω ( t )+τ( t )−Φ(ω a   B ( t )|ω a   B ( t )|) ĉ   τ ( t )+λ r {circumflex over (ω)}( t )  (14)
 
     where {circumflex over (ω)}(t) is the prediction of the vehicle&#39;s angular rate, ĉ τ (t) is the estimation of the rotational drag coefficient, λ r &gt;0 is the error feedback gain, and {tilde over (ω)}(t) is the prediction error. The adaptive law for the estimate ĉ τ (t) is given by:
 
 {circumflex over (ċ)}   τ ( t )=−γ r Φ(ω a   B ( t )|ω a   B ( t )|){tilde over (ω)}( t )  (15)
 
     which results in the error system:
 
 J {tilde over ({dot over (ω)})}( t )=−λ r {tilde over (ω)}( t )−Φ(ω a   B ( t )|ω a   B ( t )|) {tilde over (c)}   τ ( t )  (16)
 
 {tilde over (ċ)} ( t )=γ r Φ(ω a   B ( t )|ω a   B ( t )|){tilde over (ω)}( t )  (17)
 
     As in the previous case, it can be shown that the error system in Eq. (16) is globally stable, and {tilde over (ω)}(t)→0 as t→∞ when ω(t) is bounded. Additionally, if all of the components of ω(t) are nonzero, {tilde over (c)} τ (t)→0 exponentially as t→∞. It should be noted that in the case of rotational drag, partial convergence is possible. In other words, if all components are not nonzero, the rotational drag coefficients corresponding to the nonzero components exponentially converge to their true values since Eqs. (16) and (17) are decoupled. 
     B. Wind Estimation 
     Once the translational and rotational drag coefficients have been estimated, wind velocities and accelerations are estimated at 213. To this end, the dynamics equations can be written as: 
     
       
         
           
             
               
                 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ν 
                         . 
                       
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                     = 
                     
                       
                         
                           
                             
                               f 
                               T 
                             
                             ⁡ 
                             
                               ( 
                               t 
                               ) 
                             
                           
                           m 
                         
                         ⁢ 
                         
                           
                             R 
                             
                               B 
                               / 
                               E 
                             
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                         ⁢ 
                         
                           e 
                           3 
                           B 
                         
                       
                       + 
                       g 
                       - 
                       
                         
                           1 
                           m 
                         
                         ⁢ 
                         
                           
                             R 
                             
                               B 
                               / 
                               E 
                             
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                         ⁢ 
                         
                           Φ 
                           ⁡ 
                           
                             ( 
                             
                               c 
                               D 
                             
                             ) 
                           
                         
                         ⁢ 
                         
                           
                             ν 
                             a 
                             B 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                         ⁢ 
                         
                            
                           
                             
                               ν 
                               a 
                               B 
                             
                             ⁡ 
                             
                               ( 
                               t 
                               ) 
                             
                           
                            
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   18 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       ω 
                       . 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       
                         - 
                         
                           J 
                           
                             - 
                             1 
                           
                         
                       
                       ⁢ 
                       
                         ω 
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       × 
                       J 
                       ⁢ 
                       
                         ω 
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                     
                     + 
                     
                       
                         J 
                         m 
                       
                       ⁢ 
                       
                         
                           ω 
                           m 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                         J 
                         
                           - 
                           1 
                         
                       
                       ⁢ 
                       
                         
                           ω 
                           _ 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                     
                     + 
                     
                       
                         J 
                         
                           - 
                           1 
                         
                       
                       ⁢ 
                       
                         τ 
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                     
                     - 
                     
                       
                         J 
                         
                           - 
                           1 
                         
                       
                       ⁢ 
                       
                         Φ 
                         ⁡ 
                         
                           ( 
                           
                             c 
                             τ 
                           
                           ) 
                         
                       
                       ⁢ 
                       
                         
                           ω 
                           a 
                           B 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                          
                         
                           
                             ω 
                             a 
                             B 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                          
                       
                     
                   
                 
               
               
                 
                   ( 
                   19 
                   ) 
                 
               
             
           
         
       
     
     where the airspeed v a   B (t)=v B (t)−w B (t) also includes the wind velocity w B (t) expressed in the body frame, and the relative angular rate ω a   B (t)=ω(t)−ω c   B (t) includes the air mass circulation rate (i.e., vorticity) ω c   B (t) expressed in the body frame. The terms −m −1 R B/E (t)Φ(c D )v a   B (t)|v a   B (t)| and −J −1 Φ(c τ )ω a   B (t)|ω a   B (t)| are treated as external disturbance signals s v (t) and s ω (t), respectively, so Eqs. (18) and (19) take the form: 
     
       
         
           
             
               
                 
                   
                     
                       ν 
                       . 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       
                         1 
                         m 
                       
                       ⁢ 
                       
                         
                           f 
                           T 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                         
                           R 
                           
                             B 
                             / 
                             E 
                           
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                         e 
                         3 
                         B 
                       
                     
                     + 
                     g 
                     + 
                     
                       
                         s 
                         v 
                       
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                   
                 
               
               
                 
                   ( 
                   20 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       ω 
                       . 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       
                         - 
                         
                           J 
                           
                             - 
                             1 
                           
                         
                       
                       ⁢ 
                       
                         ω 
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       × 
                       J 
                       ⁢ 
                       
                         ω 
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                     
                     + 
                     
                       
                         J 
                         m 
                       
                       ⁢ 
                       
                         
                           ω 
                           m 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                         J 
                         
                           - 
                           1 
                         
                       
                       ⁢ 
                       
                         
                           ω 
                           _ 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                     
                     + 
                     
                       
                         J 
                         
                           - 
                           1 
                         
                       
                       ⁢ 
                       
                         τ 
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                     
                     + 
                     
                       
                         s 
                         ω 
                       
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                   
                 
               
               
                 
                   ( 
                   21 
                   ) 
                 
               
             
           
         
       
     
     Similar to the drag estimation case, the prediction model and adaptive law for the translational dynamics are introduced as: 
     
       
         
           
             
               
                 
                   
                     
                       
                         ν 
                         ^ 
                       
                       . 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       
                         1 
                         m 
                       
                       ⁢ 
                       
                         
                           f 
                           T 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                         
                           R 
                           
                             B 
                             / 
                             E 
                           
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                         e 
                         3 
                         B 
                       
                     
                     + 
                     g 
                     + 
                     
                       
                         
                           s 
                           ^ 
                         
                         v 
                       
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                     + 
                     
                       
                         λ 
                         v 
                       
                       ⁢ 
                       
                         
                           ν 
                           ~ 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   22 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         
                           s 
                           ^ 
                         
                         . 
                       
                       v 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       γ 
                       v 
                     
                     ⁢ 
                     
                       
                         v 
                         ˜ 
                       
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                   
                 
               
               
                 
                   ( 
                   23 
                   ) 
                 
               
             
           
         
       
     
     where λ v &gt;0 and γ v &gt;0 are design parameters, {tilde over (v)}(t)=v(t)−{circumflex over (v)}(t) is the inertial velocity prediction error, and ŝ v (t) is the disturbance estimate. For the rotational dynamics:
 
{circumflex over ({dot over (ω)})}( t )=− J   −1 ω( t )× J ω( t )+ J   m ω m ( t ) J   −1   ω ( t )+ J   −1 τ( t )+ ŝ   ω ( t )+λ ω {tilde over (ω)}( t )  (24)
 
{circumflex over ( {dot over (s)} )} ω ( t )=γ ω {tilde over (ω)}( t )  (25)
 
     where Δ ω &gt;0 and γ ω &gt;0 are design parameters, {tilde over (ω)}(t)=ω(t)−{circumflex over (ω)}(t) is the angular rate prediction error, and ŝ ω (t) is the disturbance estimate. Denoting the estimation errors as {tilde over (s)} v (t)=s v (t)−ŝ v (t) and {tilde over (s)} ω (t)=s ω (t)−ŝ ω (t), the error system can be derived as:
 
 {tilde over ({dot over (v)})} ( t )=λ v   {tilde over (v)} ( t )+ {tilde over (s)}   v ( t )  (26)
 
 {tilde over ({dot over (s)})}   v ( t )=−γ v   {tilde over (v)} ( t )+ {dot over (s)}   v ( t )  (27)
 
     for the translational dynamics and
 
{tilde over ({dot over (ω)})}( t )=λ ω {tilde over (ω)}( t )+ {tilde over (s)}   ω ( t )  (28)
 
 {tilde over ({dot over (s)})}   ω ( t )=−γ ω {tilde over (ω)}( t )+ {dot over (s)}   ω ( t )  (29)
 
     for the rotational dynamics. 
     The error system of Eqs. (26)-(29) is a stable LTI system with inputs {dot over (s)} v (t) and {dot over (s)} ω (t). Thus, these equations have bounded solutions if the signals {dot over (s)} v (t) and {dot over (s)} ω (t) are essentially bounded (i.e., bounded everywhere except for on the sets that measure zero). Assuming that the flight control system provides a bounded and continuous inertial velocity and angular rate (or at least provides these sufficiently frequently), the approach of some embodiments can produce valid estimates of any wind field, even if the wind components abruptly change during a significant number of measurement instances over time. 
     To derive the upper bounds on the components of the estimation errors, it should be noted that Eqs. (26) and (27) and Eqs. (28) and (29) are decoupled. Accordingly, the following generic system may be introduced:
 
 {dot over (x)}   1 ( t )=−λ x   1 ( t )+ x   2 ( t )  (30)
 
 {dot over (x)}   2 ( t )=−γ x   1 ( t )+ f ( t )  (31)
 
     where x 1 (t) represents any component of the linear velocity prediction errors {tilde over (v)}(t) or angular velocity prediction errors {tilde over (ω)}(t), and x 2 (t) and f(t) represent the corresponding components of {tilde over (s)} v (t) or {tilde over (s)} ω (t), and {dot over (s)} v (t) or {dot over (s)} ω (t), respectively. 
     It should be noted that if f(t)=0, then x 1 (t) and x 2 (t) exponentially converge to zero from all initial conditions, which means that translational and rotational drag estimates exponentially converge to true values on any interval where the corresponding linear or angular drag components are constant. 
     Next, ignoring the exponentially decaying effects of the initial errors, the rate of decay for which is given by the design parameters k v  and k ω , the solution of EQs. (30) and (31) can be represented in the operator form as: 
     
       
         
           
             
               
                 
                   
                     
                       
                         x 
                         1 
                       
                       ⁡ 
                       
                         ( 
                         s 
                         ) 
                       
                     
                     = 
                     
                       
                         1 
                         
                           
                             s 
                             2 
                           
                           + 
                           
                             λ 
                             ⁢ 
                             s 
                           
                           + 
                           γ 
                         
                       
                       ⁢ 
                       
                         f 
                         ⁡ 
                         
                           ( 
                           s 
                           ) 
                         
                       
                     
                   
                   , 
                   
                     
                       
                         x 
                         2 
                       
                       ⁡ 
                       
                         ( 
                         s 
                         ) 
                       
                     
                     = 
                     
                       
                         
                           s 
                           + 
                           λ 
                         
                         
                           
                             s 
                             2 
                           
                           + 
                           
                             λ 
                             ⁢ 
                             s 
                           
                           + 
                           γ 
                         
                       
                       ⁢ 
                       
                         f 
                         ⁡ 
                         
                           ( 
                           s 
                           ) 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   32 
                   ) 
                 
               
             
           
         
       
     
     Since 
     
       
         
           
             
               
                 
                   
                     
                       
                          
                         
                           1 
                           
                             
                               s 
                               2 
                             
                             + 
                             
                               λ 
                               ⁢ 
                               s 
                             
                             + 
                             γ 
                           
                         
                          
                       
                       
                         H 
                         ∞ 
                       
                     
                     = 
                     
                       1 
                       γ 
                     
                   
                   , 
                   
                     
                       
                          
                         
                           
                             s 
                             + 
                             λ 
                           
                           
                             
                               s 
                               2 
                             
                             + 
                             
                               λ 
                               ⁢ 
                               s 
                             
                             + 
                             γ 
                           
                         
                          
                       
                       
                         H 
                         ∞ 
                       
                     
                     = 
                     
                       λ 
                       γ 
                     
                   
                 
               
               
                 
                   ( 
                   33 
                   ) 
                 
               
             
           
         
       
     
     it can be concluded that 
     
       
         
           
             
               
                 
                   
                     
                        
                       
                         
                           x 
                           1 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                        
                     
                     ≤ 
                     
                       
                         1 
                         γ 
                       
                       ⁢ 
                       ess 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sup 
                       ⁢ 
                       
                          
                         
                           f 
                           
                             [ 
                             
                               0 
                               , 
                               t 
                             
                             ] 
                           
                         
                          
                       
                     
                   
                   , 
                   
                     
                        
                       
                         
                           x 
                           1 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                        
                     
                     ≤ 
                     
                       
                         λ 
                         γ 
                       
                       ⁢ 
                       ess 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sup 
                       ⁢ 
                       
                          
                         
                           f 
                           
                             [ 
                             
                               0 
                               , 
                               t 
                             
                             ] 
                           
                         
                          
                       
                     
                   
                 
               
               
                 
                   ( 
                   34 
                   ) 
                 
               
             
           
         
       
     
     where ess sup|f [0,t] | denotes the essential supremum of |f(t)| on the [0, t] interval. It follows that the drag estimation errors {tilde over (s)} v (t) or {tilde over (s)} ω (t) can be decreased as desired by the proper choice of design parameters γ and λ. 
     The translational and rotational drag estimates ŝ v (t) and ŝ ω (t) will be used for the trajectory generation and control design purposes. Their respective rates of change are generated according to the corresponding adaptive laws. 
     The wind linear and angular change velocities can be computed from the equations: 
     
       
         
           
             
               
                 
                   
                     
                       
                         s 
                         ^ 
                       
                       v 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       - 
                       
                         1 
                         m 
                       
                     
                     ⁢ 
                     
                       
                         R 
                         
                           B 
                           E 
                         
                       
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                     ⁢ 
                     
                       Φ 
                       ⁡ 
                       
                         ( 
                         
                           c 
                           D 
                         
                         ) 
                       
                     
                     ⁢ 
                     
                       
                         ν 
                         a 
                         B 
                       
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                     ⁢ 
                     
                        
                       
                         
                           ν 
                           a 
                           B 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                        
                     
                   
                 
               
               
                 
                   ( 
                   35 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         s 
                         ^ 
                       
                       ω 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       - 
                       
                         J 
                         
                           - 
                           1 
                         
                       
                     
                     ⁢ 
                     
                       Φ 
                       ⁡ 
                       
                         ( 
                         
                           c 
                           τ 
                         
                         ) 
                       
                     
                     ⁢ 
                     
                       
                         ω 
                         a 
                         B 
                       
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                     ⁢ 
                     
                        
                       
                         
                           ω 
                           a 
                           B 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                        
                     
                   
                 
               
               
                 
                   ( 
                   36 
                   ) 
                 
               
             
           
         
       
     
     Since ŝ v (t) is in the inertial frame, it is first translated to the body frame ŝ v   B (t)=R E/B (t)ŝ v (t). Once this is done, it should be noted that the components of s v   B (t) have signs opposite to those of the corresponding components of v a   B (t) in the body frame. Therefore, the following equations can be written:
 
 c   Dx   |v   ax   B ( t )| 2   =m|ŝ   vx   B ( t )|  (37)
 
 c   Dy   |v   ay   B ( t )| 2   =m|ŝ   vy   B ( t )|  (38)
 
 c   Dz   |v   az   B ( t )| 2   =m|ŝ   vz   B ( t )|  (39)
 
     Solving for the wind components in the body frame yields: 
     
       
         
           
             
               
                 
                   
                     
                       
                         w 
                         ^ 
                       
                       x 
                       B 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       
                         v 
                         x 
                         B 
                       
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                     - 
                     
                       sign 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         ( 
                         
                           
                             
                               s 
                               ^ 
                             
                             
                               v 
                               ⁢ 
                               x 
                             
                             B 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                         ) 
                       
                       ⁢ 
                       
                         
                           
                             m 
                             
                               c 
                               
                                 D 
                                 ⁢ 
                                 x 
                               
                             
                           
                           ⁢ 
                           
                              
                             
                               
                                 
                                   s 
                                   ^ 
                                 
                                 
                                   v 
                                   ⁢ 
                                   x 
                                 
                                 B 
                               
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                              
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   40 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         w 
                         ^ 
                       
                       y 
                       B 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       
                         v 
                         y 
                         B 
                       
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                     - 
                     
                       sign 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         ( 
                         
                           
                             
                               s 
                               ^ 
                             
                             
                               v 
                               ⁢ 
                               y 
                             
                             B 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                         ) 
                       
                       ⁢ 
                       
                         
                           
                             m 
                             
                               c 
                               
                                 D 
                                 ⁢ 
                                 y 
                               
                             
                           
                           ⁢ 
                           
                              
                             
                               
                                 
                                   s 
                                   ^ 
                                 
                                 
                                   v 
                                   ⁢ 
                                   y 
                                 
                                 B 
                               
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                              
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   41 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         w 
                         ^ 
                       
                       z 
                       B 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       
                         v 
                         z 
                         B 
                       
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                     - 
                     
                       sign 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         ( 
                         
                           
                             
                               s 
                               ^ 
                             
                             
                               v 
                               ⁢ 
                               z 
                             
                             B 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                         ) 
                       
                       ⁢ 
                       
                         
                           
                             m 
                             
                               c 
                               
                                 D 
                                 ⁢ 
                                 z 
                               
                             
                           
                           ⁢ 
                           
                              
                             
                               
                                 
                                   s 
                                   ^ 
                                 
                                 
                                   v 
                                   ⁢ 
                                   z 
                                 
                                 B 
                               
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                              
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   42 
                   ) 
                 
               
             
           
         
       
     
     The wind angular velocity components are found in a similar manner: 
     
       
         
           
             
               
                 
                   
                     
                       
                         ω 
                         ^ 
                       
                       
                         c 
                         ⁢ 
                         x 
                       
                       B 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       p 
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                     - 
                     
                       
                         sign 
                         ⁡ 
                         
                           ( 
                           
                             
                               
                                 s 
                                 ^ 
                               
                               
                                 ω 
                                 ⁢ 
                                 x 
                               
                               B 
                             
                             ⁡ 
                             
                               ( 
                               t 
                               ) 
                             
                           
                           ) 
                         
                       
                       ⁢ 
                       
                         
                           
                             
                               J 
                               1 
                             
                             
                               c 
                               
                                 τ 
                                 ⁢ 
                                 x 
                               
                             
                           
                           ⁢ 
                           
                              
                             
                               
                                 
                                   s 
                                   ^ 
                                 
                                 
                                   ω 
                                   ⁢ 
                                   x 
                                 
                                 B 
                               
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                              
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   43 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         ω 
                         ^ 
                       
                       
                         c 
                         ⁢ 
                         y 
                       
                       B 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       q 
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                     - 
                     
                       
                         sign 
                         ⁡ 
                         
                           ( 
                           
                             
                               
                                 s 
                                 ^ 
                               
                               
                                 ω 
                                 ⁢ 
                                 y 
                               
                               B 
                             
                             ⁡ 
                             
                               ( 
                               t 
                               ) 
                             
                           
                           ) 
                         
                       
                       ⁢ 
                       
                         
                           
                             
                               J 
                               2 
                             
                             
                               c 
                               
                                 τ 
                                 ⁢ 
                                 y 
                               
                             
                           
                           ⁢ 
                           y 
                           ⁢ 
                           
                              
                             
                               
                                 
                                   s 
                                   ^ 
                                 
                                 
                                   ω 
                                   ⁢ 
                                   y 
                                 
                                 B 
                               
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                              
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   44 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         ω 
                         ^ 
                       
                       
                         c 
                         ⁢ 
                         z 
                       
                       B 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       r 
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                     - 
                     
                       
                         sign 
                         ⁡ 
                         
                           ( 
                           
                             
                               
                                 s 
                                 ^ 
                               
                               
                                 ω 
                                 ⁢ 
                                 z 
                               
                               B 
                             
                             ⁡ 
                             
                               ( 
                               t 
                               ) 
                             
                           
                           ) 
                         
                       
                       ⁢ 
                       
                         
                           
                             
                               J 
                               3 
                             
                             
                               c 
                               
                                 τ 
                                 ⁢ 
                                 z 
                               
                             
                           
                           ⁢ 
                           
                              
                             
                               
                                 
                                   s 
                                   ^ 
                                 
                                 
                                   ω 
                                   ⁢ 
                                   z 
                                 
                                 B 
                               
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                              
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   45 
                   ) 
                 
               
             
           
         
       
     
     Next, the wind linear and angular accelerations are computed using {circumflex over ({dot over (s)})} v (t) and {circumflex over ({dot over (s)})} ω (t) from the prediction models of Eqs. (22)-(25). Differentiating Eqs. (35) and (36) with respect to time: 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           s 
                           ^ 
                         
                         . 
                       
                       v 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       
                         - 
                         
                           1 
                           m 
                         
                       
                       ⁢ 
                       
                         
                           R 
                           
                             B 
                             / 
                             E 
                           
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                         ω 
                         ⨯ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                         Φ 
                         ⁡ 
                         
                           ( 
                           
                             c 
                             D 
                           
                           ) 
                         
                       
                       ⁢ 
                       
                         
                           ν 
                           a 
                           B 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                          
                         
                           
                             ν 
                             a 
                             B 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                          
                       
                     
                     - 
                     
                       
                         2 
                         m 
                       
                       ⁢ 
                       
                         
                           R 
                           
                             B 
                             / 
                             E 
                           
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                         Φ 
                         ⁡ 
                         
                           ( 
                           
                             c 
                             D 
                           
                           ) 
                         
                       
                       ⁢ 
                       
                         
                           
                             ν 
                             . 
                           
                           a 
                           B 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                          
                         
                           
                             
                               ν 
                               . 
                             
                             a 
                             B 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                          
                       
                     
                   
                 
               
               
                 
                   ( 
                   46 
                   ) 
                 
               
             
             
               
                 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         
                           
                             s 
                             ^ 
                           
                           . 
                         
                         ω 
                       
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                     = 
                     
                       
                         - 
                         2 
                       
                       ⁢ 
                       
                         J 
                         
                           - 
                           1 
                         
                       
                       ⁢ 
                       
                         Φ 
                         ⁡ 
                         
                           ( 
                           
                             c 
                             τ 
                           
                           ) 
                         
                       
                       ⁢ 
                       
                         
                           
                             ω 
                             . 
                           
                           a 
                           B 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       ⁢ 
                       
                          
                         
                           
                             ω 
                             a 
                             B 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                          
                       
                     
                   
                 
               
               
                 
                   ( 
                   47 
                   ) 
                 
               
             
           
         
       
     
     Taking into account 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             
                               
                                 ν 
                                 . 
                               
                               a 
                               B 
                             
                             ⁡ 
                             
                               ( 
                               t 
                               ) 
                             
                           
                           = 
                           
                             
                               
                                 
                                   
                                     ν 
                                     . 
                                   
                                   B 
                                 
                                 ⁡ 
                                 
                                   ( 
                                   t 
                                   ) 
                                 
                               
                               - 
                               
                                 
                                   
                                     
                                       w 
                                       . 
                                     
                                     B 
                                   
                                   ⁡ 
                                   
                                     ( 
                                     t 
                                     ) 
                                   
                                 
                                 ⁢ 
                                 
                                     
                                 
                                 ⁢ 
                                 and 
                                 ⁢ 
                                 
                                     
                                 
                                 ⁢ 
                                 
                                   
                                     
                                       ω 
                                       . 
                                     
                                     a 
                                     B 
                                   
                                   ⁡ 
                                   
                                     ( 
                                     t 
                                     ) 
                                   
                                 
                               
                             
                             = 
                             
                               
                                 
                                   
                                     ω 
                                     . 
                                   
                                   B 
                                 
                                 ⁡ 
                                 
                                   ( 
                                   t 
                                   ) 
                                 
                               
                               - 
                               
                                 
                                   
                                     ω 
                                     . 
                                   
                                   c 
                                   B 
                                 
                                 ⁡ 
                                 
                                   ( 
                                   t 
                                   ) 
                                 
                               
                             
                           
                         
                         , 
                       
                     
                   
                   
                     
                       
                         
                           
                             
                               
                                 ω 
                                 ^ 
                               
                               . 
                             
                             B 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                         = 
                         
                           
                             
                               
                                 
                                   
                                     
                                       
                                         
                                           s 
                                           ^ 
                                         
                                         . 
                                       
                                       v 
                                     
                                     ⁡ 
                                     
                                       ( 
                                       t 
                                       ) 
                                     
                                   
                                   + 
                                   
                                     
                                       1 
                                       m 
                                     
                                     ⁢ 
                                     
                                       
                                         R 
                                         
                                           B 
                                           / 
                                           E 
                                         
                                       
                                       ⁡ 
                                       
                                         ( 
                                         t 
                                         ) 
                                       
                                     
                                     ⁢ 
                                     
                                       ω 
                                       ⨯ 
                                       
                                         ( 
                                         t 
                                         ) 
                                       
                                     
                                     ⁢ 
                                     
                                       Φ 
                                       ⁡ 
                                       
                                         ( 
                                         
                                           c 
                                           D 
                                         
                                         ) 
                                       
                                     
                                     ⁢ 
                                     
                                       
                                         ν 
                                         a 
                                         B 
                                       
                                       ⁡ 
                                       
                                         ( 
                                         t 
                                         ) 
                                       
                                     
                                     ⁢ 
                                     
                                        
                                       
                                         
                                           ν 
                                           a 
                                           B 
                                         
                                         ⁡ 
                                         
                                           ( 
                                           t 
                                           ) 
                                         
                                       
                                        
                                     
                                   
                                   + 
                                 
                               
                             
                             
                               
                                 
                                   
                                     2 
                                     m 
                                   
                                   ⁢ 
                                   
                                     
                                       R 
                                       
                                         B 
                                         / 
                                         E 
                                       
                                     
                                     ⁡ 
                                     
                                       ( 
                                       t 
                                       ) 
                                     
                                   
                                   ⁢ 
                                   
                                     Φ 
                                     ⁡ 
                                     
                                       ( 
                                       
                                         c 
                                         D 
                                       
                                       ) 
                                     
                                   
                                   ⁢ 
                                   
                                     
                                       
                                         ν 
                                         . 
                                       
                                       B 
                                     
                                     ⁡ 
                                     
                                       ( 
                                       t 
                                       ) 
                                     
                                   
                                   ⁢ 
                                   
                                      
                                     
                                       
                                         ν 
                                         a 
                                         B 
                                       
                                       ⁢ 
                                       
                                         ( 
                                         t 
                                         ) 
                                       
                                     
                                      
                                   
                                 
                               
                             
                           
                           
                             
                               2 
                               m 
                             
                             ⁢ 
                             
                               
                                 R 
                                 
                                   B 
                                   / 
                                   E 
                                 
                               
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                             ⁢ 
                             
                               Φ 
                               ⁡ 
                               
                                 ( 
                                 
                                   c 
                                   D 
                                 
                                 ) 
                               
                             
                             ⁢ 
                             
                                
                               
                                 
                                   ν 
                                   a 
                                   B 
                                 
                                 ⁡ 
                                 
                                   ( 
                                   t 
                                   ) 
                                 
                               
                                
                             
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   48 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         
                           ω 
                           ^ 
                         
                         . 
                       
                       c 
                       B 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       
                         
                           
                             
                               s 
                               ^ 
                             
                             . 
                           
                           ω 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       + 
                       
                         2 
                         ⁢ 
                         
                           J 
                           
                             - 
                             1 
                           
                         
                         ⁢ 
                         
                           Φ 
                           ⁡ 
                           
                             ( 
                             
                               c 
                               τ 
                             
                             ) 
                           
                         
                         ⁢ 
                         
                           
                             
                               ω 
                               . 
                             
                             B 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                         ⁢ 
                         
                            
                           
                             
                               ω 
                               a 
                               B 
                             
                             ⁡ 
                             
                               ( 
                               t 
                               ) 
                             
                           
                            
                         
                       
                     
                     
                       
                         2 
                         ⁢ 
                         
                           J 
                           
                             - 
                             1 
                           
                         
                         ⁢ 
                         
                           Φ 
                           ⁡ 
                           
                             ( 
                             
                               c 
                               τ 
                             
                             ) 
                           
                         
                       
                       | 
                       
                         
                           ω 
                           a 
                           B 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       | 
                     
                   
                 
               
               
                 
                   ( 
                   49 
                   ) 
                 
               
             
           
         
       
     
     where division is understood to be component-wise. It should be noted that the wind acceleration estimates involve the inertial linear and angular accelerations of the rotorcraft expressed in the body frame. 
     III. Trajectory Generation 
       FIG. 2C  is a flowchart illustrating a process  220  for generating a trajectory based on the adaptive estimation, according to an embodiment of the present invention. In some embodiments, generating a trajectory through waypoints may take less than one millisecond. However, the processing time depends on the computational power of the given rotorcraft. The trajectory may be re-planned periodically (e.g., every five seconds). Each re-planning may involve executing process  220  of  FIG. 2C  again. 
     The trajectory generation algorithm takes into account the atmospheric effects in multi-copter dynamics using the estimates ŵ(t), {circumflex over (ω)} c (t), ŝ v (t), ŝ ω (t), and their derivatives from step  210  of the process described above. For this purpose, the following simplified equation of motion is considered:
 
 {dot over (v)} ( t )=   f   ( t ) R   B/E ( t ) e   3   B   +g+ŝ   v ( t )  (50)
 
     where the rotation matrix R B/E  (t) evolves according to:
 
 {dot over (R)}   B/E ( t )= R   B/E ( t )ω x ( t )  (51)
 
     and the mass-normalized total thrust 
                 f   _     ⁡     (   t   )       =         f   T     ⁡     (   t   )       m           
and the angular rate ω(t) are viewed as control inputs. The justification of this simplification is that the controller designed for the angular rate dynamics
 
{dot over (ω)}( t )=− J   −1 ω( t )× J ω( t )+ J   m ω m ( t ) J   −1   ω ( t )+ J   −1 τ( t )+ ŝ   ω(t)   (52)
 
     can provide fast and accurate tracking of the angular rate commands in the presence of rotational drag with or without wind. The design of the controller in some embodiments is presented in the next section. 
     To generate trajectories, the jerk minimization approach of Eqs. (26) and (27) is modified so that the estimate ŝ v (t) of the aerodynamic drag can be directly taken into account. For the given trajectory r(t)=[x(t) y(t) z(t)] T , the mass-normalized thrust vector T(t)= f (t)R B/E (t)e 3   B  can be expressed as:
 
 T ( t )= ( t )− g−ŝ   v ( t )  (53)
 
     which implies that
 
∥ T ( t )∥=∥ ( t )− g−ŝ   v ( t )∥=   f   ( t )  (54)
 
     In other words, the mass-normalized total thrust magnitude to traverse the given trajectory is defined by Eq. (54). The orientation of the thrust vector is defined by roll and pitch angles, the rate of change of which is related to the jerk (i.e., the third derivative of the position) of the given trajectory through Eqs. (50) and (51). Differentiating Eqs. (50) and (54) yields:
 
 ( t )=   {dot over (f)}   ( t ) R   B/E ( t ) e   3   B   + f   ( t ) R   B/E ( t )ω x ( t ) e   3   B +{circumflex over ( {dot over (s)} )} v ( t )  (55)
 
   {dot over (f)}   ( t )=( e   3   B ) T   R   B/E ( t )  ( t )−{circumflex over ( {dot over (s)} )} v ( t ))  (56)
 
     Solving for angular rates results in: 
     
       
         
           
             
               
                 
                   
                     [ 
                     
                       
                         
                           
                             
                               ω 
                               y 
                             
                             ⁡ 
                             
                               ( 
                               t 
                               ) 
                             
                           
                         
                       
                       
                         
                           
                             - 
                             
                               
                                 ω 
                                 x 
                               
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                           
                         
                       
                       
                         
                           0 
                         
                       
                     
                     ] 
                   
                   = 
                   
                     
                       
                         1 
                         
                           
                             f 
                             ¯ 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                       
                       ⁡ 
                       
                         [ 
                         
                           
                             
                               1 
                             
                             
                               0 
                             
                             
                               0 
                             
                           
                           
                             
                               0 
                             
                             
                               1 
                             
                             
                               0 
                             
                           
                           
                             
                               0 
                             
                             
                               0 
                             
                             
                               0 
                             
                           
                         
                         ] 
                       
                     
                     ⁢ 
                     
                       
                         R 
                         
                           B 
                           / 
                           E 
                         
                       
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                     ⁢ 
                     
                       ( 
                       
                         
                           
                             r 
                             … 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                         - 
                         
                           
                             
                               
                                 s 
                                 ^ 
                               
                               . 
                             
                             v 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                       
                       ) 
                     
                   
                 
               
               
                 
                   ( 
                   57 
                   ) 
                 
               
             
           
         
       
     
     which implies that: 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           ω 
                           x 
                           2 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       + 
                       
                         
                           ω 
                           y 
                           2 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                     
                   
                   ≤ 
                   
                     
                       1 
                       
                         
                           f 
                           _ 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                     
                     ⁢ 
                     
                        
                       
                         
                           
                             r 
                             ¨ 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                         - 
                         
                           
                             
                               s 
                               ^ 
                             
                             v 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                       
                        
                     
                   
                 
               
               
                 
                   ( 
                   58 
                   ) 
                 
               
             
           
         
       
     
     Eq. (57) defines the angular rates ω x  and ω y  required to traverse the given trajectory. A given trajectory can be traversed using the control inputs  f , ω x , and ω y . In other words, only these three control inputs are needed to generate a three-dimensional (3D) trajectory in some embodiments. Then, ω z  can be used to control the onboard sensor direction (i.e., the rotation of the rotorcraft around the thrust vector) for surveillance, mapping, package delivery, etc. herein, it is assumed that ω z =0. 
     The total thrust generated by the motors satisfies the physical constraint:
 
0≤ f   min   ≤ f   ( t )≤ f   max   (59)
 
     and the angular rate input is bounded due to sensor limitations as:
 
−ω max ≤ω a   B ( t )≤ω max   (60)
 
     which directly takes into account the estimate of wind vorticity {circumflex over (ω)} c   B (t). 
     Following the steps of Eqs. (26) and (27), single axis motion primitives are generated using a third order system:
 
   j ( t )= u   j ( t )  (61)
 
     with performance index 
     
       
         
           
             
               
                 
                   J 
                   = 
                   
                     
                       ∫ 
                       0 
                       
                         t 
                         f 
                       
                     
                     ⁢ 
                     
                       
                         
                           u 
                           j 
                           2 
                         
                         ⁡ 
                         
                           ( 
                           τ 
                           ) 
                         
                       
                       ⁢ 
                       d 
                       ⁢ 
                       τ 
                     
                   
                 
               
               
                 
                   ( 
                   62 
                   ) 
                 
               
             
           
         
       
     
     initial conditions s j (0),{dot over (s)} j (0),{umlaut over (s)} j (0) and final conditions s j (t f ),   j (t f ),{umlaut over (s)} j (t f ) for each j=x, y, z, where t f  is the time to traverse. The resulting closed form s j  is a 5 th  order polynomial in time, the coefficients of which depend on final time t f  and final conditions. Therefore, to fully define s(t)=[s x (t) s y (t) s z (t)] T , the initial state s(0),{dot over (s)}(0),{umlaut over (s)}(j) needs to be selected, which is assumed herein to be coincident with the current state r(t), v(t), a(t) of the rotorcraft (assuming all measurements are available), the final time t f , and the final state s(t f ), (t f ),{umlaut over (s)}(t f ) represent the final position, velocity, and acceleration. Velocity and acceleration at the final time may be free or specified without deviating from the scope of the invention. 
     The following algorithm is used for trajectory generation in some embodiments. Let a series of waypoints p j , j=1, . . . , N p  be provided by any desired planner as a matter of design choice without deviating from the scope of the invention, and let p 0 =r(t), where r(t) is the position of the vehicle at a current time t. An example of waypoints along a trajectory is shown in graph  300  of  FIG. 3  with arbitrary coordinates. As can be seen, each pair of adjacent waypoints is connected by a respective line segment. 
     For each line segment of the trajectory, the minimum possible time-to-go is computed at  221  by dividing the distance between the waypoints by the maximum possible velocity V max . If V max  is not available from the specifications of the vehicle, the following equation may be solved:
 
 f   max   e   3   B +max  R   E/B ( t ) g =Φ( c   D )( v   B ( t )− ŵ   B ( t ))| v   B ( t )− ŵ   B ( t )|  (63)
 
     for v B (t) and set V max =∥v B (t)∥, using the available wind velocity estimate ŵ B (t). The resulting time-to-go t go  may be used as a first iteration for the final time t f     1   =t go . 
     A trajectory for a segment s(η) is generated at  222  on the interval t≤η≤t f     1    according to the optimal control problem in Eqs. (61) and (62). The mass-normalized thrust vector T(η) required to traverse the trajectory s(η) component-wise is then computed at  223  using the equations:
 
 T   x (η)=   x (η)− ŝ   vx *( t )  (64)
 
 T   y (η)=   y (η)− ŝ   vy *( t )  (65)
 
 T   z (η)=   z (η)− ŝ   vz *( t )+ g   (66)
 
     where ŝ v *(t) is the worst case prediction of the maximum drag force, since the estimates of the drag force ŝ(t) and the wind velocity ŵ(t) are available only at current time t. This prediction can be computed as s v *(t)=ŝ v (t)+η{circumflex over ({dot over (s)})} v (t), assuming a constraint {circumflex over ({dot over (s)})} v (t) on the interval t≤η≤t f     1   . The total mass-normalized thrust is computed as 
                 f   ¯     ⁡     (   η   )       =           T   x   2     ⁡     (   η   )       +       T   y   2     ⁡     (   η   )       +       T   z   2     ⁡     (   η   )                 
and the minimum and maximum values  f   min  and  f   max  are numerically are numerically computed on the interval t≤η≤t f     1   .
 
     The thrust feasibility conditions of Eq. (59) is then checked for  f   max  and  f   min  at  224 . If the conditions are satisfied, then the following equation is computed: 
     
       
         
           
             
               
                 
                   
                     χ 
                     ⁡ 
                     
                       ( 
                       
                         t 
                         , 
                         
                           t 
                           
                             f 
                             1 
                           
                         
                       
                       ) 
                     
                   
                   = 
                   
                     
                       max 
                       
                         t 
                         ≤ 
                         η 
                         ≤ 
                         
                           t 
                           
                             f 
                             1 
                           
                         
                       
                     
                     ⁢ 
                     
                       
                         
                           
                             [ 
                             
                               
                                 
                                   
                                     s 
                                     ⃛ 
                                   
                                   x 
                                 
                                 ⁡ 
                                 
                                   ( 
                                   η 
                                   ) 
                                 
                               
                               - 
                               
                                 
                                   
                                     
                                       s 
                                       ^ 
                                     
                                     . 
                                   
                                   
                                     v 
                                     ⁢ 
                                     x 
                                   
                                 
                                 ⁡ 
                                 
                                   ( 
                                   t 
                                   ) 
                                 
                               
                             
                             ] 
                           
                           2 
                         
                         + 
                         
                           
                             [ 
                             
                               
                                 
                                   
                                     s 
                                     ⃛ 
                                   
                                   y 
                                 
                                 ⁡ 
                                 
                                   ( 
                                   η 
                                   ) 
                                 
                               
                               - 
                               
                                 
                                   
                                     
                                       s 
                                       ^ 
                                     
                                     . 
                                   
                                   
                                     v 
                                     ⁢ 
                                     y 
                                   
                                 
                                 ⁡ 
                                 
                                   ( 
                                   t 
                                   ) 
                                 
                               
                             
                             ] 
                           
                           2 
                         
                         + 
                         
                           
                             [ 
                             
                               
                                 
                                   
                                     s 
                                     ⃛ 
                                   
                                   z 
                                 
                                 ⁡ 
                                 
                                   ( 
                                   η 
                                   ) 
                                 
                               
                               - 
                               
                                 
                                   
                                     
                                       s 
                                       ^ 
                                     
                                     . 
                                   
                                   
                                     v 
                                     ⁢ 
                                     z 
                                   
                                 
                                 ⁡ 
                                 
                                   ( 
                                   t 
                                   ) 
                                 
                               
                             
                             ] 
                           
                           2 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   67 
                   ) 
                 
               
             
           
         
       
     
     assuming that {circumflex over ({dot over (s)})} v (t) is constant on the interval t≤η≤t f     1   , and the rate feasibility is checked at  225 . If the rate is feasible, the trajectory s(η), t≤η≤t f     1    is marked as a feasible trajectory between waypoints p 0  and p 1  at  226 , and the algorithm is advanced to the next waypoint at  227  taking s(t f     1   ), {dot over (s)}(t f     1   ), {umlaut over (s)}(t f     1   ) as the initial state of the next segment of the trajectory. 
     However, if any of the above checks fail, the algorithm sets t f =t f +iΔt at  228  for some time step Δt and i=1, . . . , N. The algorithm then returns to step  222 . This iteration is continued until a preset reasonable number N is reached. If no feasible trajectory is found, then the algorithm may exit with no feasible trajectory generated for the corresponding waypoint. The algorithm may then wait to receive a new set of waypoints from the planner. In the meantime, the rotorcraft can execute the generated segment of the trajectory if one exists, or hover or land if not. Landing may also be selected as an option if insufficient battery power or fuel remains. 
     When the algorithm of  FIG. 2C  exits with a feasible trajectory, it is a sub-minimum time trajectory. This means that for each segment of the trajectory, the time to traverse is within a Δt margin of a true minimum time trajectory. 
     With respect to the final state s(t f     1   ), {dot over (s)}(t f     1   ), {umlaut over (s)}(t f     1   ) selection in the optimal control problem formulation in Eqs. (61) and (62), the first state (position) s j (t f     1   ), i=1, . . . , N in each direction j=x, y, z is set to a corresponding waypoint p i =[x i  y i  z i ] T  provided by the planner. The third (acceleration) state ŝ(t f     1   ) is set to zero for all waypoints. The second (velocity) state, {dot over (s)}(t f     1   ) is selected as follows. The algorithm sets 
                 s   .     ⁢     (     t     f     N   p         )       =   0         
and
 
                 s   ¨     ⁡     (     t     f     N   p         )       =   0         
at the final waypoint p N     p    (arriving at rest). For all other waypoints, set {dot over (s)} x (t f     i   )=0 if |x i+1 −x i |≤ε x  and leave {dot over (s)} x (t f     i   ) free (unspecified) otherwise. Here, ε&gt;0 is a design parameter that the designer can choose according to the scale of the distance to be traveled in the x direction. For the remaining two directions (i.e., y and z), the waypoint velocity is set in a similar manner. This setup enables the rotorcraft to travel along the straight (or approximately straight) trajectories with maximal speed without slowing down at waypoints and engaging in excessive cross-track maneuvers.
 
     IV. Trajectory Tracking Controller 
     In this section, a controller is described for the rotorcraft to track the 3D trajectory r com (t)=[x com (t) y com (t) z com (t)] T  and the heading angle (ψ com (t)) commands generated in the previous section.  FIG. 2D  is a flowchart illustrating a process  230  for controlling an aircraft to track to the trajectory generated in  FIG. 2C , according to an embodiment of the present invention. 
     A. Center of Gravity (CG) Control 
     The motion of the CG of the rotorcraft is controlled at  231  by the thrust vector  f (t)R B/E (t)e 3   B  or by the magnitude of total thrust  f (t) and the orientation angles ϕ(t) and θ(t) according to the force equation in Eq. (50), which are designed from the perspective of tracking the trajectory command r com (t) or the velocity command v com (t), depending on the preference of the designer. When the objective is to track r com (t), the corresponding v com (t) can be generated using a backstepping approach as:
 
 v   com ( t )= c   1 [ r   com ( t )− r ( t )]+ {dot over (r)}   com ( t )  (68)
 
     where c 1 &gt;0 is a design parameter, otherwise:
 
 v   com ( t )= {dot over (r)}   com ( t )  (69)
 
     The control law is defined according to the equation:
 
   f   ( t ) R   B/E ( t ) e   3   B   =−ŝ   v ( t )− ŝ   v ( t )− g+c   2 [ v   com ( t )− v ( t )]+ {dot over (v)}   com ( t )  (70)
 
     where c 2 &gt;0 is a design parameter and {dot over (v)} com (t) is computed respectively from Eq. (68) or Eq. (69). Substituting the control law in Eq. (50) results in the exponentially stable error dynamics:
 
 ė   v ( t )=− c   2   e   v ( t )  (71)
 
     for the tracking error e v (t)=v(t)−v com (t). 
     The requisite total thrust and orientation angle may be obtained from Eq. (70) assuming that −π/2&lt;ϕ, θ&lt;π/2 (in other words, assuming that there are no flip-over maneuvers). This assumption ensures that the functions cos ϕ and cos θ are nonzero, and that sin ϕ and sin θ are one-to-one invertible. It follows from Eq. (70) written component-wise that:
 
   f   ( t )[cos ϕ( t )sin θ( t )cos ψ( t )+sin ϕ( t )sin ψ( t )]=− ŝ   vx ( t )− c   1   ė   x ( t )− c   2   e   x ( t )+ {umlaut over (x)}   ref ( t )≙ k   x ( t )  (72)
 
   f   ( t )[cos ϕ( t )sin θ( t )cos ψ( t )+sin ϕ( t )sin ψ( t )]=− ŝ   vy ( t )− c   1   ė   y ( t )− c   2   e   y ( t )+ ÿ   ref ( t )≙ k   y ( t )  (73)
 
   f   ( t )cos ϕ( t )cos θ( t )= g−ŝ   vz ( t )− c   1   ė   z ( t )− c   2   e   z ( t )+   ref ( t )≙ k   z ( t )  (74)
 
     The total thrust can be obtained from Eq. (74) as: 
     
       
         
           
             
               
                 
                   
                     
                       f 
                       T 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     m 
                     ⁢ 
                     
                       
                         
                           κ 
                           z 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       
                         cos 
                         ⁢ 
                         
                           ϕ 
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                         ⁢ 
                         cos 
                         ⁢ 
                         
                           θ 
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   75 
                   ) 
                 
               
             
           
         
       
     
     which basically controls the altitude or vertical speed of the rotorcraft. Next, multiplying Eq. (72) by cos ψ(t), multiplying Eq. (73) by sin ψ(t), and then adding and subtracting yields: 
     
       
         
           
             
               
                 
                   
                     
                       ϕ 
                       
                         c 
                         ⁢ 
                         o 
                         ⁢ 
                         m 
                       
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       sin 
                       
                         - 
                         1 
                       
                     
                     ⁡ 
                     
                       ( 
                       
                         
                           
                             
                               
                                 κ 
                                 x 
                               
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                             ⁢ 
                             sin 
                             ⁢ 
                             
                               ψ 
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                           
                           - 
                           
                             
                               
                                 κ 
                                 y 
                               
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                             ⁢ 
                             cos 
                             ⁢ 
                             
                               ψ 
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                           
                         
                         
                           
                             f 
                             T 
                           
                           ⁡ 
                           
                             ( 
                             t 
                             ) 
                           
                         
                       
                       ) 
                     
                   
                 
               
               
                 
                   ( 
                   76 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       θ 
                       
                         c 
                         ⁢ 
                         o 
                         ⁢ 
                         m 
                       
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     
                       sin 
                       
                         - 
                         1 
                       
                     
                     ⁡ 
                     
                       ( 
                       
                         
                           
                             
                               
                                 κ 
                                 x 
                               
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                             ⁢ 
                             cos 
                             ⁢ 
                             
                               ψ 
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                           
                           - 
                           
                             
                               
                                 κ 
                                 y 
                               
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                             ⁢ 
                             sin 
                             ⁢ 
                             
                               ψ 
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                           
                         
                         
                           
                             
                               f 
                               T 
                             
                             ⁡ 
                             
                               ( 
                               t 
                               ) 
                             
                           
                           ⁢ 
                           cos 
                           ⁢ 
                           
                             ϕ 
                             ⁡ 
                             
                               ( 
                               t 
                               ) 
                             
                           
                         
                       
                       ) 
                     
                   
                 
               
               
                 
                   ( 
                   77 
                   ) 
                 
               
             
           
         
       
     
     Changing ϕ and θ changes the thrust direction, which enables motion of the CG. 
     B. Attitude Control 
     The attitude control is determined at  232 . The control torque for the rotational dynamics can now be derived such that the Euler angle E(t) tracks the reference signal E ref (t) generated through the dynamics:
 
 Ė   ref ( t )=− c   E [ E   ref ( t )− E   com ( t )]  (78)
 
     where c E &gt;0 is a design constant and E com (t)=[ϕ com (t) θ com (t) ψ com (t)] T  is the combination of roll and pitch angles commands obtained from the perspective of the position tracking and yaw angle command provided by the trajectory generation algorithm. Using time scale separation and dynamic inversion techniques, an expression for the desired angular rates can be derived as follows:
 
ω com ( t )= H   −1 ( t )[− c   ω   e   E ( t )+ Ė   ref ( t )]  (79)
 
     where e E (t)=E(t)−E ref (t) is the attitude angles tracking error, c ω &gt;0 is the control gain, and H −1 (t) is the inverse matrix of H(t) given by: 
     
       
         
           
             
               
                 
                   
                     
                       H 
                       
                         - 
                         1 
                       
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   = 
                   
                     [ 
                     
                       
                         
                           1 
                         
                         
                           0 
                         
                         
                           
                             
                               - 
                               sin 
                             
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               θ 
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                           
                         
                       
                       
                         
                           0 
                         
                         
                           
                             cos 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               ϕ 
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                           
                         
                         
                           
                             sin 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               ϕ 
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                             ⁢ 
                             cos 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               θ 
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                           
                         
                       
                       
                         
                           0 
                         
                         
                           
                             
                               - 
                               sin 
                             
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               ϕ 
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                           
                         
                         
                           
                             cos 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               ϕ 
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                             ⁢ 
                             cos 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               θ 
                               ⁡ 
                               
                                 ( 
                                 t 
                                 ) 
                               
                             
                           
                         
                       
                     
                     ] 
                   
                 
               
               
                 
                   ( 
                   80 
                   ) 
                 
               
             
           
         
       
     
     The control torque can then be derived using Eq. (52) as follows:
 
τ( t )=ω( t )× J ω( t )− J   r3 Ω( t ) ω ( t )− ŝ   ω ( t )+ J [− c   ω   e   ω ( t )+{dot over (ω)} ref ( t )]  (81)
 
     where c ω &gt;0 is the control gain and e ω (t)=ω(t)−ω ref (t) is the angular rate tracking error, which satisfies the exponentially stable dynamics:
 
 ė   ω ( t )=− c   ω   e   ω ( t )  (82)
 
     and the signal ω ref (t) is generated through the reference dynamics:
 
{dot over (ω)} ref ( t )=− c   ω [ω ref ( t )−ω com ( t )]  (83)
 
     The individual motor inputs can be obtained by solving the control allocation equation: 
     
       
         
           
             
               
                 
                   
                     [ 
                     
                       
                         
                           
                             f 
                             T 
                           
                         
                       
                       
                         
                           
                             τ 
                             1 
                           
                         
                       
                       
                         
                           
                             τ 
                             2 
                           
                         
                       
                       
                         
                           
                             τ 
                             3 
                           
                         
                       
                     
                     ] 
                   
                   = 
                   
                     
                       [ 
                       
                         
                           
                             1 
                           
                           
                             1 
                           
                           
                             
                               ... 
                             
                           
                           
                             1 
                           
                         
                         
                           
                             
                               b 
                               
                                 1 
                                 ⁢ 
                                 1 
                               
                             
                           
                           
                             
                               b 
                               
                                 1 
                                 ⁢ 
                                 2 
                               
                             
                           
                           
                             
                               ... 
                             
                           
                           
                             
                               b 
                               
                                 1 
                                 ⁢ 
                                 n 
                               
                             
                           
                         
                         
                           
                             
                               b 
                               
                                 2 
                                 ⁢ 
                                 1 
                               
                             
                           
                           
                             
                               b 
                               
                                 2 
                                 ⁢ 
                                 2 
                               
                             
                           
                           
                             
                               ... 
                             
                           
                           
                             
                               b 
                               
                                 2 
                                 ⁢ 
                                 n 
                               
                             
                           
                         
                         
                           
                             
                               b 
                               
                                 3 
                                 ⁢ 
                                 1 
                               
                             
                           
                           
                             
                               b 
                               
                                 3 
                                 ⁢ 
                                 2 
                               
                             
                           
                           
                             
                               ... 
                             
                           
                           
                             
                               b 
                               
                                 3 
                                 ⁢ 
                                 n 
                               
                             
                           
                         
                       
                       ] 
                     
                     ⁡ 
                     
                       [ 
                       
                         
                           
                             
                               f 
                               1 
                             
                           
                         
                         
                           
                             
                               f 
                               2 
                             
                           
                         
                         
                           
                             ⋮ 
                           
                         
                         
                           
                             
                               f 
                               n 
                             
                           
                         
                       
                       ] 
                     
                   
                 
               
               
                 
                   ( 
                   84 
                   ) 
                 
               
             
           
         
       
     
     where the coefficients b ij , i=1, 2, j=1, . . . , n are derived from the geometry of the rotorcraft, and b 3j =(−1) j d, j=1, . . . , n, where d is the ratio between the drag and the thrust coefficients of the propeller blade for f i , i=1, . . . , n. Control allocation in general is specific to the design of the aircraft. Once the CG control and attitude control are determined, the rotorcraft is then controlled based on these determinations at  233 . In some embodiments, the CG and attitude control determinations may occur in parallel. 
       FIG. 4  is a block diagram illustrating a computing system  400  configured to perform adaptive wind estimation, trajectory generation, and flight control for aerial systems using motion data, according to an embodiment of the present invention. In some embodiments, system  400  may be, or may be integrated with, the flight control system of an aircraft, such as a rotorcraft. System  400  includes a bus  405  or other communication mechanism for communicating information, and processor(s)  410  coupled to bus  405  for processing information. Processor(s)  410  may be any type of general or specific purpose processor, including a central processing unit (CPU) or application specific integrated circuit (ASIC). Processor(s)  410  may also have multiple processing cores, and at least some of the cores may be configured for specific functions. System  400  further includes a memory  415  for storing information and instructions to be executed by processor(s)  410 . Memory  415  can be comprised of any combination of random access memory (RAM), read only memory (ROM), flash memory, cache, static storage such as a magnetic or optical disk, or any other types of non-transitory computer-readable media or combinations thereof. Additionally, system  400  includes a communication device  420 , such as a transceiver, to wirelessly provide access to a communications network. 
     Non-transitory computer-readable media may be any available media that can be accessed by processor(s)  410  and may include volatile media, non-volatile media, removable media, and/or non-removable media. The media may store computer-readable instructions, data structures, program modules, and/or other data. 
     Processor(s)  410  are further coupled via bus  405  to flight control systems  425  that facilitate physical control of the aircraft (e.g., rotors, ailerons, flaps, air brakes, etc.). As computing system  400  computes new wing, trajectory, and flight control information and solutions, flight control systems  425  may be controlled accordingly. 
     Memory  415  stores software modules that provide functionality when executed by processor(s)  410 . The modules include an operating system  430  for system  400 . The modules further include an adaptive wind estimation and flight control module  435  that is configured to perform the various adaptive wind estimation, trajectory generation, and flight control processes disclosed herein. System  400  may include one or more additional functional modules  440  that include additional functionality. 
     One skilled in the art will appreciate that a “system” could be any suitable avionics system for an aircraft, or a remote computing system, such as a personal computer, a server, a console, a personal digital assistant (PDA), a cell phone, a tablet computing device, or any other suitable computing device, or combination of devices, that is configured to control the aircraft remotely. Presenting the above-described functions as being performed by a “system” is not intended to limit the scope of the present invention in any way, but is intended to provide one example of many embodiments of the present invention. Indeed, methods, systems and apparatuses disclosed herein may be implemented in localized and distributed forms consistent with computing technology, including cloud computing systems. 
     It should be noted that some of the system features described in this specification have been presented as modules, in order to more particularly emphasize their implementation independence. For example, a module may be implemented as a hardware circuit comprising custom very large-scale integration (VLSI) circuits or gate arrays, off-the-shelf semiconductors such as logic chips, transistors, or other discrete components. A module may also be implemented in programmable hardware devices such as field programmable gate arrays, programmable array logic, programmable logic devices, graphics processing units, or the like. 
     A module may also be at least partially implemented in software for execution by various types of processors. An identified unit of executable code may, for instance, comprise one or more physical or logical blocks of computer instructions that may, for instance, be organized as an object, procedure, or function. Nevertheless, the executables of an identified module need not be physically located together, but may comprise disparate instructions stored in different locations which, when joined logically together, comprise the module and achieve the stated purpose for the module. Further, modules may be stored on a computer-readable medium, which may be, for instance, a hard disk drive, flash device, RAM, tape, or any other such medium used to store data. 
     Indeed, a module of executable code could be a single instruction, or many instructions, and may even be distributed over several different code segments, among different programs, and across several memory devices. Similarly, operational data may be identified and illustrated herein within modules, and may be embodied in any suitable form and organized within any suitable type of data structure. The operational data may be collected as a single data set, or may be distributed over different locations including over different storage devices, and may exist, at least partially, merely as electronic signals on a system or network. 
     The process steps performed in  FIGS. 2A-D  may be performed by a computer program, encoding instructions for processor(s) to perform at least the processes described in  FIGS. 2A-D , in accordance with embodiments of the present invention. The computer program may be embodied on a non-transitory computer-readable medium. The computer-readable medium may be, but is not limited to, a hard disk drive, a flash device, RAM, a tape, or any other such medium used to store data. The computer program may include encoded instructions for controlling the processor(s) to implement the processes described in  FIGS. 2A-D , which may also be stored on the computer-readable medium. 
     The computer program can be implemented in hardware, software, or a hybrid implementation. The computer program can be composed of modules that are in operative communication with one another, and which are designed to pass information or instructions to display. The computer program can be configured to operate on a general-purpose computer, or an ASIC. 
     Some embodiments provide a unified adaptive wind estimation, trajectory generation, and flight control approach for aircraft, such as multi-rotor drones, to facilitate effective navigation in environments with potentially rapidly changing wind conditions, such as urban environments. The adaptive algorithms of some embodiments provide capabilities for fast and reliable estimation of the aerodynamic drag coefficients of the aircraft in zero wind conditions, as well as the wind components along the flight trajectory. The navigation algorithms of some embodiments generate sub-minimum time and minimum jerk trajectories between given waypoints of the trajectory (i.e., along segments), taking into account the estimated wind. 
     The algorithms of some embodiments are fast enough to enable real-time adaptive wind estimation, trajectory generation, and flight control, relying on the analytic solutions of the single axis optimal control problem, the feasibility of which is checked with respect to the dynamics of the aircraft. The dynamics include real-time estimations of aerodynamic drag. The control algorithms of some embodiments are designed to track the generated trajectories as long as the aircraft retains controllability. The algorithms of some embodiments are computationally effective and can readily be implemented using the relatively smaller amounts onboard computing power available to sUASs as compared to larger aircraft. 
     It will be readily understood that the components of various embodiments of the present invention, as generally described and illustrated in the figures herein, may be arranged and designed in a wide variety of different configurations. Thus, the detailed description of the embodiments of the present invention, as represented in the attached figures, is not intended to limit the scope of the invention as claimed, but is merely representative of selected embodiments of the invention. 
     The features, structures, or characteristics of the invention described throughout this specification may be combined in any suitable manner in one or more embodiments. For example, reference throughout this specification to “certain embodiments,” “some embodiments,” or similar language means that a particular feature, structure, or characteristic described in connection with the embodiment is included in at least one embodiment of the present invention. Thus, appearances of the phrases “in certain embodiments,” “in some embodiment,” “in other embodiments,” or similar language throughout this specification do not necessarily all refer to the same group of embodiments and the described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments. 
     It should be noted that reference throughout this specification to features, advantages, or similar language does not imply that all of the features and advantages that may be realized with the present invention should be or are in any single embodiment of the invention. Rather, language referring to the features and advantages is understood to mean that a specific feature, advantage, or characteristic described in connection with an embodiment is included in at least one embodiment of the present invention. Thus, discussion of the features and advantages, and similar language, throughout this specification may, but do not necessarily, refer to the same embodiment. 
     Furthermore, the described features, advantages, and characteristics of the invention may be combined in any suitable manner in one or more embodiments. One skilled in the relevant art will recognize that the invention can be practiced without one or more of the specific features or advantages of a particular embodiment. In other instances, additional features and advantages may be recognized in certain embodiments that may not be present in all embodiments of the invention. 
     One having ordinary skill in the art will readily understand that the invention as discussed above may be practiced with steps in a different order, and/or with hardware elements in configurations which are different than those which are disclosed. Therefore, although the invention has been described based upon these preferred embodiments, it would be apparent to those of skill in the art that certain modifications, variations, and alternative constructions would be apparent, while remaining within the spirit and scope of the invention. In order to determine the metes and bounds of the invention, therefore, reference should be made to the appended claims.