Position Hold using Accelerometers global vectors

[Magnetron]

After the great work made by mahowik with barometric altitude hold routine released in R1129 a new very important concept was introduced: accelerometer global vector. In this case only on Z axis but I think that this can be calculated on X and Y axis also. So I would to try with mahowik to obtain this result that can be used to a new position hold routine without using a GPS, just like NAZx do.
Mahowik, could we try to implement the same on X and Y axis?
Stay tuned...

[alexia]

ouaaaa great project magnetron!

[mahowik]

Hi Magnetron!

Strange but it seems that the same thoughts can be catched at the same time in the different minds )))
I got the same idea on the beach during my vacation

Actually theoretically it possible to improve GPS position hold by using ACC but i don't think that is possible to do without GPS at all, because after the integration of acc data we need to correct accumulated error... in this case by GPS, otherwise you will get position hold with drift...

As good start we can improve precision of the GPS velocity by ussing ACC and check this in air, because according to my experience a lot of theoretical things so far from real practice

Code: Select all

////////////////////////////////////////////////////////////////////////////////////
// Calculate our current speed vector from gps position data
//
static void GPS_calc_velocity(){
  static int16_t speed_old[2] = {0,0};
  static int32_t last[2] = {0,0};
  static uint8_t init = 0;
  // y_GPS_speed positve = Up
  // x_GPS_speed positve = Right

  if (init) {
    float tmp = 1.0/dTnav;
    actual_speed[_X] = (float)(GPS_coord[LON] - last[LON]) *  GPS_scaleLonDown * tmp;
    actual_speed[_Y] = (float)(GPS_coord[LAT]  - last[LAT])  * tmp;
  
    actual_speed[_X] = (actual_speed[_X] + speed_old[_X]) / 2;
    actual_speed[_Y] = (actual_speed[_Y] + speed_old[_Y]) / 2;
  
    speed_old[_X] = actual_speed[_X];
    speed_old[_Y] = actual_speed[_Y];
  }
  init=1;

  last[LON] = GPS_coord[LON];
  last[LAT] = GPS_coord[LAT];
}

p.s. I will look how get gloabl X and Y axis...

thx-
Alex

[timecop]

proper gps, eg. ubx already provides gps velocity vectors in xyz

[Magnetron]

Mahowik your code for alt hold really works! I would to obtain some effects in x and y axis calculating velocity like in z axis your routine do, so we must obtain actual vel (debug2) behaviour on x and y, then using it to compensate position, then we can try data fusion with gps data by complementary filter, like baro routine. We must try to obtain velocity on x and y axis with global vector.
My main idea is to implement a new stabilization position routine, not level, but position, with results of calculation, using a new pid we must compensate collective command to obtsin reaction on x-y... On z we obtain this result by your baro routine.

I am out for work so I can not try, could you try to obtain global vector integrating acc+gyro?

[Magnetron]

I have analized behaviour of accelerometers data. I think that the first think is to reintroduce moving average on accs due to presence of noises.
So we must obtain a vector that indicate direction and intensity of movement independet of inclination.
Mahowik could you implement a simple calculating routine to obtain this vectors?

GPS is poor precise, we must integrate acc data with high weight with complemetary filter.

[hkubota]

So we must obtain a vector that indicate direction and intensity of movement independet of inclination.

I am curious: how do you want to calculate this data?
With GPS, you get the move vectors (according to TC, and it makes sense the GPS system has that information, so I believe he's right).

Where do you want to canculate this data from without GPS? Can you maybe write some actual code so we can follow the logic you have in mind? If you have GPS, there's no need to calculate this. Without GPS you have no reference absolute data like you have with the barometer.

But I might just not see the simple and elegant solution here, so I am not saying it's impossible. I just don't get it.

[shikra]

@hkubota

To summarise
Measures any tilt and how long its tilted, then puts compensation to return it back to the original position. Not just level though - maybe a little more to return it back before applying level.
Will still drift with wind, But coupled with GPS hold it should make horizontal positioning better.

I haven't used a Naza, but I've been told that this is a nice visible feature of it....

[Magnetron]

@hkubota

To summarise
Measures any tilt and how long its tilted, then puts compensation to return it back to the original position. Not just level though - maybe a little more to return it back before applying level.
Will still drift with wind, But coupled with GPS hold it should make horizontal positioning better.

I haven't used a Naza, but I've been told that this is a nice visible feature of it....

Right.
Mahowik routine calculates velocity on z axis using only accs + gyros (variable on debug2), we must obtain the same result on x and y axis so after that we can use this vectors to compensate gps data (but we can use without also...)

[aymeric]

Hi,

@hkubota:
Just guessing (read a bit on Mahowik's implementation), but I'd say you need to integrate acceleration to get velocity, and integrate velocity to get position.
So position hold is "just" maintaining 0 velocity on all axes, using baro to compensate vertical errors and using GPS to compensate gyro/acc drifts (horizontal situation).
It's also a way to filter GPS errors.

More than just "position hold", it gives ability to estimate position quite precisely between GPS fixes, thus opening a door to a nice navigation system.
That's the way complex aircraft get their position : see INS, IRS, AHRS, etc.

Cheers.

[mahowik]

@hkubota

To summarise
Measures any tilt and how long its tilted, then puts compensation to return it back to the original position. Not just level though - maybe a little more to return it back before applying level.
Will still drift with wind, But coupled with GPS hold it should make horizontal positioning better.

I haven't used a Naza, but I've been told that this is a nice visible feature of it....

Right.
Mahowik routine calculates velocity on z axis using only accs + gyros (variable on debug2), we must obtain the same result on x and y axis so after that we can use this vectors to compensate gps data (but we can use without also...)

We can't to use it without GPS, because we need some data to correct integration errors...
I know one dirty way how to keep integrated velocity near zero without correction by some real data (GPS or baro), but it will work only for short periods... the idea - you just remove second part of the complementary filter and use BIG coefficient for main part... e.g.
vel = vel * 0.985f + baroVel * 0.015f;
and if you will use just
vel = vel * 0.985f
velocity will be reduced on each cycle iteration... so for short periods it will work also... but I don't like this

so about solution for PH... i see the following:
1) get global x, y axis... actually we need to get global x, y vectors... I think it's possible to get by rotation of global Z vector (EstG.V in our case)
2) get projections of ACC vector (accLPFVel in our case) to global x, y vectors
accX = A * X / |X| where X is global x vector
accY = A * Y / |Y| where Y is global y vector
note: we don't need to substruct 1G here (acc_1G in our case)
3) make an integration:
velX+= accX * accVelScale * dTime;
velY+= accY * accVelScale * dTime;
4) apply Complimentary Filter
velX = velX * K + gpsVelX * (1-K); where K=0..1 (actually 0.90-0.99)
velY = velY * K + gpsVelY * (1-K);

