Position Hold using Accelerometers global vectors

[mahowik]

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...

As I told before w/o gps it will be not possible, because we need to correct errors after integration... it's similar to current alt hold, where errors corrected by baro-velocity...

So we can improve gps PH but not replace gps...

[Magnetron]

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...

As I told before w/o gps it will be not possible, because we need to correct errors after integration... it's similar to current alt hold, where errors corrected by baro-velocity...

So we can improve gps PH but not replace gps...

Yes you are right but I would to explain our concept goal. We need angles also so we do not approximate multirotor behaviour to level at 0°.
I know we need to integrate gps data to vectors...
Your baro+acc routine is great job, we must have acc+gps... but also, with complemetary filter, gps component must be very small like baro... we need the main component, vectors!

[Mis]

We need angles also so we do not approximate multirotor behaviour to level at 0°..

In position hold ? Why not ? This topic and discutions is about position hold, not about acro or even normal flight behaviour.
At position hold we can use lot of simplifications, because this is ONLY POSITION HOLD with small corects to 0 deg level. I know, this is not right think, but with poor 8-bit AVR's this is only way to success.

[mahowik]

We need angles also so we do not approximate multirotor behaviour to level at 0°..

In position hold ? Why not ? This topic and discutions is about position hold, not about acro or even normal flight behaviour.
At position hold we can use lot of simplifications, because this is ONLY POSITION HOLD with small corects to 0 deg level. I know, this is not right think, but with poor 8-bit AVR's this is only way to success.

Magnetron is right.
Simple example:
During the wind correction, angle to keep copter can be 5-15 degree. If you are not take into account inclination, it will be translated as constant acceleration and integrated as velocity... velocity will grow grow etc., but actually it's zero...

[Magnetron]

We need angles also so we do not approximate multirotor behaviour to level at 0°..

In position hold ? Why not ? This topic and discutions is about position hold, not about acro or even normal flight behaviour.
At position hold we can use lot of simplifications, because this is ONLY POSITION HOLD with small corects to 0 deg level. I know, this is not right think, but with poor 8-bit AVR's this is only way to success.

Magnetron is right.
Simple example:
During the wind correction, angle to keep copter can be 5-15 degree. If you are not take into account inclination, it will be translated as constant acceleration and integrated as velocity... velocity will grow grow etc., but actually it's zero...

RIGHT! This is THE KEY!
My idea is relatively simple: if I can calculate global vector on X and Y axis I know if I am moving (angle independent) on the plane x-y. If my vector (or velocity) grows I need to compensate from other side. But I think we must try to think to something like this:

Code: Select all

VX=velocity on X axis;  AX=angle on X axis (0=level...1=+/-90°);
VY=velocity on Y axis;  AY=angle on Y axis;
CX=compensation effect on X axis;
CY=compensation effect on Y axis;

so

Code: Select all

CX=VX*(1-AX);
CY=VY*(1-AY);

so (for X axis) if I reach 0° (level) my velocity components if I move is VX, but if I had some inclination I obtain a less velocity components.
At this time we can implement a correction PID only on compensation effect (in the second time we implement also GPS data by complementary filter on CX and CY).
What do you think about?

[timecop]

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?

hey, yeah,, pitch was wrong, but fix is just - in front of asinf().
went to fly it, seems to work, so its ok.

[mahowik]

My idea is relatively simple:

not shure that I got your idea... does it make sense to reduce compensation according to current inclination? why?

[mahowik]

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

I played with your matrix yesterday (with accLPFVel as ACC vector). Checked in GUI (earth_acc_z - acc_1G) and accZ (my solution). It's the same even in very fast vertical movements. Diff only in 1-2 pixels in MultiWiiConf GUI. Only one issue here: when it's "head over heels" matrix doesn't work...

Code: Select all

  int16_t acc_x = -accLPFVel[PITCH];
  int16_t acc_y = accLPFVel[ROLL];
  int16_t acc_z = accLPFVel[YAW];

Also not sure about earth_acc_x, earth_acc_y. Actually it sees x,y acceleration according to current heading BUT during the vertical movement it's also has essential amplitude in some fixed heading...
So are sure that all signs are correct? What the main principals to set right axis and signs to matrix?

[Magnetron]

My idea is relatively simple:

not shure that I got your idea... does it make sense to reduce compensation according to current inclination? why?

It make not sense is only a inherited concept from a gaming imu.
Main key is global vectors...
We must calculate that vectors.
Have you some idea on how?

[mahowik]

but Sebbi provided all you need... now just integrate acc to velocity and play further with these:
viewtopic.php?f=8&t=2503&start=10#p23631
and apply to viewtopic.php?f=8&t=2503#p23400

[Sebbi]

I played with your matrix yesterday (with accLPFVel as ACC vector). Checked in GUI (earth_acc_z - acc_1G) and accZ (my solution). It's the same even in very fast vertical movements. Diff only in 1-2 pixels in MultiWiiConf GUI. Only one issue here: when it's "head over heels" matrix doesn't work...

