Tunable LFP filter for ACC with float implementation

Post Reply
marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Tunable LFP filter for ACC with float implementation

Post by marbalon »

Hi,
I've just try to tune my quadrocopter because on new soft 1.9 level mode can't be called stable :(

First of all I will provide my setup and my problem:

KDA 20-22L
Propellers GF 10”
Mystery 20A with quax firmware (fast response)
ADXL345 ACC
L3G4200D gyro

The main problem in new version is that my ACC is not filtered enough. This ACC is good in my opinion but output data need to be filtered. So with standard version (and default PID’s) my acro mode is perfect but stable mode is nervous. Its stays for a while and then fly to random direction but this looks like user input - just like little stick touch. In my opinion this is normal because I have non standard ESC with fast response and every bad value from ACC affected quadro behavior. So I need to filter my ACC data more, but the best If I can do this in fly time.

So I found good idea. First I changed current filter method to known method for LFP

global varioable in main file:

Code: Select all

static float lfpFactor = 0.27;


in IMU.pde

Code: Select all

  static float accTemp[3];

….

Code: Select all

 accTemp[axis] = accTemp[axis] * (1 - lfpFactor) + accADC[axis] * lfpFactor;
  accSmooth[axis] = accTemp[axis];


Then I added two lines to tune this value in flue time and read it leter.

after this line:

Code: Select all

    rcOptions = (rcData[AUX1]<1300)   + (1300<rcData[AUX1] && rcData[AUX1]<1700)*2  + (rcData[AUX1]>1700)*4
               +(rcData[AUX2]<1300)*8 + (1300<rcData[AUX2] && rcData[AUX2]<1700)*16 + (rcData[AUX2]>1700)*32;

I’ve added this

Code: Select all

 lfpFactor = 0.0005 * (rcData[AUX2] - 1000);


And I display this line in debug3 window.

So now I can fly and find the best lfpFator value.

Result? I found that my quad is realy flyiable with value 0.27, have no problem with drift and no problem with fast return to flat position.

In my opinion every user should enable Aux2 just for initial setup and then put this into code or EEPROM. After this Aux2-lfpFator relation can be disabled using ifdef.

I think we can’t use hardcoded values or defines in IMU.pde because non expert users can find this and don’t know how to find correct velue. With this method I found best value in 5min and try my quad in nonwind wether and can left my it about 1min without stick input and drift is about 5m!

This method is similar to AeroQuad and I think we should use similar. Every quadro setup: ACC, ESC and motor are different so user should have easy way to find good filter option.

What do you think guys?

Cheers,
Marcin.
Last edited by marbalon on Tue Nov 22, 2011 8:34 am, edited 2 times in total.

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Tunable ACC filter - another method to tune level mode.

Post by mahowik »

Hi Marcin,

I have found out the same, that new ACC LPF filter doesn't work as required. As I told before in another thread: Yes, this LPF has good precision/accuracy for small input values, BUT these small ACC values are vibrations as usual... than that vibrations will be integrated and as result we have a drift in random direction...

viewtopic.php?f=8&t=198&p=5016#p5016

New 1.9 LPF (started from MultiWii_dev_20111029)

Code: Select all

      accTemp[axis] = (accTemp[axis] - (accTemp[axis] >>4)) + accADC[axis];
      accSmooth[axis] = accTemp[axis]>>4;


So I have rolled back the MultiWii_dev_20111017 filter and found out that it works for LEVEL mode, because it's cutting off small ACC values (vibrations) and IMU calculates small and fast inclinations based only on gyro values BUT you know that gyro not so sensitive to vibrations in diff with ACC:

Code: Select all

accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) + accADC[axis] + ACC_LPF_FACTOR/2) / ACC_LPF_FACTOR; // LPF for ACC values

where ACC_LPF_FACTOR=12 is good for me...

As for your LPF I suppose it's also cutt of the small value changes. E.g. if ACC have a precision 4 for 1 degree (i.e. 1degree=4), and lfpFactor = 0.27, your implementation will take into account ACC values >=4.
E.g. 0.27*3=0.81f, where when it's converted to int it will be zero: (int)0.81f=0