As for now I'm back from vacation an little bit busy but going to to play with this in details...

thx-
Alex

[mahowik]

Hi,

@hkubota:
Just guessing (read a bit on Mahowik's implementation), but I'd say you need to integrate acceleration to get velocity, and integrate velocity to get position.
So position hold is "just" maintaining 0 velocity on all axes, using baro to compensate vertical errors and using GPS to compensate gyro/acc drifts (horizontal situation).
It's also a way to filter GPS errors.

More than just "position hold", it gives ability to estimate position quite precisely between GPS fixes, thus opening a door to a nice navigation system.
That's the way complex aircraft get their position : see INS, IRS, AHRS, etc.

Cheers.

In theory yes it's simple to do double integration of acc values to get relative heigh or position... on practice In my experience it possible to keep only one integration even with LPFs and CF, because of vibrations, sensor drifts and not scaled sensor values...

[hkubota]

We can't to use it without GPS, because we need some data to correct integration errors...

My thinking too.

so about solution for PH... i see the following:
1) get global x, y axis... actually we need to get global x, y vectors... I think it's possible to get by rotation of global Z vector (EstG.V in our case)
2) get projections of ACC vector (accLPFVel in our case) to global x, y vectors
accX = A * X / |X| where X is global x vector
accY = A * Y / |Y| where Y is global y vector
note: we don't need to substruct 1G here (acc_1G in our case)
3) make an integration:
velX+= accX * accVelScale * dTime;
velY+= accY * accVelScale * dTime;
4) apply Complimentary Filter
velX = velX * K + gpsVelX * (1-K); where K=0..1 (actually 0.90-0.99)
velY = velY * K + gpsVelY * (1-K);

Now it makes sense!

Would that, together with some absolute reference like GPS not be what a Kalman filter does? That would estimate the current location based on an absolute reference (start location or GPS) and estimating location based on collected sensor data (gyro, acc, over time)?

That being said, I still think a GPS with 10Hz update and motion vector output does better, but I can see a use in case GPS drops out (e.g. city environment, tunnels, radio interference) and then bridging the lack of absolute references is a good thing.

Anyway, got it now. Thanks for the explanation. I certainly will watch this thread. And read a bit more about Kalman filters in general and if and where it's used in the various FCs.

[Magnetron]

@hkubota

To summarise
Measures any tilt and how long its tilted, then puts compensation to return it back to the original position. Not just level though - maybe a little more to return it back before applying level.
Will still drift with wind, But coupled with GPS hold it should make horizontal positioning better.

I haven't used a Naza, but I've been told that this is a nice visible feature of it....

Right.
Mahowik routine calculates velocity on z axis using only accs + gyros (variable on debug2), we must obtain the same result on x and y axis so after that we can use this vectors to compensate gps data (but we can use without also...)

We can't to use it without GPS, because we need some data to correct integration errors...
I know one dirty way how to keep integrated velocity near zero without correction by some real data (GPS or baro), but it will work only for short periods... the idea - you just remove second part of the complementary filter and use BIG coefficient for main part... e.g.
vel = vel * 0.985f + baroVel * 0.015f;
and if you will use just
vel = vel * 0.985f
velocity will be reduced on each cycle iteration... so for short periods it will work also... but I don't like this

