V2.2 - ACRO PID implementation is wrong, right?

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.

V2.2 - ACRO PID implementation is wrong, right?

Postby alex.khoroshko » Mon Jun 10, 2013 5:16 pm

Hello!
Here's my schematic (sorry for that - made in hurry) of current PID implementation for ACRO mode. Red - variables names.
Новый рисунок (9).gif

1. comment says, that it takes into account the cycle time, but in fact, there's no such thing.
2. why there is 32-bit division?
Code: Select all
else error = ((int32_t)rcCommand[axis]<<6)/conf.P8[axis]
why not replace with multiplication on constant and precalculate it on startup and P8 change?
3. Why P8 is added to I component? also, see 4.
4. P8 for I component and P8dyn for P are in the feedback part of the loop, not the forward part. This means, that changing these variables would not only change the open loop gain, but would also alter the total system amplification (in this case - gyro scaling). I can't bear the fact, that when I increase P in GUI, the rate to stick position scaling is decreased. Is that done intentionally? I don't feel like it's a good thing.
5. When dynamic P adjustment takes place (P8dyn is decreased) the scaling for P and I component becomes different - they start to try maintaining the different rate values. I don't think that it helps. Initial idea was to change not the open loop gain (as well as I and D), but just change the scaling corresponding to the throttle, right?
6.
Code: Select all
if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
This not only shuts off the I component when rate exceeds some value, but also zeroes the accumulated I value. Why?
7. D component doesn't take into account stick position at all. That's why it doesn't make the control sharper - it just tries to dampen fast angle changes of the body, and that's it.
I'm currently testing the rewritten version, where classic PID controller is implemented - P I and D are in the forward part of the loop, so no scaling problem present and other, supposedly, problems are fixed. But first I would like to hear the opinion, do I get it all right, or some/all of listed facts serve the useful purpose?

P.S. Forgot to say - "deltasum" includes difference calculation, moving average of 3 elements that code does and multiplication by dynD8
alex.khoroshko
 
Posts: 27
Joined: Sun Jun 09, 2013 9:17 am

Re: V2.2 - ACRO PID implementation is wrong, right?

Postby EmmeDi8 » Mon Jun 10, 2013 7:26 pm

Hi,
in this days I play with PID controller too.
IMO acro PID is not wrong is only super optimized for fast execution.
Reading the code is easy to see various method to save byte (cast to 32 bit only where is needed) and cpu time, such as lsb (<<) or rsb(>>) instead of regular multiply or division.
Maybe there is other margin, but is very hard optimize further. I try to reply to your question and I'm not an engineer, so have patience if the answers are not precise.

1. comment says, that it takes into account the cycle time, but in fact, there's no such thing.


Yes, I think consider cycle time is the optimum. But, maybe Alex have place on the balance CPU vs RESULT, and maybe more complex software can have too long cycle time.

2. why there is 32-bit division?

rcCommand can be 500 then * 64 = 32000 at max.
However, precalculate imo is good idea.

6. This not only shuts off the I component when rate exceeds some value, but also zeroes the accumulated I value. Why?

I term after some time, can drift to the max value. Any little error is always sum to the I term. Maybe on large movement this must be cleared.

7. D component doesn't take into account stick position at all

D term is at the end of the function, is not calculated into account of stick position, but by the velocity of error changes.

The next my experiments is on "I" term. The idea is copy/past "angle mode" code to "horzion mode" without "I" sum.
So I would try to hover in angle mode and when quadcopter is stable, switch to horizon mode. I'm expect to see less drift.

When available if you can share your code I can test with my little multirotor.
Last edited by EmmeDi8 on Mon Jun 10, 2013 11:03 pm, edited 1 time in total.
EmmeDi8
 
Posts: 15
Joined: Thu Sep 06, 2012 5:15 pm

Re: V2.2 - ACRO PID implementation is wrong, right?

Postby chris_kmn » Mon Jun 10, 2013 7:34 pm

I'm also a friend of a good control loop integration and would love to test the result too with my test'o'copter :)

There is something like a demo application for a PID controller at the atmel.com site. Maybe that could give some hints.

And here is another example:

https://code.oregonstate.edu/svn/appliedrobotics/ExampleCode/pid_demo/pid.c
Attachments
image.jpg
chris_kmn
 
Posts: 31
Joined: Sun Jun 09, 2013 11:11 am