So your implementation also doesn't take into account the small ACC values and this the reason why it's work perfect! ;)

thx-
Alex

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: Tunable ACC filter - another method to tune level mode.

Post by ziss_dm »

Hi mahowik,

I think, you missed this:

Code: Select all

static float accTemp[3];


As a result it actually has better precision, and not dropping bits. ;)

@marbalon
The 0.27 is pretty much 0.25 and this what we have in 1.9 but using integer math. Is there so big difference? :shock:
Can I ask you to compare Floating point implementation with factor 0.25 and integer one (stock 1.9). I'm still reparing my gears. :(

I think we can’t use hardcoded values or defines in IMU.pde because non expert users can find this and don’t know how to find correct velue. With this method I found best value in 5min and try my quad in nonwind wether and can left my it about 1min without stick input and drift is about 5m!
This method is similar to AeroQuad and I think we should use similar. Every quadro setup: ACC, ESC and motor are different so user should have easy way to find good filter option.

I think, you have valid point, but first we need to understand what caused major improvement. Is it floating point implementation or finelly tuned value. ;)
And if you interested, we already had some discussion about LPF here:
viewtopic.php?f=8&t=198&start=20

regards,
ziss_dm

marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Tunable ACC filter - another method to tune level mode.

Post by marbalon »

ziss_dm wrote:Hi mahowik,

I think, you missed this:

Code: Select all

static float accTemp[3];


Hmmm this line is already in first post...

ziss_dm wrote:@marbalon
The 0.27 is pretty much 0.25 and this what we have in 1.9 but using integer math. Is there so big difference? :shock:
Can I ask you to compare Floating point implementation with factor 0.25 and integer one (stock 1.9). I'm still reparing my gears. :(


Strange because I've tested default 1.9 implementation and can't fly with this. Symptoms I described in first post.

mahowik wrote:E.g. 0.27*3=0.81f, where when it's converted to int it will be zero: (int)0.81f=0


I can agree with this but you miss second pass because when you are using int implementation with bit operations you miss small values, but if you analyze this example:

ACC data 3, old value 0, lfpFactor = 0.27

if fist pass you will get

0*(1-0.27) + 0.27*3 = 0.81 -> then this will be converted to int and IMU gets 0

second pass with ACC data 3

0.81*(1-0.27) + 0.27*3 = 1.4013 -> IMU gets 1 !

So now after conversion to int get 1! So this is the main goal for float implementation. Please try guys this in your quads, I think you will see the difference.

Regards,
Marcin.

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: Tunable ACC filter - another method to tune level mode.

Post by ziss_dm »

Hi marbalon,

I will, but I cant right now.. ;)
You also can try to fly without LPF. Just increase GYR_CMPF_FACTOR, or even assign it to AUX2.. It is hard to describe difference by words, but it feels more "natural" for me.

regards,
ziss_dm

marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Tunable ACC filter - another method to tune level mode.

Post by marbalon »

ziss_dm wrote:Hi marbalon,

I will, but I cant right now.. ;)
You also can try to fly without LPF. Just increase GYR_CMPF_FACTOR, or even assign it to AUX2.. It is hard to describe difference by words, but it feels more "natural" for me.

regards,
ziss_dm

I will try. But I think we should not afraid floats for LFP because we can get perfect level mode, and I think users expect this for this mode ;) But to save CPU we can try to implement the same with big int32 values.

ps. I've changed a little topic subject.

Marcin.

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Tunable LFP filter for ACC with float implementation

Post by mahowik »

marbalon wrote:I can agree with this but you miss second pass because when you are using int implementation with bit operations you miss small values, but if you analyze this example:

ACC data 3, old value 0, lfpFactor = 0.27

if fist pass you will get

0*(1-0.27) + 0.27*3 = 0.81 -> then this will be converted to int and IMU gets 0

second pass with ACC data 3

0.81*(1-0.27) + 0.27*3 = 1.4013 -> IMU gets 1 !

So now after conversion to int get 1! So this is the main goal for float implementation. Please try guys this in your quads, I think you will see the difference.


good example... i understood this but was a lazy to describe... So it's drop the small values obviously...