so about solution for PH... i see the following:
1) get global x, y axis... actually we need to get global x, y vectors... I think it's possible to get by rotation of global Z vector (EstG.V in our case)
2) get projections of ACC vector (accLPFVel in our case) to global x, y vectors
accX = A * X / |X| where X is global x vector
accY = A * Y / |Y| where Y is global y vector
note: we don't need to substruct 1G here (acc_1G in our case)
3) make an integration:
velX+= accX * accVelScale * dTime;
velY+= accY * accVelScale * dTime;
4) apply Complimentary Filter
velX = velX * K + gpsVelX * (1-K); where K=0..1 (actually 0.90-0.99)
velY = velY * K + gpsVelY * (1-K);

As for now I'm back from vacation an little bit busy but going to to play with this in details...

thx-
Alex

RIGHT ALEX, this is the main solution.
Now we must implement routine for velX and velY and integrate.
But I have not understand your point no.1...

[mahowik]

1) get global x, y axis... actually we need to get global x, y vectors... I think it's possible to get by rotation of global Z vector (EstG.V in our case)

RIGHT ALEX, this is the main solution.
Now we must implement routine for velX and velY and integrate.
But I have not understand your point no.1...

As we have global Z vector, theoreticaly it's possible to get global X vector by rotation of Z vector for 90 degree around Y axis and global Y vector by rotation of Z vector for 90 degree around X axis...
http://en.wikipedia.org/wiki/Rotation_%28mathematics%29
http://en.wikipedia.org/wiki/Rotation_matrix
So i'm looking for simplest way how to do that...

[Magnetron]

very good!!!

[Magnetron]

I tryed to develop the routine to calculate global axis on x and y but I have not reached the target...
and You?

[Sebbi]

I tryed to develop the routine to calculate global axis on x and y but I have not reached the target...
and You?

Isn't that a very simple problem? You have the orientation of the copter (pitch/roll/yaw) and an acceleration vector. So just apply a rotation matrix (wikipedia reference: http://en.wikipedia.org/wiki/Rotation_m ... _rotations or http://en.wikipedia.org/wiki/Euler_angl ... rientation) ... and you get "earth frame" acceleration which can be integrated to get velocities.

[mahowik]

Isn't that a very simple problem? You have the orientation of the copter (pitch/roll/yaw) and an acceleration vector. So just apply a rotation matrix (wikipedia reference: http://en.wikipedia.org/wiki/Rotation_m ... _rotations or http://en.wikipedia.org/wiki/Euler_angl ... rientation) ... and you get "earth frame" acceleration which can be integrated to get velocities.

I had not time for investigation yet but if it's simple for you plz. provide solution with good performance
plz. don't forget that we are still using AVR 8-bit chips but not stm32 at least...

thx-
Alex

[Sebbi]

Good performance might be a problem. Define good There are quaternion implementations to get a more accurate orientation which results in better performance (but since it is more accurate it is still slower than the MultiWii implementation).

Here is some pseudocode how to do it with a rotation matrix:

Code: Select all

cr = cos(roll);
cp = cos(pitch);
cy = cos(yaw);
sr = sin(roll);
sp = sin(pitch);
sy = sin(yaw);

earth_acc_x = (cp*cy) * acc_x + (sr*sp*cy - cr*sy) * acc_y + (sr*sy + cr*sp*cy) * acc_z;
earth_acc_y = (cp*sy) * acc_x + (cr*cy + sr*sp*sy) * acc_y + (-sr*cy + cr*sp*sy) * acc_z;
earth_acc_z = (-sp) * acc_x + (sr*cp) * acc_y + (cr*cp) * acc_z;

That should transform the measured acc values to ones that are in earths frame.

P.S.: I am not sure about the orientation of acc_x, acc_y and acc_z ... so experiment

[Sebbi]

Example:

Pitch is 45 degrees, roll and yaw are 0 degrees. The accelerometer will measure something like acc_x = -362, acc_y = 0 and acc_z = 362 (512 being 1 G). One pseudocode execution later you get:

Code: Select all

cr = cos(roll); //=1
cp = cos(pitch); //=sqrt(2)/2
cy = cos(yaw); //=1
sr = sin(roll); //=0
sp = sin(pitch); //=sqrt(2)/2)
sy = sin(yaw); //=0

earth_acc_x = sqrt(2)/2 * -362 +  0 + sqrt(2)/2 * 362 [b]= 0[/b];
earth_acc_y = 0 + 0 + 0 [b]= 0[/b];
earth_acc_z = -sqrt(2)/2 * -362 + 0 + sqrt(2)/2 * 362 [b]= 512[/b];

Does that work for you? Performance should be ok, but cycle time will obviously increase ...

[Sebbi]

A small observation from your altitude hold code:

Code: Select all

float invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G; 

That's from your altitude code, right? I do get what this is supposed to do, but it seems to be the wrong approach. You multiply a smoothed acceleration with the estimated gravity vector, normalise it and then subtract 1G? That is incorrect and doesn't calculate what you think it does. It will get the dynamic acceleration but that could be in any direction, e.g. if the copter was accelerated to the left or up ... the results would be the same. It works while the copter is level, but as soon as pitch/roll changes this part of the algoithm will likely fall apart ...

Using the rotation matrix code from above will result in correct values for all accelerations.

Sorry, I don't mean to rain on your parade and if this is from old code and has been fixed since, I apologise (and please point me to recent code).

