MARG sensor fusion AHRS

[Syberian]

Hello,
I am already PMed to Alex in RCG but had no response.

I`ve implemented the MARG fusion algorithm into the Multiwii code and it works just great!
It is based on the Ciskje`s MARG source code and goes without magnetometer because I still haven`t one.
My setup is WMP+NK+ArduMEGA tricopter.
The cycle time stays unchanged (6000 +-16)
http://www.youtube.com/watch?v=rX_0BrbZwyI

The dark side of the code: it requires some gyroFactor tuning to match the gyro degree/sec/value ratio. I have tuned it for my WMP though.

The code is here:
//============================
Latest version:
ftp://syberian.dyndns.org/arduino/MW1pt7_MARG21.zip

All versions repository:
ftp://syberian.dyndns.org/arduino/
//==================
I put the yaw output into the 'heading' variable to see the yaw in the GUI
Since the MARG was implemented, the code would not fit into the smaller boards (Pro MINI and Nano). Just Arduino and Seeduino MEGA .
//===============
Main features of ver.2.0
-heading hold mode: when YAW stick released, the heading is held on its last position spotted before releasing the stick.
-full MARG fusion with magnetometer: uncomment one of this lines and install the corresponding mag:
/* I2C magnetometer */
//#define HMC5843
//#define HMC5883
If there are no magnetometer, there will be a general MARG IMU implementation selected. The magnetic field inclination was fully compensated.

- the heading hold mode can be activated via Tx by checking the MAG checkboxes in the GUI, no matter whether the MAG was installed. The LEVEL mode should be enabled too.
- the heading hold strength can be tuned by modifying the YAW P field. This field serves as usual P factor in the 3D mode and when the YAW Tx stick was inclined.
- the firmware was tested on the ground for motors reaction and sensitivity. I cannot fly it yet because my tricopter was damaged.
//===========================

I need some testers to figure out different gyroFactor values.

Also it`ll be cool to integrate this fusion into the MW on the constant or selectable basis.
I am ready to develop it further but don`t want to split from the mainstream.
Thanks.

[Hamburger]

interesting.
Without diving into the code, what is the underlying idea of the MARG sensor fusion AHRS?
Hamburger

[Syberian]

I am not a mathematician but a good embedded programmer Thus cannot explain what is a quaternion or Euler transform, but it works.
I saw a video from this post and was pretty inspired:
http://www.rcgroups.com/forums/showpost ... ount=11414

I was not satisfied with the current IMU approach because am flying FPV in autolevel mode with an artificial horizon indicator fed by MultiWii IMU.
This is a list:
1) It does not tolerate long forward flights and turns
2) When doing some maneuvers (e.x. yaw while tilted) the IMU goes crazy and is doing strange things
3) It is working on the angles up to 25 degrees and doesn`t work while inverted
4) It doesn`t involve the yaw calculations
5) The magnetometer Heading hold routine doesn`t recognise the magnetic field inclination.

Also I am planning to develop the GPS RTH mode. Without reliable attitude and heading hold it will be unreachable.

[Hamburger]

thank you. I have a basic understanding of quaternions and euler trafo mean (and its limitations, theoretically).
I would like to give it a try, but as I am currently working on advanced battery power meter in software, it will be a challenge to keep both developments up to date at the same time.
You mentioned some configuration, that needs to be done to one's specific sensors.
Is there a step by step how to proceed?

Hamburger

[Syberian]

2.0 version for 1preter7
...moved to the first post...

[Hamburger]

cool. I see on your ftp site you already have a version 2.1.
Fast progress - I will be busy till end of next week - am very interested to give it a try.

Hamburger

[ciskje]

Great Syberian, I don't understand only why you can't put it in a normal arduino code is not so big.

[jhoexp]

Hey, great news Syberian!!
Tried your code with my freeimu 0.1 (itg3200- adx345 - 5843) made by F. Varesano (Fax8).

I had to change the adress of my adxl345 (now is 0x6A write register, 0x7B read) and directions, but now it's working ok.
The magnetometer line and quadrocopter orientation sketch are not displayed on the configurator...is it normal?

How can I help you with gyro ratio, what should i mesure?
Antway, it's looking good so far..cycle time is 3600 @400Khz. Any suggestion before flying?

[Syberian]

Hi,
There was a bug in the MW code not allowing to display AHRS data (and moving copter too) when a non-Wii accelerometer is used.
Fixed.
ftp://syberian.dyndns.org/arduino/MW1pt7_MARG21.zip

Please don`t fly it until the GUI indication and gyroFactor was tuned!

I`ll describe the tuning process later. There can be gyro and accel axes differences between Wii and other chips. I need to sort them out.

[jhoexp]

