Guidance algorithm for tracking smooth trajectories


I have written with my friend Yuri Kapitanyuk (the main idea was from him) an algorithm for solving the problem of tracking smooth curves by an unmanned aerial vehicle travelling with a constant airspeed and under a wind disturbance. Basically, if the trajectories are twice differentiable, i.e., they are continuous, without "spikes", etc. then the algorithm is capable to construct an attractive vector field around the trajectory in order to guide the vehicle.

The algorithm has been successfully tested on different fixed wings and it is available in Paparazzi. It has been written as an independent module there, so it should be easy to port to other platforms if somebody is interested. In fact, the code of the algorithm has been split in two parts there.

  • The first one is the core of the algorithm and it is transparent for the user that wants to include its own custom trajectories.
  • The second one corresponds to the trajectories, where the user has to provide the equations of the trajectory, its gradient and its Hessian (the gradient of the gradient).

That makes the algorithm very modular. As an examples (in the gif) I have already implemented in Paparrazi ellipses (they cover the case of circumferences) and sinusoidals (they also cover the particular case of straight lines a.k.a. zero frequency).

If one is interested in the details and how the algorithm works, I have written a more detailed post in my blog .


Views: 1543

Comment by Vladimir "Lazy" Khudyakov on November 5, 2016 at 9:36am

Nice job! 

Available as module?

Comment by Hector Garcia de Marina on November 5, 2016 at 10:03am

sorry, I forgot that no everybody is familiar with Paparazzi. 

Module in Paparazzi is an "almost" independent piece of code that can be connected to other modules/system. The algorithm can be found here:

You have some inputs (ground velocity and position) and an output (desired bank angle). How to access to these signals depends on the platform. The rest is in plain C code and does not have any further dependency beyond the std math library. Of course, how to include the code, compile, etc depends on the platform too.

By the way, the algorithm has been submitted to the next ICRA, we are waiting for the acceptance (we hope so!). For citations in a project or other papers: 

For a short or more accesible explanation:

And here how to use it in Paparazzi:

Comment by Vladimir "Lazy" Khudyakov on November 5, 2016 at 12:31pm


Many thanks, waiting for next release :)))

Comment by JB on November 5, 2016 at 9:35pm

Nice work Hector, and thanks for the share. I Like the more analog type flight paths created with your algorithm. This will likely improve flight efficiency and also allow mission planning to be more robust without hitting the max control outputs for steering (like max roll). Does this algorithm also work for height changes as well, so in "3D"?

Comment by Hector Garcia de Marina on November 6, 2016 at 12:40am

Hi JB, thanks!

Indeed, the algorithm accepts the popular splines as trajectories. Therefore, typical optimization routines for setting lateral accelerations offline can be applied.

Another nice thing about the algorithm is that one can tune "how aggressive" is the vector field with a single gain. The required maximum bank angle can be computed/assigned off-line, so planes with limited maneuvering can still make it. 

We can combine both things above. For example, you give a set of waypoints (or better "gates" like in downhill sky), the constraints of the airplane such as maximum/minimum expected ground speed and maximum bank angle. Then it should be possible to generate a set of splines to be followed. In a future, (not very close unfortunately xD), I would have students to work in the integration of such a stuff. 

Does it work in 3D?

Yes, it totally works in 3D, so you can define your desired climbing rate. In fact, it works with also time varying trajectories. For example, consider a 3D paraboloid whose 3D equation is X(x,y,z) = 0. Now consider a tilted plane (not parallel to the ground) Y(x,y,z) = 0. The following new equation X - Y = 0 defines their intersection! The following is a bit extreme for an airplane, but you will get the idea:

 Now the paraboloid can be fixed, but you can move over time the plane (for example up and down in order to cover the perimeter of the paraboloid in a spiral fashion), now you will have Y(x,y,z,t). Do not let the maths to scare you, as an user, you only have to provide (once) to the algorithm the explicit X, Y and their gradients and Hessians.

In my opinion, this approach has a lot of potential to be exploited! Since the math for the user that wants to define his custom trajectories are very simple. I would refer to the Paparazzi entry for more details about how to actually define a trajectory in your code. Of course, for the end-user all of this is transparent, the guy only needs to choose in his flightplan a preprogammed trajectory done by the developers.

Comment by Martin Rüedi on November 6, 2016 at 1:38am

@Hector, how has the wind be provided to the algorithm? Is it assumed to be constant on all altitudes? How can wind changes be quickly considered?

Comment by Hector Garcia de Marina on November 6, 2016 at 1:54am


Actually, the algorithm does not require constant wind, just to be "bounded", i.e. to have the ground speed far from zero. The algorithm needs the yaw and the heading course in order to calculate the cosine of the sideslip. This is an "inner" calculation, in fact, it is not needed for the construction of the vector field but for steering the plane to the vector field. 

In practice, we have found that by substituting the cosine of beta (sideslip) by one (even for actual big cross wind), and in the equations by changing all the "yaws" by "course headings" the algorithm stills works quite well. Therefore simplifying the sensing requirements (only GPS measurements). This is the current version implemented in Paparazzi.

While it is not strictly the same analytic solution provided in the paper, after several field test with different airplanes (from 500gr up to 2Kg and with wind speeds beeing almost the 50% of the nominal airspeed), we are quite happy with this (engineering) approximation where the sideslip is not longer required.

Comment by JB on November 6, 2016 at 6:17am

Hector thanks for the additional info. Looks and sounds great. Might have to see if I can motivate some devs to adopt it in Arduplane! I think it would be a great feature that can potentially give some "pre-emptive" rather than just "reactive" control to automated flight. I'm assuming all of this is being calculated on the FC MCU whilst in flight? Does it have much processor overhead?

Comment by Hector Garcia de Marina on November 6, 2016 at 6:36am


This is another of the ``beauties'' of the algorithm. The code of the algorithm is the function gvf_control_2D (line 97) at

as you can see there, it is rather inexpensive. In fact in the paper, it is just to read your position and velocity GPS and evaluate the expression in Theorem 3.4. Such a expression is just a couple of trigonometric operations, additions and multiplications. In our Paparazzi setup (typical STM32) I guess it is executed at 300Hz? 

In the experiment from the paper we were running it at 60Hz.

Comment by JB on November 6, 2016 at 9:01am

Very nice. Will have to add this as a feature request for Arduplane.

I had a quick look at your other work. Awesome stuff! I like! :-)

Keep it up. I think there's a whole bunch of things that can be done to optimise UAV flight strategies still. With better monitoring and control comes better hardware solutions as well (ie we can start leaving stuff out of the plane and still end up with a more capable platform) In particular the swarming behaviour is of interest to me, especially when decision making becomes a shared and distributed process. My mantra is decentralise, distribute and diversify. So "swarming intelligence" is key to those goals. ;-)


You need to be a member of DIY Drones to add comments!

Join DIY Drones

© 2017   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service