[mahowik]

The code is correct but you forgot to read this

// projection of ACC vector to global Z, with 1G subtructed
// Math: accZ = A * G / |G| - 1G
float invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;

It sees only vertical movements because it's projection of ACC vector to global Z which is always point to the ground... you can just check this in GUI (i did it 100+1 times ) on debug[0] and debug[2] (debug[0] = accZ, debug[2] = vel)

thx-
Alex

[Sebbi]

The code is correct but you forgot to read this

// projection of ACC vector to global Z, with 1G subtructed
// Math: accZ = A * G / |G| - 1G
float invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;

It sees only vertical movements because it's projection of ACC vector to global Z which is always point to the ground... you can just check this in GUI (i did it 100+1 times ) on debug[0] and debug[2] (debug[0] = accZ, debug[2] = vel)

thx-
Alex

I don't think you understand what EstG really is. You are right, it always "points" toward the ground, but so does EVERY acc measurement, since gravity is the main acceleration that is measured. EstG is basically the same as the raw ACC values (except for when the copter is rotating), so you are basically calculating "|G| - 1G" ... the dynamic acceleration which can be in ANY direction. Since baro hold is mostly active when the copter is also level and not flying around this SEEMS to work, but it really doesn't ... that why you probably introduced a throttle_angle_correction (which btw is also calculated incorrectly)

If you want a projection towards the vector (0, 0, 1G) then use that vector and not EstG. You will find that a projection is not what you want. You want everything rotated with a rotation matrix ... only then you'll get correct global (earth frame) accelerations.

A quick example:
- The copter is in angle mode and leveled out. There is no wind so it flies perfectly still in the air (assume perfect calibration and no vibrations)
- EstG will read (0, 0, 512) with 512 = 1G
- Now some wind accelerates the copter to one side
- EstG will start to change slowly (it is smoothed with a factor of 400), lets assume after half a second it reads (45, 0, 512)
- accLPFVel will see those changes too, but faster since it's smoothed with a factor of only 10, lets assume after half a second it reads (50, 0, 512)
- now put that into your formula, what do we get?
- acc_z = (50*45 + 0*0 + 512*512) / sqrt(45*45 + 0*0 + 512*512) - 512 = 2,41

An incorrect result, or not?

second edit:
if you use (0, 0, 512) for the projection instead of EstG than the above example would result in a acc_z value of 0, which is correct.

[mahowik]

Just check this in GUI first for horizontal x,y and vertical z movements with any degree/inclination... after that we can continue our discussion

[Sebbi]

I edited my reply with an example. The gui updates too fast to see anything ... if you still wont believe me after my example, I can use a logger to "prove" it to you

[mahowik]

I edited my reply with an example. The gui updates too fast to see anything ... if you still wont believe me after my example, I can use a logger to "prove" it to you

Your example correct but:
- it's true if you have constant acceleration in long time period... for short time EstG and ACC vectors are different because of big gyro factor in gyro/acc CF, so it's possible to project acc vector to EstG
- we correcting that errors by applying deadband and using CF for velocity

So for real life it works with any inclinations but yes there are small inaccuracies can occur...

And if you can provide more precise but real solution (not on the paper) it would be great!

I have a lot of examples in my short experience when formulas doesn't work in real flights Because of temperature drifts, vibrations, not scaled data sensors...

thx-
Alex

[Sebbi]

EstG and ACC vectors are different because of time lag, not because they are a different thing. A projection might be somewhat correct in short timeframes and with litte motion/rotation (just like the rotateV function which, well ... lets not talk about it ... but it kind of works for supersmall angles). Introducing errors and then correcting them in a different part of the application is a bad practice :/

