Quadcopter stabilization algorithm

@sbright33

Im using the same algorithm for the PID as describe here Improving the Beginner’s PID – Introduction « Project Blog

Its working well for roll and pitch, but I have a lot of problem to stabilize on the yaw axis, did you succeed?

I think you found the best solution for your goal of roll and pitch. How often do you sample? Have you worked on altitude hold yet? The yaw will drift without a compass. Do you have one connected? How did you choose tour tuning variables?

How about this code to fuse Ameter and pressure? I could sample at 20hz instead of 2hz. wasft-ft would often be 0.

set_throttle((wasft-ft)*gain1 +(gravity-accel)*gain2 +wasthrottle);

I know it's too simple, but would it work?

sbright33:
How about this code to fuse Ameter and pressure?

Have a read of this. It's not that simple and the Aeroquad guys still haven't got it working reliably. You can get it working on a test bench:
this video

But it's a totally different story on an actual flying quad.

Since it already works with my original code, I think the 2nd term in my formula may prevent/reduce any bumps, if I choose the proper value for gain2. Will experiment. Coming soon is Yaw. Also working on a fixed wing.

You can see the red line in the video does not perfectly match the movement of the sensor. There is still a delay. The peaks don't line up chronologically with reality. Sometimes the line is going up, when there is no movement etc. I admit it is better than I can do with pressure alone, because I cannot even detect 6" of movement with my cheap sensor. I have a delay also. But what good is such amazing resolution when it is often wrong? It will cause the quadcopter to shoot up or down when these errors happen, as you admitted they will. For my application I would rather have a reliable algorithm that keeps the craft within 10ft, instead of one that sometimes detects 6" of motion, other times screws up by a few feet. I am considering to make a similar demonstration using the same 2 sensors.

kakou19:
Its working well for roll and pitch, but I have a lot of problem to stabilize on the yaw axis, did you succeed?

Can you explain how you're trying to control yaw?

sbright33:
I am considering to make a similar demonstration using the same 2 sensors.

Please do! I haven't got a quad yet but want to make my own and write all the code for it myself. So I've done a lot of research and wrote a simulator to test some ideas.

Since you only want an accuracy of 10ft or so, you may not need to do complex sensor fusion. But I'm thinking you'll need the accelerometer to limit the vertical speed, in the cases where the baro gives incorrect readings. I've got to buy a baro sensor so I can play.

BTW what are planning to do with the fixed wing? I was thinking of making an autonomous glider which would detect and stay in thermals. But alas it's already been done:

Sorry that I haven't had a chance to look at your actual code - it's been a busy couple of days. Since you say it is achieving a more-or-less stable height and not oscillating or drifting, I assume that you either have implemented the I & D components, or don't need them.

I am probably teaching Granny to suck eggs (I hope that expression translates to your culture!) but here are my thoughts:

If you are working at the limits of the resolution of your barometer then I suggest you design the algorithm to hover at the transition between two values, rather than sitting in the middle of a value (iyswim). So, rather than saying the target height is 'X' and the height error is (current height - X), you say that the target height is the boundary between X and X+1 (X + 0.5, in other words). When you are at that height, you should see relatively frequent transitions in the baro readings between X and X+1 and the proportion of time at X and X+1 should be roughly equal. By averaging multiple readings of a signal with noise present you can get better resolution than the sensor's nominal resolution for a single sample. I suspect you are already doing something of the sort, but the algorithm needs to be designed to find and stay at the boundary between adjacent sensor values for this to work well.

If you have a vertical accelerometer then you can use this to interpolate between the barometer readings, and the barometer can be used to compensate for accelerometer drift and cumulative integration errors. I have a motorsport data logger (I didn't make it!) which provides a resolution down to a few inches using the combination of GPS and an accelerometer; you should be able to obtain far better resolution if your barometer alone gives you resolution down to a few feet. The way I'd approach this is similar to the algorithm used for NTP - periodically compare the position implied by the accelerometer against the position implied by the barometer. If there is a consistent drift between them then skew the accelerometer accel/speed/position integration formulae to reduce that drift to zero. I'm sure you'll see the potential for a PID algorithm to control the drift. :slight_smile:

@jabbdo
We should share ideas! I suggest that you start with a KK board for $12 including gyros. There's plenty of work to do besides that, why reinvent the wheel? At least you can see how it's supposed to work with your 'copter hardware. My autonomous glider will also have a motor. I fly paragliders for a living so I'm familiar with thermals. I have seen that R/C is far easier to keep up in the same thermals. Especially for ridge soaring. I think that is overly ambitious for cross-country. Instead I will try to optimize my flight time by flying faster during a down draft, and slower when going up, even turning off the motor.

@Peter
This granny likes eggs so thanks! Boundary transitions, wonderful idea! Will update my code. I knew I posted here for a reason. I will listen to you from now on. I need to make some improvements to reduce oscillating and drifting. I'm thinking about your last paragraph. Sounds complicated, I wish it were simpler. Maybe I will come up with something.

I will keep you updated here as my projects progress!

PeterH:
The way I'd approach this is similar to the algorithm used for NTP

Is that Marzullo's algorithm you're talking about?

sbright33:
I fly paragliders for a living so I'm familiar with thermals.

That's not a job. It's getting paid to have fun :slight_smile:
I was looking at the KK boards since you mentioned them several days ago. But I can't find much info on firmware etc.

Be careful there is more than one version of the hardware.
Within each, there is more than one version of the firmware.
Look here under the Files tab:

Or Google kk board firmware v2.1?
I cannot see a reason to change the firmware that comes loaded when you buy it. But you can look at the code to learn how it works. Unless you want to download X configuration.

I think Marzullos algorithm is overkill for our needs. I'm not even sure how it is relevant. But you do raise an interesting question. It's not that the Ameter data is better than barometer. It has more precision, and less delay. That's the important part. But it also has more noise. How to use both together?

jabbado:

PeterH:
The way I'd approach this is similar to the algorithm used for NTP

Is that Marzullo's algorithm you're talking about?

Not really.

A lot of NTP is about determining propagation delays to find the consensus time, but the part that seems most relevant here is the algorithm used to steer the local clock towards the consensus time. Rather than simply adjust the local clock to correct for any discrepancies, the algorithm aims to control the local clock by skew adjustments so that the discrepancies tend to zero. That seems analogous to the current problem of detecting and correcting long-term errors in the integrated speed/position calculations by comparing them against the low resolution, but believed accurate, barometric height measurements.

sbright33:
I need to make some improvements to reduce oscillating and drifting.

Not to labour the point, but this is precisely the purpose of the D and I terms in PID. Your reluctance to use a conventional PID algorithm still puzzles me; it strikes me as the simplest and most appropriate solution to this problem.

I am using the I term, it just looks different. The D term is harder to implement because of the large dt. This problem is odd because of the large resolution of the sensor, the large dt compared to the changes in direction/speed, and the large number of 0's in my variable chg (even at a larger dt than I've chosen). Please show me how to implement the conventional PID within these limits? Are you suggesting my pressure sensor is inappropriate? Then why does it nearly work with only 3 lines of pseudocode? I don't mean to be difficult. Please teach me or show me some untested code that may help?