Re: V2.2 - ACRO PID implementation is wrong, right?

Postby shikra » Mon Jun 10, 2013 11:27 pm

Starting point - MultiWii ACRO is already considered one of the best if not the best performance in acro. (when tuned). Good luck to improve!

1 - true - intention is to run with fastest loop time possible. I think that is what words mean. There is option to run fixed loop time I think - or maybe that is baseflight port.

2 - as above!

3 Where is this? I am not clear here.

4 Yes, but does it matter in flight - can you notice?

5 - intentional - default is 0 throttle compensation. PID rates reduce from mid throttle to full throttle to compensate for the motors generating proportionally more power to change. It does help reduce wobble full throttle in acro. I use it and it helps. I think what concerns you does not matter in real flight. Multiwii is based on many approximations that perfect theory is not noticeable to us human flyers. Maybe Warthox can tell!

6 - assumption is high value means fast angular change. Hence I is not needed - and too slow to give good input. I is more to help maintain / force to desired position if not reaching quickly.

7 - indirectly it does. more stick input=larger error angle = stronger force to return = faster attitude change=larger D.
I think there is potential here. Fast ACRO fliers may want a large D value only when fast angular change only approaching 0 error angle. For very fast, sharp snappy control. I posted a level mode variant of that a whole back. It was very effective. I'd like to revisit to get it added one day.
User avatar
shikra
 
Posts: 494
Joined: Wed Mar 30, 2011 7:58 pm

Re: V2.2 - ACRO PID implementation is wrong, right?

Postby alex.khoroshko » Tue Jun 11, 2013 3:26 am