A precise solution is in my reply above. I even posted an example ... using a rotation matrix it will always give correct global acceleration in every direction (that's what you wanted and asked me to provide for you, remember?). There is still the problem with gimbal lock, but lets say if a copter is that much tilted you have other problems to worry about. Please take a look at my pseudocode and the example ...

[Sebbi]

I took the liberty to fully implement it, so I can sleep well and don't have to dream about rotation matrices

Here you go (place it at the end and inside of getEstimatedAttitude):

Code: Select all

#define CALC_GLOBAL_ACC  
#if defined(CALC_GLOBAL_ACC)
  // Get global (earth frame) acceleration
  // ======================================
  // this code adds up to 1000 ms to the cycle time and 930 bytes to the compiled binary
  // knows bugs:
  // - lag or no lag but settle issues with different acc-vectors used ... 
  //   looks like pitch/roll aren't as realtime as everybody believes?
  // - significant pitch AND roll causes MultiWii to calculate wrong pitch/roll values ...
  //   earth_acc will obviously report wrong values in that case (example: with a quadX configuration
  //   place the copter on one of its arms, z-axis pointing sideways. MultiWii will report pitch/roll of both 90°
  //   but in reality that placement equals a pitch and roll of only 45°)
  //   fix is replacing
  //     angle[ROLL]  =  _atan2(EstG.V.X , EstG.V.Z) ;
  //     angle[PITCH] =  _atan2(EstG.V.Y , EstG.V.Z) ;
  //   with
  //     angle[ROLL]  =  _atan2(EstG.V.X , sqrt(EstG.V.Y*EstG.V.Y + EstG.V.Z*EstG.V.Z)) ;
  //     angle[PITCH] =  _atan2(EstG.V.Y , sqrt(EstG.V.X*EstG.V.X + EstG.V.Z*EstG.V.Z)) ;
  // - gimbal lock is a problem
  
  
  // heading is not really precise only whole degrees
  // need to convert to rad
  #define deg2rad(x) (float)(x*PI/1800.0f)
  float cr = cos(deg2rad(angle[ROLL]));
  float cp = cos(deg2rad(angle[PITCH]));
  float cy = cos(deg2rad(heading*10));
  float sr = sin(deg2rad(angle[ROLL]));
  float sp = sin(deg2rad(angle[PITCH]));
  float sy = sin(deg2rad(heading*10));
  
  // MultiWii implementation doesn't follow standard literature so axis are switched and inverted
  // I assume this is because of the original Wii hardware
  
  // use acc values directly (lags, because angles are lagging behind reality)
  #define USE_ACC_FOR_EARTH_ACC 1
  
#if USE_ACC_FOR_EARTH_ACC == 1
  axis = 1; int16_t acc_x = -ACC_VALUE;
  axis = 0; int16_t acc_y = ACC_VALUE;
  axis = 2; int16_t acc_z = ACC_VALUE;
#else
  // use EstG values (doesn't lag, but needs time to settle after a rotation)
  int16_t acc_x = -EstG.V.Y;
  int16_t acc_y = EstG.V.X;
  int16_t acc_z = EstG.V.Z;
#endif
  
  float earth_acc_x = (cp*cy) * acc_x + (sr*sp*cy - cr*sy) * acc_y + (sr*sy + cr*sp*cy) * acc_z;
  float earth_acc_y = (cp*sy) * acc_x + (cr*cy + sr*sp*sy) * acc_y + (-sr*cy + cr*sp*sy) * acc_z;
  float earth_acc_z = (-sp) * acc_x + (sr*cp) * acc_y + (cr*cp) * acc_z;
  
  debug[0] = earth_acc_x;
  debug[1] = earth_acc_y;
  debug[2] = earth_acc_z;
  

#endif

It's commented and not optimised, but works (in the limits of the known bugs, but I'm sure that can be worked around). Have fun and good night

[mahowik]

EstG and ACC vectors are different because of time lag, not because they are a different thing. A projection might be somewhat correct in short timeframes and with litte motion/rotation (just like the rotateV function which, well ... lets not talk about it ... but it kind of works for supersmall angles). Introducing errors and then correcting them in a different part of the application is a bad practice :/