I understand you have a large dt because the resolution of the barometer is poor so you don't get value changes very often. Changing your algorithm to hover at the boundary between adjacent values would help with that but not solve the problem. However, if you have an accelerometer then you can get a much finer resolution on the vertical velocity which gives you the input you need to the I term. You just need to ensure that if you do use an accelerometer, you merge it with the barometric input to eliminate drift and cumulative integration errors before applying it to the PID (or whatever feedback algorithm you prefer).

I think I understand. I'm experimenting. My first thought was to focus on the Ameter, even though the calculated velocity will slowly drift after 10 seconds. Because of the time between 1ft changes in position, the velocity will always be rounded greatly until you go very fast, which is not the goal when you're stabilizing a Quad. So I thought, when do you know the speed exactly? When you're going 0, and you have many 0's in a row. When that happens you can reset the velocity calculated by the Ameter to 0. But how do you get it to go 0 speed in the first place? First you have to measure the speed with the Ameter. Which doesn't help because of drift. It's a loop with no solution.

Try again. I'm going to sample the pressure every 1 sec which gives me absolute speed. The last 100-200ms of Accel will not be captured by the last data point so I will use that to adjust the speed. To give me more resolution I will spread out each 1ft move into 2x 0.5 ft changes. This will work great when the speed is in that range. I will always reset the speed to 0,0.5,or 1 ft/sec when it's moving slow, adjust by the last 100ms. When it's moving 0 for 3 in a row, I will assume I am stable and not likely to move again for a bit, or get data from the pressure sensor. Then I will be forced to keep summing the Ameter to get speed, until the pressure sensor gives me another tick. I know all this is hard to understand. Don't even bother until I tell you how well it works! I'll be back soon.

sbright33:
But how do you get it to go 0 speed in the first place? First you have to measure the speed with the Ameter. Which doesn't help because of drift. It's a loop with no solution.

That's not really true. You can use the accelerometer to detect small changes in vertical acceleration, vertical speed and height. The speed and height terms will be subject to cumulative errors. You can use the height indication from the barometer to detect errors in the height from the accelerometer. If the accelerometer says the height has changed by more than the resolution of the barometer, and the barometer does not indicate a change, then you can conclude the accelerometer is giving you an error. There are a couple of ways you can compensate for that. You could add a constant offset to the indicated height to make it consistent with the barometer reading. (In effect, you correct the error in the height but do not attempt to correct the speed.) Or you can calculate the drift between the two and offset the speed term accordingly. You can do this based on a single sample, or you can maintain a longer history and do it across multiple samples. Given the memory constraints, it probably makes sense to do it based on single samples.

Since this approach does not inherently bring the two data sources into perfect alignment, it only detects when they have diverged far enough to indicate an error, I would expect the altitude hold based on the accelerometer+barometer will drift until it reaches the boundary between adjacent barometer readings and then sit there. Of course you could help this process by designing your altitude control algorithm so that the target height is at the boundary too. This will make it easier to use averaging techniques to interpolate between multiple barometer samples, making it possible to get better than 'one bit' resolution. This in turn will enable you to detect and correct for smaller errors in the accelerometer derived speed and height.

As I mentioned previously, the process of detecting the discrepancy between the two altitude measurements and skewing the accelerometer to keep them in sync, seems like the sort of thing that PID would be good for.

I understand what you're saying. Remember the goal is to keep the speed at 0 using a feedback loop. Then slowly move the distance to match where you started. I didn't know PID was used to sync or combine 2 sensors. I thought PID was normally used in a feedback loop to modify something in the physical world which is being measured on a variable scale.

Has anyone had any success at making their own code to control attitude using pressure?