Also I suppose you can get the same without float:

Code: Select all

accTemp[axis] = accTemp[axis] * (1 - lfpFactor) + accADC[axis] * lfpFactor = (accTemp[axis] * (100 -  lfpFactor * 100) + accADC[axis] * lfpFactor * 100)/100 


if lfpFactor in range [1..100] it will be

Code: Select all

(accTemp[axis] * (100 -  lfpFactor) + accADC[axis] * lfpFactor)/100 = (accTemp[axis] * (4 -  lfpFactor/25) + accADC[axis] * lfpFactor/25)/4


then with your value (lfpFactor = 27 ~= 25) it will be

Code: Select all

(accTemp[axis] * (4 -  25/25) + accADC[axis] * 25/25)/4 = (accTemp[axis] * (4 -  1) + accADC[axis])/4 


So i would like to say that is the same filter as was before MultiWii_dev_20111029. Talking about:

Code: Select all

accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) + accADC[axis]) / ACC_LPF_FACTOR;

where ACC_LPF_FACTOR=4

marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Tunable LFP filter for ACC with float implementation

Post by marbalon »

mahowik wrote:So i would like to say that is the same filter as was before MultiWii_dev_20111029. Talking about:

Code: Select all

accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) + accADC[axis]) / ACC_LPF_FACTOR;

where ACC_LPF_FACTOR=4


Ok, But I think it is wrong LFP implementation. Please try to fallow this example. accSmooth[old] = 0 and new data accADC = 3

accSmooth[axis] = (0 * (4 - 1) + 3) / 4 = 3/4 -> converted to int = 0
second pass looks the same because old value is 0

So I think the source of problem is bad LPF implementation, it have a problem with small values. if value is smaller then ACC_LPF_FACTOR IMU gets 0 and can't correct position.

Code: Select all

accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) + accADC[axis]) / ACC_LPF_FACTOR;


is not the same as:

Code: Select all

accTemp[axis] = accTemp[axis] * (1 - lfpFactor) + accADC[axis] * lfpFactor;


And another think is that if you browse internet for LPF implementation you will find the same like in my example.

Hmm I'm right or I miss something?

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: Tunable LFP filter for ACC with float implementation

Post by ziss_dm »

Hi,

Mathematically

Code: Select all

accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) + accADC[axis]) / ACC_LPF_FACTOR;

and

Code: Select all

accTemp[axis] = accTemp[axis] * (1 - lfpFactor) + accADC[axis] * lfpFactor;

are the same, assuming that lfpFactor = 1/ACC_LPF_FACTOR
So if you change accSmooth[axis] to be float you will get same result.

regards,
ziss_dm

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Tunable LFP filter for ACC with float implementation

Post by mahowik »

Hi Ziss,

Yes, it seems you are right again :)
If we take into account that accSmooth[axis] is float it will be the same LPF with high accuracy (like current 1.9).
BUT why in this case marbalon's version helps to keep level and avoid the drift?!

I have also made some test. I have added the cut condition to ignore small ACC values and it seems that level become some better with 1.9 LPF filter:

Code: Select all

accTemp[axis] = (accTemp[axis] - (accTemp[axis] >>4)) + accADC[axis];
accSmooth[axis] = accTemp[axis]>>4;

if(accTemp[axis] < k) {
  accTemp[axis] = 0;
}

where k=0..(1<<4)= 0..16
in my config with k=10 it was more stable...

marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Tunable LFP filter for ACC with float implementation

Post by marbalon »

Hi,
Ziss have right like always :) But I think it is more stable because have much more precision than default 1.9 LFP. I think when IMU use this function about 300 per sec. precision is important and maybe this is the difference.

Regards,
Marcin.

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Tunable LFP filter for ACC with float implementation

Post by mahowik »

hmmmm... and now I'm fully confused :)
first theory - we should reduce the precision
second one - we should increase the precision

BTW I have tried to increase precision by increasing form >>4 to >>6 (from 16 to 64).

Code: Select all

      accTemp[axis] = (accTemp[axis] - (accTemp[axis] >>6)) + accADC[axis];
      accSmooth[axis] = accTemp[axis]>>6;