A precise solution is in my reply above. I even posted an example ... using a rotation matrix it will always give correct global acceleration in every direction (that's what you wanted and asked me to provide for you, remember?). There is still the problem with gimbal lock, but lets say if a copter is that much tilted you have other problems to worry about. Please take a look at my pseudocode and the example ...

Actually with rotation matrix you will get simular or one to one issue/bug. In IMU roll/pitch angles calculated based on EstG vector which is not correct during the constant linear accelerations...
As result, input parameters of rotation matrix will have the same errors AND incorrect result accordingly...
E.g. during the constant horizontal acceleration (x or y) EstG will magnet to ACC (with speed proportional to gyro factor in gyro/acc complimentary filter). As result IMU will register that acceleration like inclinations... so... i see the same issues and as I remember it's known unresolved issue in IMUs because gyro corrected by accelerometer which doesn't actually know where is inclination and where is acceleration...

In any case THANKS A LOT for your thoughts! It can help but I afraid it will be performance issue to calculate 3cos+3sin...

I took the liberty to fully implement it, so I can sleep well and don't have to dream about rotation matrices

Powerful! Thanks! I will look!

Alex

[mahowik]

Code: Select all

// use acc values directly (lags, because angles are lagging behind reality)
  #define USE_ACC_FOR_EARTH_ACC 1
  
#if USE_ACC_FOR_EARTH_ACC == 1
  axis = 1; int16_t acc_x = -ACC_VALUE;
  axis = 0; int16_t acc_y = ACC_VALUE;
  axis = 2; int16_t acc_z = ACC_VALUE;
#else
  // use EstG values (doesn't lag, but needs time to settle after a rotation)
  int16_t acc_x = -EstG.V.Y;
  int16_t acc_y = EstG.V.X;
  int16_t acc_z = EstG.V.Z;
#endif

You have a lag with ACC because ACC_VALUE = accLPF[axis] with big LPF factor =100
Try to use accLPFVel[axis] (its LPF =10 where cut off frequency about 5-10hz)
or accADC[axis] w/o LPF - but only for tests because it's sensitive for vibrations...

[Magnetron]

hei boys, GOOD JOB.

But I think that is a new more efficient solution:

Quaternions

3D orientations are commonly represented using rotation matrices and Euler angles. Quaternions offer an alternative representation that is computational more efficient than rotational matrices and is not subject to the problem of a singularity (AKA gimbal lock) that can make an Euler angle representation hazardous. The resources below include a document summarising this use of quaternions and an accompanying MATLAB library.

link: http://www.x-io.co.uk/node/8#open_sourc ... _algorithm

this is an implemetation from Madgwick and actually this routine is used on SIMPLO project by Ciskje!!!
We must implement it on multiwii!!!

Code: Select all

//=====================================================================================================
// AHRS.h
// S.O.H. Madgwick
// 25th August 2010
//=====================================================================================================
//
// See AHRS.c file for description.
// 
//=====================================================================================================
#ifndef H_AHRS
#define H_AHRS

#define Kp 2.0f         // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.005f      // integral gain governs rate of convergence of gyroscope biases

class Ahrs
{
  //---------------------------------------------------------------------------------------------------
  // Variable definitions
private:
  float exInt , eyInt , ezInt ;   // scaled integral error
  unsigned long last,now;
public:
  float q0, q1 , q2 , q3 ;   // quaternion elements representing the estimated orientation
  float p,r,y;
  unsigned long prevTime;


  void setup()
  {
    exInt=0;
    eyInt=0;
    ezInt=0;
    q0=1.0;
    q1=q2=q3=0;
    now=micros();
  }

  float dt()
  {
    last=now;
    now=micros();
    float dt=(now-last)/1e6;

    return dt;
  }

  void updateMARG(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {

    float norm,halfT;
    float hx, hy, hz, bx, bz;
    float vx, vy, vz, wx, wy, wz;
    float ex, ey, ez;

    halfT=dt()/2.0;

    // auxiliary variables to reduce number of repeated operations
    float q0q0 = q0*q0;
    float q0q1 = q0*q1;
    float q0q2 = q0*q2;
    float q0q3 = q0*q3;
    float q1q1 = q1*q1;
    float q1q2 = q1*q2;
    float q1q3 = q1*q3;
    float q2q2 = q2*q2;   
    float q2q3 = q2*q3;
    float q3q3 = q3*q3;          

    // normalise the measurements
    norm = sqrt(ax*ax + ay*ay + az*az);       
    ax = ax / norm;
    ay = ay / norm;
    az = az / norm;
    norm = sqrt(mx*mx + my*my + mz*mz);          
    mx = mx / norm;
    my = my / norm;
    mz = mz / norm;         

    // compute reference direction of flux
    hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
    hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
    hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         
    bx = sqrt((hx*hx) + (hy*hy));
    bz = hz;        

    // estimated direction of gravity and flux (v and w)
    vx = 2*(q1q3 - q0q2);
    vy = 2*(q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;
    wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
    wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
    wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  

    // error is sum of cross product between reference direction of fields and direction measured by sensors
    ex = (ay*vz - az*vy) + (my*wz - mz*wy);
    ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
    ez = (ax*vy - ay*vx) + (mx*wy - my*wx);


    // integral error scaled integral gain
    exInt += ex*Ki;
    eyInt += ey*Ki;
    ezInt += ez*Ki;



    // adjusted gyroscope measurements
    gx +=Kp*ex + exInt;
    gy +=Kp*ey + eyInt;
    gz +=Kp*ez + ezInt;

    // integrate quaternion rate and normalise
    q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
    q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
    q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  

    // normalise quaternion
    norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 = q0 / norm;
    q1 = q1 / norm;
    q2 = q2 / norm;
    q3 = q3 / norm;

    toEuler();
  }

  void updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
    float norm,halfT;
    float vx, vy, vz;
    float ex, ey, ez;         

    halfT=dt()/2.0;

    // normalise the measurements
    norm = sqrt(ax*ax + ay*ay + az*az);       
    ax = ax / norm;
    ay = ay / norm;
    az = az / norm;      

    // estimated direction of gravity
    vx = 2*(q1*q3 - q0*q2);
    vy = 2*(q0*q1 + q2*q3);
    vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

    // error is sum of cross product between reference direction of field and direction measured by sensor
    ex = (ay*vz - az*vy);
    ey = (az*vx - ax*vz);
    ez = (ax*vy - ay*vx);

    // integral error scaled integral gain
    exInt = exInt + ex*Ki;
    eyInt = eyInt + ey*Ki;
    ezInt = ezInt + ez*Ki;

    // adjusted gyroscope measurements
    gx = gx + Kp*ex + exInt;
    gy = gy + Kp*ey + eyInt;
    gz = gz + Kp*ez + ezInt;

    // integrate quaternion rate and normalise
    q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
    q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
    q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  

    // normalise quaternion
    norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 = q0 / norm;
    q1 = q1 / norm;
    q2 = q2 / norm;
    q3 = q3 / norm;

    toEuler();
  }

  void toEuler()
  {
    /* STANDARD ZYX
     y=atan2(2*q1*q2-2*q0*q3,2*q0*q0+2*q1*q1-1);
     p=-asin(2 * q1 * q3 + 2 * q0 * q2); // theta
     r=atan2(2 * q2 * q3 - 2 * q0 * q1, 2 * q0 * q0 + 2 * q3 * q3 - 1); // phi
     */
    y=atan2(2*q1*q2+2*q0*q3,2*q0*q0+2*q1*q1-1);
    p=-asin(2 * q1 * q3 - 2 * q0 * q2); // theta
    r=atan2(2 * q2 * q3 + 2 * q0 * q1, 2 * q0 * q0 + 2 * q3 * q3 - 1); // phi
  }
} ahrs;

#endif

[timecop]

let's do it on a platform that can handle more than one sinf() per loop iteration.

[Sebbi]

@mahowik: ACC_VALUE is the raw adc in default configuration. But I get the other problem ... however, I figured out that you can use EstG here if rotateV() is corrected (that function uses a nasty optimisation).

@Magnetron: I tried an AHRS implementation on MultiWii. The copter flew, but this needs a lot of adjustments and it will use more bytes and cycle time. And "new" might be the wrong wording ... quaternions are pretty old and common (in 3D graphics & IMUs).

@timecop: feel free to implement this on the STM32 platform The 8-bitters can do a lot of sinf/cosf in the loop. Other FCs have fixed loop times of 10ms and they fly fine, too ... MultiWii only needs low cycle times because the whole PID controller thing is tuned to that and doesn't take variable cycle times into account.

[timecop]

<Sebbi> if rotateV() is corrected (that function uses a nasty optimisation).
I suspected this, do you have the "unoptimized" version?
Is that the one where sin = angle for small angles?

[mahowik]

@mahowik: ACC_VALUE is the raw adc in default configuration. But I get the other problem ... however, I figured out that you can use EstG here if rotateV() is corrected (that function uses a nasty optimisation).

1) NO! It uses LPF by default at least since 1.8 release... now it's

Code: Select all

#ifndef ACC_LPF_FACTOR
  #define ACC_LPF_FACTOR 100
#endif
.........................

#if defined(ACC_LPF_FACTOR)
      accLPF[axis] = accLPF[axis] * (1.0f - (1.0f/ACC_LPF_FACTOR)) + accADC[axis] * (1.0f/ACC_LPF_FACTOR);
     
     accLPFVel[axis] = accLPFVel[axis] * (1.0f - (1.0f/ACC_LPF_FOR_VELOCITY)) + accADC[axis] * (1.0f/ACC_LPF_FOR_VELOCITY);
      
     accSmooth[axis] = accLPF[axis];
      #define ACC_VALUE accSmooth[axis]
    #else 
...................

i.e. as result ACC_VALUE = accLPF[axis] by default

2) using of EstG as ACC vector is not correct at all from my point of view... by default for stable mode it's required LPF with big factor (=100) to avoid drift in level mode + you have delay in CF (complimentary filter)... so, EstG far from real acceleration...

