Hi, I have been lurking for a while, but recently I got my quad up in the air on ardupirates, and just this week, I started getting successful flights with AC2. I am very impressed with the progress since early this year.


Now, what is the yoyo effect? when manually or automatically attempting to control the throttle of a multirotor, the craft will have a tendency to slowly oscillate in a fairly wide altitude range. That is, given a step throttle input, a quad will not go up to some height, instead it will overshoot its equilibrium height, and oscillate for a long time before settling (if it settles at all) so to hold a position, very careful operator input is required.


While many experienced pilots probably know how to hover at a specified altitude, the fact is, there is an inherent instability in the system. Here is my analysis and my proposal on how to deal with this issue.


Consider typical second order systems: A weight hanging from a spring in a frictional medium: pluck the weight and it will bob up and down. The oscillation will die out only as quickly as the frictional medium (air, honey, water?) is able to dissipate the energy.


I believe the process of attempting to hold altitude in a quad is a second order system: The quad has a mass (which will hold kinetic energy) but where is the spring that holds the potential energy? well, it turns out that the thrust your propellers are able to create is proportional to air pressure, and for all intents and purposes, we can say that thrust will vary linear over the range of oscillation: the higher the quad is, the lower the thrust. This efect is quite obvious closer to the ground, where the oscillations become more rapid, but regardless of altitude, thrust varies.


Now, when we model the system as if it was a spring, it is easy to see why it oscillates, but also, it is clear that the rate of oscillation will be rather slow, and DECEIVING, since an inexperienced pilot (or hypothetically an autopilot) would not quite realize what is happening and will produce throttle inputs that are always reactive, amplifying the problem.


Now, if the system had greater damping, these oscillations would die out quickly and it would be easier to maintain altitude, but we can't just make air "thicker" or put a sail on the quad to do this. We can produce a damping force by applying a differential term to thrust, in other words, create a little extra thrust in direct proportion to how fast the quad is moving and in opposition to it: When the quad is bobbing up, automatically create a little "downward" force by cutting thrust, and vice-versa.


There is a problem though: For this to work we need an accurate vertical speed to derive the term from. And neither of the altitude sensors (sonar or baro) has the responsiveness required to achieve a well behaved damping.


My approach (which I have tested for a number of weeks now) is to derive the differential throttle term from the accelerometer Z axis.


Here is the pseudo code:

th_dampK = 0.05;

th_dampP = -0.10;

leakRate = 0.1;


deltaZ = Z - lowPass_Z;

lowPass += Z * th_dampK;

speed_estimate += deltaZ;

speed_estimate *= 1 - leakRate; // leak 10 percent each iteration


throttle_damping = speed_estimate * th_dampP;


From physics class you will remember that the integral of acceleration is speed. So we can integrate the accelerometer output to obtain a value that is proportional to speed. There is a little problem with this method: The accelerometer will give a constant negative reading because there is gravity. If we were to merely integrate, we would come up with a speed that is always getting larger and larger, as if the quad was under free fall.


So that won't work without a small change: we high pass filter the accelerometer signal before integration, or use something called a "leaky" integrator. On every iteration the integrator looses a percentage of the total integral.


But will that work? The leaks have effectively highpassed the accelerometer's integral reading, We now only have "high passed" vertical velocity, or relative changes in velocity over the short term, but we never get an absolute velocity. Well, it turns out that this still works very well, because our goal here is not to read velocity but to create a "loss" in the yoyo effect. As long as there is yoyo effect to be dealt with, there is a short term velocity reading worth using.