Ok Syberian, update to your 2.1 code and now the copter is moving in the gui.(not perfectly, but it's moving)
The magnetometer value and line are showing, but it's continuosly drifting.

Anyway the code is 19Kbytes, so you can compile it in arduino mini (with 328) or nano too.

[Syberian]

jhoexp
Thanks for your observations.
What gauges are you using? WMP+NK or ITG,BMA etc...
Can you please provide some data about the axis movement character on the GUI main chart?
I need it to compare with my setup.
Thanks in advance

For Example:

1. The copter stays level
ACC_Z above 0
others are zero

2. Tilt the copter forward
ACC_Z goes down
ACC_PITCH goes up
GYRO_PITCH goes up first
others are steady

3. Roll the copter right
ACC_Z goes down
ACC_ROLL goes up
GYRO_ROLL goes up first

4. Rotating the copter clockwise (YAW)
GYRO_YAW goes up
others are steady

[ziss_dm]

Hi Syberian,
This is a nice catch! Could you please report it separately?

My first impressions after fly tests:
1) Had to re-adjust PIDs, even in acro mode.. Probably due increased cycle time: from 2400 to 3600. But my quad is quite sensitive, for some reason. Attempt to make flip almost failed..
2) Heading without magnetometer is quite useless. Gyro drift is not corrected, and you can archive same result with "I" value on the yaw PID. In my case it just started wobbling like crazy..
3) Banked turns: It still going crazy. No miracle here. Without magnetometer the acceleration vector is not corrected. The original WMC code has a fork of "trusted" acc values and it really heps. You probably should implement something similar.

The Gyro Factor for ITG3200 looks like should be like this:

Code: Select all

gyroFactor = deltaTime * ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 0.75);

The easiest way to check it is to set Kp and Ki to zero. In this case algorithm switches to the "Gyro only" mode and you can easily to check angles.

I think, you should better protect algorithm from divisions by zero and overflows. Once integral error overflows (zero values from ACC, for example) it loks up and never return to working state. You can do something like this:

Code: Select all

// normalise quaternion
    norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    if (uint16_t(norm * 100) < 1) {
      q0 = 1.0f;
      q1 = 0.0f;
      q2 = 0.0f;
      q3 = 0.0f;
      exInt = 0.0f;
      eyInt = 0.0f;
      ezInt = 0.0f;
    } else {  
      q0 = q0 / norm;
      q1 = q1 / norm;
      q2 = q2 / norm;
      q3 = q3 / norm;
    }  

The conversion to the Euler angles from quaternion has standard issue of flipping Roll, when you go inverted. Probably nothing you can do about it without implementation of the angular velocity matrices and all that jazz.. I think better idea would be to use quaternion in PID regulator without conversion to the Euler angles. (in case we really need to make it work inverted.)

Regards,
ziss_dm

[Syberian]

Hi ziss_dm
Do you have a bunch of surplus quads? I`ve asked NOT to fly with it yet!
If you flown the 2.0 version with ITG and BMA in the autolevel mode - it means you flown it without any gauges at all. They doesn`t feed the AHRS module. Good skills though

The same question goes to you: can you please provide the data as described in my previous post? Use version 2.1.
P.S. your gyroFactor formulae seems to be true with this AHRS. But it is a subject to check.


===
Just saw your addition. Thanks!
You are found by yourself the gyro axis placement to feed to the AHRS. The same thing should be done with the ACC axes: Make the gyro inputs =0, increase the Kp to 1000 and tilt the copter while looking at the GUI arrows.

===
Perhaps I have to capture a new video how the latest version works for me. It acts very precise and have the tail drift about 2 degs per minute.

[ziss_dm]

Hi Syberian,
I have updated my previous post with info how I checked Gyro..
And I have fixed lockup issue before flight My point was: Flying characteristics are changed even in acro mode. So it is better to recheck PIDs again

PS: Why you decided to drop LPF in latest version(s)?

Regards,
ziss_dm.

[ziss_dm]

Hi Syberian,

I'm usually setting gx,gy,gz to zero (no gyro input at all), just to check "rate" as well..

Regards,
ziss_dm

[jhoexp]

Hi Syberian, i am using a Freeimu 0.1 (itg3200- adxl345 - hmc5843) made by F. Varesano (Fax8).
The orientation of the sensor is different from default, so I matched the gyro and acc direction. Rolling right i get positive values on both gyro and acc. Tilting forward and jawing clockwise is positive too.
ACC_z is 250 at rest, after calibration, it goes down when i tilt forward or backward.

The lines and values are all OK, but the copter attitude is strange. For example when i fast roll it 90° right it overshoots and then sloooowly goes back to the correct angle.
Jawing is the same, it's very slow to regain the correct heading bouncing around it very slowly.

[ziss_dm]

Hi,

It also would be interesting to collect comparicing reports for AHRS algorithm and this one getEstimatedAttitude: Idea (Does not have a name ). This would help us to make decision, which one is more suited. And little competition is always helps.

Regards,
ziss_dm

[Syberian]

jhoexp
You have to tune the gyroFactor divisor:

#if defined(ITG3200)
gyroFactor = deltaTime/670e6; //empirical