I am pretty sure the matrix code is correct, but the angles MultiWii is calculating are off when getting bigger or the copter/plane is flipped upside down. Timecop found a better formula to calculate angles, but the heading seems to be wrong too, when upside down. As Mis wrote, we can maybe ignore that, because in position hold the angles wont be extreme enough for it to matter (needs testing, the more precise everything is, the better the outcome we get).

Code: Select all

  int16_t acc_x = -accLPFVel[PITCH];
  int16_t acc_y = accLPFVel[ROLL];
  int16_t acc_z = accLPFVel[YAW];

Also not sure about earth_acc_x, earth_acc_y. Actually it sees x,y acceleration according to current heading BUT during the vertical movement it's also has essential amplitude in some fixed heading...
So are sure that all signs are correct? What the main principals to set right axis and signs to matrix?

I don't understand what you mean. The signs "might" be correct ... Timecop switched the pitch angle instead, I switched acc exis and inverted one to get it to work with a regular rotation matrix (the one from Wikipedia). There is another article about Euler angles and rotation matrices on Wikipedia which list all right-hand matrices (difference is in the order of rotations).

[ziss_dm]

Hi,

I'm lost.. ;(

What is the point of rotating acc to earth frame? To calculate correction and rotate back?
Or I'm missing something?

regards,
ziss_dm

[Sebbi]

If you have acc in earth frame orientation you can integrate it and get the velocity of the copter ... you can use that for position hold (correct it with gps/optical flow), altitude hold (correct it with baro/sonar)

[ziss_dm]

ok, you have velocity in earth frame. What is next?

[mahowik]

ok, you have velocity in earth frame. What is next?

next it's possible to improve gps velocity and increase precision of position hold (PH) viewtopic.php?f=8&t=2503&start=10#p23631

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);

[ziss_dm]

Let's imagine, we have very precise velocity in earth frame. What are we doing next?

[Magnetron]

Let's imagine, we have very precise velocity in earth frame. What are we doing next?

next is implementing formulas in routine and try, test, test, test...

[Sebbi]

Let's imagine, we have very precise velocity in earth frame. What are we doing next?

Lets imagine it is possible to fuse acc derived velocity with the velocities from GPS/optical flow/baro/sonar. The copter would then know its precise velocity even in short time frames so velocities from GPS and baro should improve.

These known velocities can be used to improve flight modes. You can use the Z component for altitude modes:
- hold: velocity should be 0
- reach a certain altitude with a specified velocity
- throttle stick as velocity (up/down) controller

Y/X components can be used for position hold. I don't think acc velocities only can be used for position hold since level mode would be position hold if that were possible. But slight offsets will always leed to the copter accelerating in a directing and it will still think its velocity is 0. But could be useful to improve GPS accuracy.

[ziss_dm]

next is implementing formulas in routine and try, test, test, test...

Test exactly what?

Y/X components can be used for position hold

How? I mean how transform vel X/Y/Z in earth frame to control (roll, pitch yaw)?

regards,
ziss_dm

[timecop]

You gotta install side thrusters, of course.

[Magnetron]

We must try to implement routine to obtain a stable value of velocity on X and Y axis reproducint Z behaviour of baro-acc vel introduced by mahowik.
Then we can try to hold zero velocity on X and Y axis correctiong pitch/roll command filtered to become stable.
Someone had implemented algorithm to obtain X and Y valocity?

[ziss_dm]

X, y relative to earth or aircraft body?

[Magnetron]

X, y relative to earth or aircraft body?

to aircraft body...

[ziss_dm]

In this case, EstG - Acc is acceleration vector in body frame. Integrate it and you'll get velocity vector in body frame.

[Sebbi]

I think there is a missunderstanding here. When you want to have something like position hold or altitude hold or "fly there with velocity X" you need those velocities in earth frame like GPS and barometers are providing them. And when we want to augment/enhance those velocities by using the accelerometer we need to transform the body frame accelerations to earth frame accelerations by rotating them according to the current orientation/attitude of the copter/plane.

Is that clear now?

[ziss_dm]

I repeat my question: How to use vel in earth frame for control? Rotate it to local frame?

regards,
ziss_dm

[Sebbi]

Again: the x/y velocity (or call it north/east velocity since it is in earth frame) would augment the velocity calculated from the GPS signal (also in earth frame). This could (we hope) improve short term GPS accuracy, especially in position hold mode where velocities from GPS aren't that reliable.

Same thing goes for z velocity and baro measurements (which is already more or less implemented).

You could do some optimisations and assume that when you want the copter to hold its position it is also level. In this case the velocities from the accelerometer can be used without rotation. However, this would ignore flying in windy conditions where position hold is not equal level flight.

[ziss_dm]

We are going nothere..

I can see atleast 2 ways to build CF:

1) Rotate Local ACC to the earth frame and run CF in earth frame.
2) Rotate GPS vel to local frame and run CF in local frame.

My question was: Why method (1) was chosen? Only due similarity with AltHold?
Or if I re-phrase it: What is the benefit of having vel. in earth frame? How exactly we are going to use it for control? Will it require to rotate it back to local frame?

regards,
ziss_dm

[Algorithmer]