Is it your personal experience, about best performance? What boards did you compare it to? I compared to KK2.0 board and was disappointed. I love multiwii and really want it to be the best, that's why I started this thread. A little disclaimer - I don't want to make someone's work look less valuable, but I feel that I can help improve something.
2. - division is the slowest thing EVER (float math not included). How, in your opinion, does it help to improve processing time?
1. The main loop is processed for different time (sometimes there is a GPS package to process, sometimes there isn't, for example). Thus, D difference ('delta' variable) coresponds to different dt and would be different each time, resulting in different D component each time. That's why, I suppose, moving average was introduced for D. Hardware multiplication needed for different dt correction takes only 2 cycles for 8 bit - 0.125 uS. 1 or 2 microsecond - is this such a horrible sacrifice? I suspect correction would help to remove moving average or make if of 2 points, not 3 - that would result in comparable performance. Also, fixing point 2 of the discussion (removing division) would instantly improve overall performance. Second impact of uncompensated loop processing time is on I. It is not sensible to jitter. But, different builds would have different average loop processing time (GPS on and off, for example), so I component would accumulate with different speed, resulting in different meaning for I value in each build. This effect might be acceptable.
3. it's to the left of sum block for I on my diagram.
4. Yes, and I didn't like that so much. First, I flied with 5ah battery - quad flied long, but was heavy. I swapped to 2200 and retuned PIDs. Stick scaling changed. Not good feel at all. Nothing of the sort happened on my KK2.0 - retune, feel the same.
Point 5 and 6 takes start from misunderstanding the PID control, sorry if that's a bit rude. The formula for calculating the closed loop gain (not inside the loop like open loop gain, but from input to output) is ForwardGain/(1+ForwardGain*FeedbackGain). In current version, forward gain is not adjusted, but the FeedbackGain is changed - it is reduced. Substitute to the formula and you'll see, that overall gain from sticks to quad angle rate is increased - that was the idea, and actually, this is the same, as simple expo curve, which also increases scaling in extreme positions. At the same time, the precision of the closed loop is decreased (it is proportional to open loop gain) and that's a bad thing. Nonlinear control systems utilize quite opposite approach - increase gain when the error is big to make system move toward the desired point faster and decrease it when system arrives to desired point to prevent oscillations. I tried this approach in my PhD dissertation (resonant mode power supply was controlled, but whatever) and it worked excellent! A little diclaimer here - dissertation was mostly in power electronics, not control systems, so I do not claim myself to be a guru of control systems, even though I have a great experience in them.
Same for I. Imagine the system, where the motors, ESCs are slow and start to introduce the dynamical influence early - this multirotor would be stable on very low P. I gain though can still be raised relatively high. When you move the stick, first, some force is applied because of P, then I is accumulated and applies some force as well. Rate is increased, until this condition (to shut off the I) is met. So, what's the point? Imo if it works as I described, nothing except great error would happen. Yes, in some systems, I component would slow down the response. But, once again, gain increasing approach is so much better - just increase the I gain (before integrator - to help it accumulate faster) when error is too big - problem solved. To limit I influence there's classic Imax value, why not use?
7. wrong. more stick input=larger angle rate (not error - I remind that stick position is not taken into account in current implementation) = stronger system opposes to such a change - no matter, whether the angle change is introduced from outside (D does a good thing) or is it from stick change (current D implementation opposes this one). And last statement about big P only when error is small - it's only because of current, let's say self-scaling, implementation. If I think right, classic implementation (especially with non-linear additions) would be much better.

Also, do anyone have a video confirming this high-power instability everybody talking about? Quad wobbles during making a loop (sticks on extreme), are you sure? From control systems theory that's not logical - motors loose efficiency when approaching to the maximum speed, which would result in open loop gain reduction and increment in stability. I'm not 100% sure, because propellers have their own nonlinearity, which is not quite obvious to me. Anyway, my multirotors if wobble, do that noticeably only during hover.
alex.khoroshko
 
Posts: 27
Joined: Sun Jun 09, 2013 9:17 am

Re: V2.2 - ACRO PID implementation is wrong, right?

Postby Hamburger » Tue Jun 11, 2013 9:46 am

interesting. The best way to tell would be to have an alternate second implementation. As the code section is relatively small and contained, it should be easy enough to swap it for testing or even keep it in parallel.
AlexK, are you going to implement your alternate pid handling? If you provide a unified diff, I will test fly it, promise.
User avatar
Hamburger
 
Posts: 2273
Joined: Tue Mar 01, 2011 2:14 pm

Re: V2.2 - ACRO PID implementation is wrong, right?

Postby alex.khoroshko » Tue Jun 11, 2013 10:05 am

Oh wow, thanks! I've made the alternative implementation, including nonlinear I adjustment (the most simple single-step increment for now). Didn't have enough time yesterday evening to even pick all the divisions in code so, that gains (ACRO: P, I and D) more or less correspond to current (v2.2) values - it was so hard for me to discover the ranges/scales of the signals (especially the gyro), that I decided to just pick all from practical experiment. Hope to finish today evening and then I would post the code here.
alex.khoroshko
 
Posts: 27
Joined: Sun Jun 09, 2013 9:17 am

Re: V2.2 - ACRO PID implementation is wrong, right?

Postby alll » Tue Jun 11, 2013 12:07 pm

Since I and D are time driven, it is very important that they are precisely called at regular intervals!
http://www.pidstop.com/index.php?r_id=999
If we could have this “behavior” for all PID implementations of multiwii (knowing what the error and reference are for each pid control) it would be wonderful:

And as said before, no need of physical values are needed, just increasing/decreasing P, I and D would be enough in the gui (0..100%), the code should do the conversion math and deal with the physical sensor data units / ranges. (my whish list ;)
User avatar
alll
 
Posts: 211
Joined: Fri Dec 07, 2012 9:53 am

Re: V2.2 - ACRO PID implementation is wrong, right?

Postby shikra » Tue Jun 11, 2013 12:55 pm

Best performance board - views will be biased here...! I think its what is best for you that is important.

I use KK, MultiWii, Baseflight and Openpilot. I have flown others too. I will not say multiwii is the best.... I think they are all pretty close when set up. I think KK is sharpest, but for my style that is not the best. I think for some it would be. I just prefer slightly softer Wii. When set up correctly they are all excellent.

Great to have you on board and hope to see some results from testing. If I get chance I'll try some out. The challenge is when you see Warthox flying - how to make something better than he can demonstrate the Wii is capable!!

1 - yes - it is what it is. I could be wrong, but I recall in IMU there may be averaging of last 2 readings - might be worth checking. And yes - individual tuning for perfection. In practice I haven't really notice much difference when enabling / disabling features. Maybe the loops are still so fast that the overall averages are enough for humans...

2 - agree, it doesn't help. I have wondered about the multiple divisions / multipliers to get to end result.

3 - OK - I was looking at code. Could not see diagram on PC. Will take a look.

4 - OK - I don't vary batteries so maybe not see this

5+6 very rude! LOl ;) I do agree with you on the non linear element. However bear in mind that for practical flying, sometimes it is not best to have fast instantaneous change. that is why the D component in level PID was introduced - to provide a more controlled return to position accross the range. But we are talking ACRO here - so I am with you...