There are a couple of last things to make this method safe: First, its range of influence has to be limited, so the damping term has to be constrained to some reasonable range, Otherwise, the throttle response will be adversely affected and the user will feel like they are fighting molasses on the way up or down; we want the pilot to feel like they are fighting molasses vertically when they are *trying* to hold the quad still (a good thing, because this "friction is helping hold still as well), but not when they are trying to climb quickly or drop quickly (not that you should be trying to drop quickly!, but that is another story)


throttle_damping = constrain(throttle_damping, -200, 100);


The end result is a throttle response that is much closer to "do what I mean" and not "do what I told you to" and I happen to believe that it doesn't only benefit manual control, but that automated means of throttle control *should* be wrapped around a "rate throttle controller" which is exactly what this method describes. We already do this for other axes of control, why not do it with throttle?


And the last thing: I have not tested this method at a tilted angle, nor do I think it is necessary for it to work at an angle.


So throttle_damping is scaled down gradually as the quad deviates from the horizontal plane. That way, rapid forward flight won't be affected. Remember the point of this is to facilitate keeping the quad still.


I haven't fully tested altitude hold in AC2 due to tuning, but I hope to get it working today. I am not necessarily proposing that this method be used in the altitude holding loop, my main objective with this method is to use it as manual throttle augmentation to smooth inputs, but I have a very strong feeling that the performance of altitude holding control methods is likely to drastically improve with this term added to the throttle. I will be testing this both ways once I have fully tested and tuned altitude holding as it is now.

I welcome feedback!


You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –


  • Does anything hardware related stop us from just ditching the sonar sensor all together and simply using the Z accel to hold minute changes in altitude and using say the barometer to correct what the Z accel messes up on? Essentially Im saying have the barometer used to get to a certain altitude and once at that altitude, hold it within the confines of the baro's accuracy by just using the Z accelerometer. It makes for a much simpler and more elegant sensor package, and probably more accurate. The sonar is kindof garbage in my opinion, its not a great way to measure altitude.

    I assume this is what MK and DJI do with their controllers and I hate to admit it but their position hold (taking into account no sonar) blows APM out of the water, even when considering the physical sensors are almost the same.

  • Hi guys, I've been interested in this problem too, because I find the altitude hold to be... well it just doesn't work.  I'm flying a 600 size trad. heli outdoors.  Wind gusts really have a bad effect on the Baro sensor, which is one problem.  I get height swings of up to 20m showing up while the heli is just sitting on the ground on a windy day.


    As for the yo-yo effect, I don't see that at all with my heli when I'm in Stabilize mode.  In Alt-Hold though, it's all over the place.


    As I understand it, the Throttle_Rate_PI and Alt_Hold_PI are both driven by the baro (or sonar) sensors yes?  I'm not really sure why it's like that.  Clearly the signal is way too "noisy" to work well.


    I'm trying to follow the math that you guys are doing, but it's a bit difficult, and I'm not really even sure it's necessary.  If I knew how to code well enough, I'd simply change the Throttle_Rate_PI to be driven by the virtual Z axis accelerometer, and then have the Alt_Hold_PI terms driven by the baro after heavy filtering. 

  • So what is the final outcome

  • Just wanted to say hello and I am delighted to learn that you guys are on to deeper usage of the accelerometer sensor data for performance enhancement. Now don´t get me wrong, but entering the arducopter realm with my ignorant optimistic beliefs I assumed we were already far into this road. I think some of the other (proprietary) auto pilot initiatives (like MK and DJI) are into this. Only way they could obtain the altitude stability demonstrated in some videos seen on the net.

    I said don´t get me wrong! I admire the development success achieved so far and I am greatful to be able to play with the stuff at hand with it´s amazing possibilities!

    AND I am convinced that once you have mastered optimal integration of accelerometer data into the control algorithms we have arrived at a new level of performance. Control responses will be more smooth, precise and predictible.

    Looking further down the road I would hope to see "intelligent" self optimizing tuning aides, which I believe will be totally realistic, one day. (Mikado had it for years in their flybarless unit "V-bar").

    Since I don´t know how to write a single line of C++ I try to contribute with some cheer-leading :D

  • All I did since now was trying to understand better what is one really measuring while "leaking" an integral. Naturally, the leaking process is more or less equal to high-pass filtering, which is more or less the opposite to integrating... That's why the leakRate is the key. I was trying to show this in the post with 3 charts in it. In my last post I merely added a comparison of how much better is accelerometer vertical velocity estimate than a dirty derivative from mere sonar readings.

    Regarding possible auto take-off and landing schemes: I'm going to test a climb rate hold controller basing on the leaking-integral idea in the next days. I would suggest turning on climb-rate-hold for say a second and checking if 50cm are reached after that time and then automatically switching to altitude-hold with sonar for take-off and the opposite for landing. I'll test that as well.

    Nice to hear you're going to test some more advanced sensor fusion, this would be best, of course. Provided it's not too performance hungry.

    Regarding phase lead - I mostly was trying to show, it's partly acceleration you're measuring with leaking integral as there is negative phase shift. I don't know yet how to design a controller to counteract this, but maybe this is not a big issue.

    So, thumbs up, and keep up the good work :). Nice we synchronized on that one.

  • To anybody following this discussion, I have started work on an improved version of this algorithm. Namely, instead of leaking the velocity away, the velocity drift is controlled by fusing the low frequency component of sonar derived velocity (constrained) and the error term from the difference between measured altitude and the altitude integrated from the velocity estimate.


    Lag in the sensors is dealt with by filtering the dead reckoned position prior to calculating the error. This subject probably deserves its own blog post and I will try to find time this weekend to make a detailed description.

  • I made another analysis, trying to avoid anything from causing sonar reading distortions (like keeping my sleeves out of the way) and, more importantly, adding sonar dirty-derivative to show the negative phase shift induced by high-pass filtering the integral (leaky-integral). It clearly shows how much better this approach should be as D-term in altitude control or for climb-rate control while taking-off or landing. One would only have to estimate the phase shift (should work as-is as well). See the picture below.

    3692296589?profile=originalIt's pretty easy to guess what is what. Sonar altitude in cm, vertical velocities in m/s, time in seconds. I'll try to implement some take-off and landing mechanism and make it a blog post next.

    Are you there Aurelio? :)

  • I researched the leaky-integrator a little further and this is what I found. I measured accelerometer velocity estimate for 3 different leak rates (0,1; 0,01; 0,001) and recorded them against sonar provided altitudes. Acc vertical velocities are depicted above, measured in m/s, altitudes shown below, expressed in cm.


    leakRate = 0,1



    leakRate = 0,01



    leakRate = 0,001


    For the leakRate = 0,1 one can clearly see there is still a really big amount of acceleration in the velocity estimate. If it were speed we were measuring, the blue line should cross 0m/s exactly where altitude starts rising / falling. So for me 0,1 is too little. leakRate of 0,001 seems to high, because there is too much of bias already in the velocity estimate (which would only get bigger if motors were on). So I'd go with 0,01 - there is also some acceleration visible in the velocity estimate, but it's acceptable for our needs already.

    Don't mind the noise in altitude measurement - the movement of quadrotor was smooth, I held it in my hand with motors off.


  • Developer

    This is god timing. I have just started building this for AC2. I started a few months ago, but didn't get far. I do have a rotated Z value from the DCM so it works at angles.

    I've taken a similar approach, but have not had success yet.

  • Moderator

    I wonder if you could combine this idea, and also add some configurable controls to limit or scale max vertical speeds when under a certain altitude. By doing so, you might create very precise vertical movement, create a "virtual airbag", and simultaneously improve automatic landing and takeoff routines. 

This reply was deleted.


Shivchand Jaysaval liked Shivchand Jaysaval's profile
Aug 25