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!
Mike, I second your proposal of different altitude hold modes. I just suggested implementation of a new mode:
Thanks Robert. I generally fully agree. I'm also for introducing an internal loop for rate of climb control and an external loop for altitude control - so that's what I'd like to implement eventually.
Here is my strategy for sonar sampling. Sonar's (Maxbotix-XL) bandwidth is 20Hz. But this doesn't mean, we can only sample it at 20Hz. In fact, what I'm doing, is oversampling at 50Hz and averaging. This introduces a sort of a sub-resolution accuracy, and mostly, helps avoid any potential control problems due to delay caused by late sampling (ie. sampling with a large positive phase lag). It's because we don't actually know when new data arrives. I think additional filtering won't be necessary as soon as I can use vertical speed from accels instead of calculating it from sonar by dirty-derivating. Up until now I additionally filtered the oversampled sonar data with a moving average of 10 samples. This introduces a pretty big delay (phase shift), though. What it definitely helps is filters out altitude peaks occurring at very low altitudes (below 30cm) See picture below.
Also, let's stay focused on altitude control in this topic. Vertical position control with accelerometers on a VTOL aircraft is not feasible - the only thing that might be realistic is preventing unwanted behaviour due to wind gusts. It's because vertically, a quadrotor, remains in free fall, so accelerometers really measure almost nothing. What they do measure is Coriolis force and possible other accelerations due to accelerometer displacement from aircraft's center of gravity. They would measure accelerations due to wind gusts, and generally aerodynamic drag, though. But let's leave it for other discussions.
Tom, I like that graph, it demonstrates well the phase shift you are talking about. I've seen that myself when employing moving average filtering to stabilize an operator display which fluctuates too much. And I've seen how the display always lags reality when ramping speed up or down.
I can see how this would institute PIO, Pilot Induced Oscillation.
How about maintaining 2 filtered altitude averages? One of them is a simple moving average as you've shown, which appears to do an excellent job of filtering out that spike at the end, by the way. Have a second filtered altitude which attempts to be more responsive, by simply comparing each new datapoint against the last, determining the derivative, and comparing that to the Z-axis sensor. If the new datapoint does not correlate with the Z-axis data, then just throw out the data point. You could use the first filtered average to slowly affect the second to keep it close to reality over a timespan of... several seconds at least.
I dunno, just trying to figure out a way to use the sensor for what it's good at. It's not good at rapid control of altitude. It is good at long-term, absolute altitude measurements at over wide ranging heights.
I don't really understand what you're saying in your last paragraphy. Remains in freefall?
You mention the displacement of the accelerometer from the CoG, and I agree completely. I noticed on the DJI setup, they require the user to input the XYZ coordinates of the sensor relative to the CoG of the heli.
It's pretty easy to see how , if the sensor is several inches behind the CoG, that if the heli pitches forward (nose dips) the the sensor rises, resulting in an upwards acceleration to be measured. For sure that will confuse things. It needs to be accounted for mathematically.
Regarding sonar sampling: I've forgotten to mention one more filtering step I'm doing, ie. comparing each sample to the previous one and allowing only for 10% change each step. Due to the oversampling this is a safe mechanism and that's the thing that allows for peak cancellation.
Regarding vertical free-fall in near-hover scenarios - the most intuitive description of the effect I know of can be found here, p.41. I guess there is more info on that here (I mean the attachments) - sadly didn't have the time to read yet. I also happen to own a book "Aircraft control and simulation" by Stevens & Levis. There is a great explanation, p.25, of what an accelerometer measures.
Oh, and I forgot to comment on the oversampling, 50hz vs. 20hz. I think that's actually a good idea, because wouldn't it allow us to filter out some electrical noise within the sample? ie: assuming electrical noise is not occuring at 20hz, or even 50hz, two adjacent 50hz readings within the same 20hz sample should be identical.
Wouldn't we benefit from slowing down the sample rate on the sonar to even 1hz, but sample it 50 times and average, that should get rid of most electrical noise, no?
I don't know what possible error sources for sonars are and am not convinced they are mostly of electrical nature. I also would leave the sonar filtering discussion for later (in another topic). Here let's focus on altitude hold :).
I figured solving the problem of a noisy input was integral to get altitude hold to work?
Perhaps use 2 sonars, and average them in real time, to filter out noise?
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.
I am really pro Z-accel usage, but I think it would be a loss to completely abandon the sonar. Think of it, there are tasks only possible while using a cm resolution height measuring sensor, like following a ground contour, or locking altitude 2500 cm over a water surface. Let´s improve the sonar handling and keep it where it counts.
During Christmas I had the opportunity to play around a little with my new Parrot AR Drone, and I must admit it was fashinating to have it speed around 30 cm over the ground, without worrying about it hitting ground (the AR Drone uses sonar). I want that functionality on our drones too. Actually I think we can get some inspiration from the AR Drone implementation. While being a toy, still they have implemented techonology partly superior to what we have here. Let´t take that as a challange!
I am running .49 on my stock quad and regularly fly 30 cm hi at great speeds while yawing rapidily. The sonar is dead accurate (not so with later versions) in a a way a baro can only dream of. Terrain following really low is one of the coolest things this can do.