And it's not working for me also...

thx-
Alex

marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Tunable LFP filter for ACC with float implementation

Post by marbalon »

Hmm why we should reduce precision? Maybe I miss something but I don't read all post about drift - most of new topics here is about drift and ACC ;)
I think we should have the most precision as possible but this should be filtered to to get correct values. But strange things that algorithm above is not working. It is similar idea, old value plus part of new...
But I'm happy now with my quads and just made my first (controlled) flips in acro ! ;)

Cheers,
Marcin.

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Tunable LFP filter for ACC with float implementation

Post by mahowik »

marbalon wrote:Hmm why we should reduce precision? Maybe I miss something but I don't read all post about drift - most of new topics here is about drift and ACC ;)

the answer is in my first post of this thread :) viewtopic.php?f=7&t=926#p5843
marbalon wrote:But I'm happy now with my quads and just made my first (controlled) flips in acro ! ;)

This filter only for level mode... or you mean that now you can be more free in acro with having perfect level? ;)

marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Tunable LFP filter for ACC with float implementation

Post by marbalon »

mahowik wrote:the answer is in my first post of this thread :) viewtopic.php?f=7&t=926#p5843

Ok I forgot about this. But I think we can ignore small values. We need to setup correct filter, then we can use even small values because this values will show correct quad position.

mahowik wrote:This filter only for level mode... or you mean that now you can be more free in acro with having perfect level? ;)


No I just really happy with my first flips and just want to share with someone ;)

Marcin.

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Tunable LFP filter for ACC with float implementation

Post by mahowik »

marbalon wrote:Ok I forgot about this. But I think we can ignore small values. We need to setup correct filter, then we can use even small values because this values will show correct quad position.


But what for that cases when you have a vibrations on board/PCB?! In this case small values it's noise...
Just to be in sync... My config has a cheapest props from HK and has a lot of vibrations on board... Or probably you are using some kind of mechanical damper to reduce the noise? How you you can evaluate your config/quadr vibrations with scale 1..10? :)

marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Tunable LFP filter for ACC with float implementation

Post by marbalon »

I have setup with KDA 20-22L motors and Gemfan propellers, but this motors are not good in my and my friend opinion. In some speeds it oscillate a lot and arm with vibrate. But in last days I've finally made dumpers with works!
Here is a picture

This is nylon screw, foam around, nut and termo tube. On the picture one is with termo and one without.

With this dumpers I can increase PID's a lot.

Marcin.

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: Tunable LFP filter for ACC with float implementation

Post by ziss_dm »

Hi,

Small values - usually small inclinations.

@mahowik
Can you explain again how incorrect transfer function of filter (due acumulative round-off errors) can reduce noise in the output of the filter?

What output of the filter you expecting, in case input is:
1
3
1
3
1
3
1
3
... etc

2 or 0?


regards,
ziss_dm

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Tunable LFP filter for ACC with float implementation

Post by mahowik »

ziss_dm wrote:Small values - usually small inclinations.


I agree but you know that ACC more sensitive for vibrations and this noise will be registered to small inclinations for IMU... then integrated etc...

ziss_dm wrote:Can you explain again how incorrect transfer function of filter (due acumulative round-off errors) can reduce noise in the output of the filter?


As I wrote before: this is because that filter implementation "skip" small input values (angles) in spite of frequency and IMU will calculate the angles based only on gyro values (in case of small changes)... and give the correct small angle changes (because gyro less sensitive for vibrations) for PID controller... particularly for "I" stabilization parameter...

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: Tunable LFP filter for ACC with float implementation

Post by ziss_dm »

Hi,

As I wrote before: this is because that filter implementation "skip" small input values (angles) in spite of frequency and IMU will calculate the angles based only on gyro values (in case of small changes)... and give the correct small angle changes (because gyro less sensitive for vibrations) for PID controller... particularly for "I" stabilization parameter

In short term yes, but CF eventually return it back to incorrect value, in our example to 0 instead 2. And I cannot see difference in noise level between constant 0 and constant 2. ;)

regards,
ziss_dm

Post Reply