Increase the 670 (not touching 'e6') with the steps of 20-50 until it stops overshooting. It shouldn`t undershooting too.

========
I have rechecked my GUI charts. They are confirmed things written above:

1. The copter stays level
ACC_Z = 200, others are zero

2. Tilt the copter forward
ACC_Z goes down, ACC_PITCH and GYRO_PITCH goes up

3. Roll the copter right
ACC_Z goes down ACC_ROLL and GYRO_ROLL goes up

4. Rotating the copter clockwise (YAW)
GYRO_YAW goes up


jhoexp
Your axises are pretty the same, thus all you need is tune the gyroFactor.

ziss_dm
What criteria will be used for the comparison?


PS English is not my native language, although I didn`t using any translator. Does someone understands me clearly?

[jhoexp]

Hi,

It also would be interesting to collect comparicing reports for AHRS algorithm and this one getEstimatedAttitude: Idea (Does not have a name ). This would help us to make decision, which one is more suited. And little competition is always helps.

Regards,
ziss_dm

I have tried your modified algorithm, but can't display acc and copter attitude in configurator. Everything is working in regular 1.7preter.
Can you check?

[jhoexp]

Syberian, tried some settings... magneto code is not working good and i think it's the main problem with the weird attitude showed in configurator.
If you disable the magneto everything start to work as it should, and i think that the 670 gyro factor is good (maybe a little less?). I tried to tune Kp and Ki but i am not sure if it's something i can do observing the results on configurator.

Without the magneto with small angle variations it converges very fast to the correct angle. Large variation and repeated movement in all direction leave it a bit disoriented for (at worst) a few seconds, than the correct angle is reached.

[Syberian]

jhoexp

Well, I must confess
I have NO magnetometer at all. It is still in transit to me. I cannot tune the AHRS using MAG without having one. There are the same axis placement problem which still has to be solved.

Re the right attitude convergency. It is necessary to tune the gyroFactor that way it will reach the desired attitude from the first try, without further slow approaching.
It is a good step to tune with YAW rotation: Place the board (or the copter itself) at the level position. Turn on the system and wait for MAG=0. Turn the board on the yaw axis at 90 degrees clockwise and look at the MAG value. If the MAG shows more than 90 => increase 670e6. If lower => decrease the value.
To achieve 90deg yaw, I am aligning the arduino board edges to the table edge. Try not to roll or tilt the board at this time.

Ki and Kp shouldn`t be tuned for now because they are cheating the results.

[ziss_dm]

Hi Syberian,

This could probably speed-up algorithm a litle bit:
http://beyond3d.com/content/articles/8

Code: Select all

float InvSqrt (float x){
    float xhalf = 0.5f*x;
    int i = *(int*)&x;
    i = 0x5f3759df - (i>>1);
    x = *(float*)&i;
    x = x*(1.5f - xhalf*x*x);
    return x;
}

And fast way to check for NAN/INF:

Code: Select all

  if (norm != norm) {
   ... 
  }  

Fast atan2 approximation (returns angles):

Code: Select all

int16_t _atan2(float y, float x)
{
  #define fp_is_neg(val) ((((byte*)&val)[3] & 0x80) != 0)
  float z = y / x;
  int16_t zi = abs(int16_t(z * 100)); 
  int8_t y_neg = fp_is_neg(y);
  if ( zi < 100 ){
    if (zi > 10) 
      z = z / (1.0f + 0.28f * z * z);
    if (fp_is_neg(x)) {
      if (y_neg) z -= PI;
      else z += PI;
    }
  } else {
    z = (PI / 2.0f) - z / (z * z + 0.28f);
    if (y_neg) z -= PI;}
  z *= (180.0f / PI * 10); 
  return z;
}

Regards,
ziss_dm

[Syberian]

Thanks, ziss_dm

Though I didn`t get how ever it could return 'true': (m !=m)?
It`ll be a simpler way to compare with zero to prevent div by 0.

[ziss_dm]

Hi,

NAN not equal to anything..

regards,
ziss_dm

[Syberian]

If the 'not equal' statement is using a bytewise comparison, those 2 NaN numbers will be absolutely equal.
Second, if we already got a NaN number - the erroneous division by zero already has take a place. Shouldn`t we avoid that?

Also if 'norm=0' that condition will not work too, thus the quaternion will be damaged.

[andv]

Any news on this? Sounds really interesting to integrate Fabios (and others) work into MultiWii. How much does it hurt overall performance (cycle time etc)?

[jhoexp]

I stopped testing the marg release, but will be glad to reapply if Syberian and Ziss put on a working release to start with.
The magnetometer integration was far off for any flight test, while the gyro + acc new code was working good.... maybe it's time to isolate the problem and start again with the sensor fusion.

[Sebbi]

I am currently testing a MARG implementation, is somebody out there still interested to get this working?

Sourcecode (for current 2.1 MultiWii):
https://github.com/sebastianherp/MultiW ... MENTAL.ino

It works in the GUI, but the copter doesn't seem to stay level for very long and doesn't come back to level as fast as before. It could be a PID thing as I haven't adjusted them yet and the default attitude estimation algorithm was/is very bad for angles > 45 degrees.

Another downside will be that MARG uses considerably more memory (+3650 bytes currently, but could likely be improved).