Hi everybody, new here, first post.
I fly RC wince 20years, develop autopilots as a hobby since 5 years, using mainly arduinos.
I have actually an ardupilot hw and Mwii crius clone (from HK the 46$ one with 328 mcu), now developing my code for Mwii (ardupilot has my own code but for fixed wings, can't port it because 328 is way too slow..looking for a 32bit platform but should learn everything before).

I already did that work you trying to achieve just to show myself what math already tells..it was a couple of years ago but still remember important things I learned.
say you have a 0.04 m/s^2 error (real error, the one that average can't take out, born when you discriminate acc and gravity)..
after 10seconds you have 0.4 m/s speed error (v=a*t) and position error is 2m (x=1/2a*t^2), you pid is already doing crazy things
after 100 sec you are running at an error speed of 4 m/s and 20m..go for a walk to find the crashed copter.
0.04m/s is really small, just vibration will return way bigger error.
In this example error is assumed constant so I can make easy calcs.
The reason why accelerometer is usable on alhold is because baro returns a straight postion, is exactly like a vertical gps (different resolution of course).

anyway I tried..
first problem is computing power, a full imu with no approximations and advanced drift correction eats power, a lot. Without a good imu code you have bigger errors, mine ran at 50hz.
second problem is the position needs 2 integration and the added error become too big after some seconds as expected by math.
third..this error is when holding in hands..with copter vibrations..way bigger error.
result: after just seconds it diverges at an impressive speed with imu stucked on a table.


I hope I missed something and someone discover a way to do that, actually I found impossible to compute a position for more than seconds with only IMU (no gps),

[Sebbi]

We are going nothere..

I can see atleast 2 ways to build CF:

1) Rotate Local ACC to the earth frame and run CF in earth frame.
2) Rotate GPS vel to local frame and run CF in local frame.

My question was: Why method (1) was chosen? Only due similarity with AltHold?
Or if I re-phrase it: What is the benefit of having vel. in earth frame? How exactly we are going to use it for control? Will it require to rotate it back to local frame?

regards,
ziss_dm

The benefit? Because that's the only frame you can have velocities in? I don't see how local frame velocities can be used for anything without rotating them with the copter (when the copter rotates) and then you could have rotated them to earth frame as well and get meaningful velocities (those relative to the world and that is what you need to "hold your position", or isn't it?). Please explain how (2) would work? You seem to already have the solution to this problem

And I repeat ... GPS uses the earth frame velocity already to get to a target and stay there. Augmenting those velocities with accelerometer derived ones should - in theory - result in better position hold. Why? Because the velocity from GPS when the copter is almost still isn't that accurate.

@Algorithmer:
I (and I hope we all) are aware of this problem. You can't really use those accelerometers (the cheap ones we like to have on our copters) to get an accurate path/location. But it works for short periods of time and can be fused with GPS data, so the errors can be corrected.

[dramida]

Hi everybody, new here, first post.
.....
I hope I missed something and someone discover a way to do that, actually I found impossible to compute a position for more than seconds with only IMU (no gps),

Hi there Algorithmer, i am glad a new member joined our efforts to build a cheap open source powerfull copter.

The point is merging short term acurate IMU positioning with long term acurate GPS position. Is the same ideea like in Alt Hold using IMU and baro. I saw it done in DJI FC and it is amaizing.

[Algorithmer]

I understood that the purpose was to use it withoug gps.
Fused with gps it works, I did that using some tricks. I think Premerlani worked on that too and he is a good guy, he probably has some code published.
I am implementing my code on Mwii but there is no computing power to do that function accurately on 328p or even a mega (you can't use small angle approximation, full trigonometry is needed plus quaternions operators, at least this is where I ended ).

[dramida]

More and more voices are saying that 8 bit MCU are absolete. Please bring it to its knees and have a strong reason to step forward.

[Magnetron]

I understood that the purpose was to use it withoug gps.
Fused with gps it works, I did that using some tricks. I think Premerlani worked on that too and he is a good guy, he probably has some code published.
I am implementing my code on Mwii but there is no computing power to do that function accurately on 328p or even a mega (you can't use small angle approximation, full trigonometry is needed plus quaternions operators, at least this is where I ended ).

I think that we can discuss this but could you post a sketch code to try to optimize it with gps and try an idea like mahowik baro-alt code?
What do you think about?

[Crashpilot1000]

Done. Working.

viewtopic.php?f=23&t=1947&start=210#p31263

[dramida]

This sounds like the begining of the end for 8 bit mcu's for MWC...or maybe someone could prove i am wrong

[Crashpilot1000]

I think it will be possible to do it on 8 bit as well, 3drobotics is currently working on that, but it will be much more pain, and i think it will degrade overall flight performance due to higher cycletime (i think apm is around 10ms+). There might be some financial interest to stick to this 8 Bit stuff, i see no reason why not to take a cheaper and much more powerful cpu for this task. In the meantime naze clones start to appear. The one i have seen is inferior by boarddesign and has only one 3,3V regulator. Perhaps the 32Bit Arduino or even a F4 are the future but 8 Bit had it's time, unless you get paid and program the hell out of it in assembler....
Cheers
Kraut Rob