7 -OK - I understand what you say - I agree it does not affect D value - I was thinking of the impact of D value - this is how I was mentored for PID tuning. I care only for impact analysis - the effect of D is larger for larger error angles by inference of faster angular change. Importantly, I agree on end result as you put it - simply dampens fast angular changes. !
User avatar
shikra
 
Posts: 494
Joined: Wed Mar 30, 2011 7:58 pm

Re: V2.2 - ACRO PID implementation is wrong, right?

Postby Alexinparis » Tue Jun 11, 2013 4:44 pm

Hi,

Let’s focus first in the ACRO mode for understanding.
The scheme is mainly correct.

a small code could be:
Code: Select all
PTerm = rcCommand – gyroData* dynPcoef / 2^6

error = rcCommand * 2^6 / Pcoef – gyroData
errorI += error
If(gyroData>limit) errorI = 0
ITerm = (errorI / 2^7 * Icoef) / 2^6

DTerm = sum_last_3_delta(gyroData) * dynDcoef / 2^5

PID = P+I-D



1) comment says, that it takes into account the cycle time, but in fact, there's no such thing.


Where did you see this comment ?
I believe an accurate cycle time is very important for integration of gyro to get an accurate angle (in IMU part).
The theory says: it’s needed for I and D term, but I don’t believe it makes a lot of differences for ACRO PID.

2. why there is 32-bit division?

To minimize cycle time when possible (8bit computation vs 32bit)

why not replace with multiplication on constant and precalculate it on startup and P8 change?

Right,
There is still room for cycle time reduction here.
I expected a full fixed point algorithm, and a float * is probably faster here.


3. Why P8 is added to I component? also, see 4.


The theory:
error = command – measure
P = Pcoef * error

Here:
command = rcCommand/Pcoef
measure = gyroData / 2^6
error = rcCommand/Pcoef - gyroData / 2^6
=> P = rcCommand – gyroData* Pcoef / 2^6

The theory:
errorI = errorI + error*dT

Here:
errorI = errorI +error (no variable dT)
windup limit on errorI
I = errorI * Icoef

4. P8 for I component and P8dyn for P are in the feedback part of the loop, not the forward part. This means, that changing these variables would not only change the open loop gain, but would also alter the total system amplification (in this case - gyro scaling). I can't bear the fact, that when I increase P in GUI, the rate to stick position scaling is decreased. Is that done intentionally? I don't feel like it's a good thing.

Basically, I follow the PID scheme defined here:
http://en.wikipedia.org/wiki/PID_controller

What is surprising and introduces confusion is maybe command = rcCommand/Pcoef
I agree it could be changed for a constant stick feeling whatever the settings.

5. When dynamic P adjustment takes place (P8dyn is decreased) the scaling for P and I component becomes different - they start to try maintaining the different rate values. I don't think that it helps. Initial idea was to change not the open loop gain (as well as I and D), but just change the scaling corresponding to the throttle, right?

The basic idea behind dynP&dynD is to decrease the PID and sensor influence when huge command is required to tend toward PID = rcCommand. There is no math model behind, just empirical tests.
Iterm is not affected because:
/ Pcoef could lead to division by zero
Iterm is zeroed in case of huge command
Iterm impact is anyway limited regarding its weight in P I D

6 This not only shuts off the I component when rate exceeds some value, but also zeroes the accumulated I value. Why?

The I term is important in static position mainly to counter drift. It is useless in case of huge rotation rate.
The accumulated I is zeroed in order to “memorize” the position where quick stick stop order is required. With a remaining accumulatedI, I think there would be a small drift after each sudden rate deceleration.

7. D component doesn't take into account stick position at all. That's why it doesn't make the control sharper - it just tries to dampen fast angle changes of the body, and that's it.

Theory:
D = (error - previous_error)/dT

Here:
we estimate the command term of error in negligible in this equation.
See:
http://en.wikipedia.org/wiki/PID_controller
“In most commercial control systems, derivative action is based on PV rather than error.”
=> D = (measure – previous_measure) * Dcoef (no variable dT)
+ a little averaging over it
Alexinparis
 
Posts: 1546
Joined: Wed Jan 19, 2011 9:07 pm

Next

Return to Software development

Who is online

Users browsing this forum: happul3 and 4 guests