3) Do you have a look on this? viewtopic.php?f=8&t=2503&start=20#p24138
there are the same incorrections in angles before matrix rotation...

[Sebbi]

@timecop: Yeah, that's the function that assumes cos(small number) = 1 & sin(small number) = small number ... the correct version would look a lot like the code I posted above (rotation matrix)

@mahowik:
1) Oh, I didn't notice that. That explains a lot.
2) I agree
3) That's an issue of the accelerometer correcting the gyros ... less correction (MPU6050 gyros are not drifting that much) would counter that problem a little bit.

Again, you asked for a way to get global accelerations. I gave one ... will that work for what you plan to do? Btw, I had similar plans a few months ago, but never got anywhere (viewtopic.php?t=2015&p=18255), I hope you succeed

[mahowik]

Again, you asked for a way to get global accelerations. I gave one ... will that work for what you plan to do? Btw, I had similar plans a few months ago, but never got anywhere (viewtopic.php?t=2015&p=18255), I hope you succeed

Sebbi! One more time! Many many thanks for BIG effort!
I hope it helps to improve gps position hold... Also going to play with your code soon...

[mahowik]

I had similar plans a few months ago, but never got anywhere (viewtopic.php?t=2015&p=18255), I hope you succeed

I have a look through "[idea] Introduce a velocity vector" topic. You was near the right way with your ideas! I also played a lot about half an year ago with raw acc data and attempts to integrate ACC data to improve altitude calculations for alt hold. Also had a lot of discussions with alexmos on althold in that time...
Yes, it was sensitive to temperature drift, vibrations and double integration issues etc. When I was back to alt hold again (one month ago viewtopic.php?f=8&t=2371&start=40#p22485) I got it. And now we know more on this:
1) LPF for ACC should be tuned for golden mean, where fast acceleration still visible but not noise/vibrations (accLPFVel with factor =10 on cycle time about 3-4ms).
2) to avoid drift/errors each integration should be corrected by real data, e.g. first ACC integration by baro velocity, second by baro altitude etc.

So, for what this story and PR?! I would like to ask you to do it together. At least three people involved to discussion have a knowledge on this: you, Magnetron and me... Make sense to merge our minds and release gps PH improvement!

[timecop]

Code: Select all

void rotateV(struct fp_vector *v, float *delta)
{
    struct fp_vector v_tmp = *v;
 
#if 0
    v->Z -= delta[ROLL] * v_tmp.X + delta[PITCH] * v_tmp.Y;
    v->X += delta[ROLL] * v_tmp.Z - delta[YAW] * v_tmp.Y;
    v->Y += delta[PITCH] * v_tmp.Z + delta[YAW] * v_tmp.X;
#else
    float mat[3][3];
    float cosx, sinx, cosy, siny, cosz, sinz;
    float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
 
    cosx = cosf(-delta[PITCH]);
    sinx = sinf(-delta[PITCH]);
    cosy = cosf(delta[ROLL]);
    siny = sinf(delta[ROLL]);
    cosz = cosf(delta[YAW]);
    sinz = sinf(delta[YAW]);
 
    coszcosx = cosz * cosx;
    coszcosy = cosz * cosy;
    sinzcosx = sinz * cosx;
    coszsinx = sinx * cosz;
    sinzsinx = sinx * sinz;
 
    mat[0][0] = coszcosy;
    mat[0][1] = sinz * cosy;
    mat[0][2] = -siny;
    mat[1][0] = (coszsinx * siny) - sinzcosx;
    mat[1][1] = (sinzsinx * siny) + (coszcosx);
    mat[1][2] = cosy * sinx;
    mat[2][0] = (coszcosx * siny) + (sinzsinx);
    mat[2][1] = (sinzcosx * siny) - (coszsinx);
    mat[2][2] = cosy * cosx;
 
    v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];
    v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1];
    v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
#endif
}

Incase anyone's interested, I *think* this is the correct de-8bitization of the rotatevector.
I had to invert pitch for some reason... wii leftovers??

[Sebbi]

@timecop:
- are sinf and cosf expecting angles in degrees or radians?
- i switched the accelerations around to make it work, but inverting pitch seems to do the same
- rotateV is supposed to do the opposite of what we are trying to do here (rotate the acceleration vector, so it can be later used to calculate pitch and roll from that). I hope you confirmed that the rotation you are doing is in the correct direction

@mahowik:
I'd like that. I can imagine using quaternions only or use rotation matrices ... but part of what makes MultiWii work is "magic" and I fear changing one part will lead to a lot of areas that need changes too . There is a mirror of the _shared-branch on Github (https://github.com/multiwii/multiwii-firmware), want to start from there?

[timecop]

radians, but so is the deltaGyro values (after you multiply by gyroscale etc).
yes, I did test the rotations. they work in gui. thats how i found out i had to swap sign on one axis ??
I found this while researching this junk: http://gluonpilot.com/forum/viewtopic.php?f=4&t=43

Code: Select all

        float g = sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
        angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
        angle[PITCH] = asinf(EstG.V.Y / -g) * (180.0f / M_PI * 10.0f);

[Sebbi]

Code: Select all

  angle[ROLL]  =  _atan2(EstG.V.X , sqrt(EstG.V.Y*EstG.V.Y + EstG.V.Z*EstG.V.Z)) ;
  angle[PITCH] =  _atan2(EstG.V.Y , sqrt(EstG.V.X*EstG.V.X + EstG.V.Z*EstG.V.Z)) ;

Would be the correct way when using atan2 only ...

[timecop]

Code: Select all

  angle[ROLL]  =  _atan2(EstG.V.X , sqrt(EstG.V.Y*EstG.V.Y + EstG.V.Z*EstG.V.Z)) ;
  angle[PITCH] =  _atan2(EstG.V.Y , sqrt(EstG.V.X*EstG.V.X + EstG.V.Z*EstG.V.Z)) ;

Would be the correct way when using atan2 only ...

Except it wasnt, I tried that version (from your earlier comments) and it was weird in GUI.
Try the one I pasted, pitch now behaves properly.

[Sebbi]

I tried the code, the pitch seems reversed, but it works when to copter is "flying" wrong side up (good for airplanes). Nice find! There is still a gimbal lock problem with 90° positions though. And shouldn't roll and pitch be 45° when a quadx is standing on one of its arms?

[mahowik]

I'd like that. I can imagine using quaternions only or use rotation matrices ... but part of what makes MultiWii work is "magic" and I fear changing one part will lead to a lot of areas that need changes too . There is a mirror of the _shared-branch on Github (https://github.com/multiwii/multiwii-firmware), want to start from there?

Yes, it would be great join to the project officially.
It means that on code.google.com it will be closed? Or it's just synchronized between google and github automatically?
Or probably it's one more mwii clone?

thx-
Alex

[Sebbi]

It's just a mirror of the official google-svn-repository.

[mahowik]

It's just a mirror of the official google-svn-repository.

Who is responsible to create new account to svn? And where I can read rules (if it's exists) for code commit(s) in terms of mwii project?

thx-
Alex

[Mis]

One small question. We discussing here about the rotation matrix depending on the tilt angle for the position hold function.
But in my opinion, during position hold the tilt angle of copter is close to zero. In this case all of advanced and sloooow calculations are really needed ?

[Magnetron]

One small question. We discussing here about the rotation matrix depending on the tilt angle for the position hold function.
But in my opinion, during position hold the tilt angle of copter is close to zero. In this case all of advanced and sloooow calculations are really needed ?

We are trying to obtain position hold without gps using global vector to reach results like this:
http://www.youtube.com/watch?v=3F5yPlraFHI
in this condition the quad was not on 0° on axis... so we need global vector not only angles...