next topic »

[Reply #225 on:]

#batista1987
Try to send a PM to Kashif and ask him for his code

[Reply #226 on:]

ok:-) tks

[Reply #227 on:]

Hey lauszus, first off thanks for writing this great guide- I'm also using kas's approach and both have helped me get much farther than I would have thought- however I have hit a roadblock and through you might be able to help. btw- I'm using a 2-axis XY gyro (http://www.pololu.com/catalog/product/1266/specs) and 3-axis accelerometer (http://shop.moderndevice.com/products/mma7260qt-3-axis-accelerometer) in tandem. If it's needed I can also connect a standalone single-axis gyro for z-axis measurement, but I don't think it will be needed for angle measurement now.

Like yours, my bot also has the requirement of incline-sensing in both X and Y axes- therefore, I used the force-vector method as you did. My raw acc-angles look great, showing incline measurements independent of one another (essential for my bot to properly balance, as it's a biped). Here's a shot of my acc-calculate angles :

[img=http://img43.imageshack.us/img43/9821/scrn1c.th.jpg]

red line = y-axis inclination, black line = x-axis inclination. As you can see, the angle measurements are clearly independent of one another when the imu is tilted in a cardinal direction. (imu board tilted 90deg right with no y-axis inclination)

However, after applying kalman filtering, I get this:

[img=http://img85.imageshack.us/img85/8039/scrn2n.th.jpg]

Here, the board is in the same orientation as in the previous screenshot, but it is clear that the two measurements are not independent of each other after applying the kalman filter. What's more, the angle estimation jumps and then settles at the true angle. Finally, a screenshot with both acc and kalman data shown:

[img=http://img689.imageshack.us/img689/6681/scrn3m.th.jpg]

The highlighted box shows how the x-axis measurement jumps and settles in response to the y-axis tilt, while the accelerometer reading is relatively unchanged.

Here is the relevant code:

source of gyro data

Code:

int getGyroRateX() {                                             // ARef=3.3V, Gyro sensitivity=2.5mV/(deg/sec)
  return int(sensorValue[GYR_X] / 0.775);                 // sensitivity = 0.0025 / 3.3 * 1023
}

 int getGyroRateY() {                                             // ARef=3.3V, Gyro sensitivity=2.5mV/(deg/sec)
  return int(sensorValue[GYR_Y] / 0.775);                 // i'm guessing this value is in degrees
}

force vector calcs

Code:

 R = sqrt(pow(sensorValue[ACC_X],2) + pow(sensorValue[ACC_Y],2) + pow(sensorValue[ACC_Z],2));
  xACC_angle = (acos(sensorValue[ACC_X]/R)-pi/2)*-1;  //unless I'm not mistaken, this angle is in rads
  xACC_angleDeg = xACC_angle * RAD_DEG;
  xACC_angleQuid = xACC_angle * RAD_DEG * DEG_QUID;
  yACC_angle = (acos(sensorValue[ACC_Y]/R)-pi/2)*-1;
  yACC_angleDeg = yACC_angle * RAD_DEG;
  yACC_angleQuid = yACC_angle * RAD_DEG * DEG_QUID;

kalman calculations

Code:

 xGYRO_rate = getGyroRateX();
  xGYRO_angle = xGYRO_angle + xGYRO_rate*loopTime/1000;
  yGYRO_rate = getGyroRateY();
  yGYRO_angle = yGYRO_angle + yGYRO_rate*loopTime/1000;

  xfinAngleDeg = kalmanCalculateX(xACC_angleDeg, xGYRO_rate, loopTime);
  yfinAngleDeg = kalmanCalculateY(yACC_angleDeg, yGYRO_rate, loopTime);

These angle measurements will be feeding a PID that drives an IK calculator to get joint angles for the bot, so it is a must that these angles are nearly dead-on  
Could you help me figure this out? Thanks

[Reply #228 on:]

Hi Lauszus,

Quick question. What's the other gyro in this IMU for? You use the Gx4 but, what's the Gx1? I just don't get why you have 2 gyros but only use this one.

Thanks!

[Reply #229 on:]

#asrinivasan
You are doing the conversion from radians to degrees wrong:

Code:

xACC_angle = (acos(sensorValue[ACC_X]/R)-pi/2)*-1;  //unless I'm not mistaken, this angle is in rads
xACC_angleDeg = xACC_angle * RAD_DEG;

Instead it should be:

Code:

xACC_angleRad = (acos(sensorValue[ACC_X]/R))*-1;
xACC_angleDeg = xACC_angleRad * RAD_DEG;

I guess you are multiplying by -1, to transform the angle, so it has the same sign as the gyro

#elwylly
The Gx4 output the Gx1 output multiplied by 4. It just allows the user to get either higher or lower sensitivity depending on what you need it for.

[Reply #230 on:]

@lauszus

Thanks for the quick reply! However, I don't think that changed my problem   The pi/2 I subtracted only represents a translation of the accelerometer's angle value, which was already an accurate reading- it was the kalman filtering that produced the erroneous results.

The reason I convert it like this

Code:

 R = sqrt(pow(sensorValue[ACC_X],2) + pow(sensorValue[ACC_Y],2) + pow(sensorValue[ACC_Z],2));
  xACC_angle = (acos(sensorValue[ACC_X]/R)-pi/2)*-1;

is because in your original code, you have it like this.

Code:

 R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector
  accXangle = acos(accXval/R)*RAD_TO_DEG-90;
  accYangle = acos(accYval/R)*RAD_TO_DEG-90;

instead of subtracting 90 from the converted angle, I simply subtracted pi/2 from the angle in rad. My problem is not with the accelerometer data- the angle estimate from my accelerometers seem to be accurate; x-axis inclination does not affect y-axis data, and so on- my problem arises when I perform the kalman filtering.
As I showed in my previous post, the last picture [img=http://img689.imageshack.us/img689/6681/scrn3m.th.jpg]  shows how the distinct difference between the kalman filtering and the accelerometer data. This was not a problem when I was measuring a single axis using atan2 method, so I am wondering why, if the accelerometer data is accurate, my results are any different. (Also there is a definite lag in the kalman-filtering but I will approach that once this problem is fixed   ).

If need be I can give you my source code
Thanks

[Reply #231 on:]

#Lauszus

i find this code  http://mbed.org/users/aberk/programs/IMUfilter_RPYExample/lijojl/docs/main_8cpp_source.html. How convert cpp files in pde? tks

[Reply #232 on:]

hi....i am using imu 5dof... for buildin my self balancin robot..... i used ur program..... bt,in imu reading dre is a drift within seconds.....
and also the its measurin upto -90 to +90..... cn u help me pls.....  xpct a quick reply.....

[Reply #233 on:]

#asrinivasan
Sorry my bad
Do your gyro data seem correct? Do it start out correct and then start to drift?
Also try adjusting the constants in the Kalman filter (the process and measurements noise constants), as you use a different accelerometer and gyro than me.
Do you get better results using the complimentary filer - if so, then it's something with the Kalman filter.
Try posting your code and I will have a look at it

#batista1987
That might take a bit of work, as he uses a "library" for the mbed. Have a look at the following files and try to port them:
"IMUfilter.h"
"ADXL345.h"
"ITG3200.h"

#iamnarendra
Have you tried both the Kalman filter and the complimentary filter approach? Try both of them and see if it helps.

Regards
Lauszus

[Reply #234 on:]

@lauszus

Sorry for the late reply, we've had a lot of work in school these days
As far as I know, the gyro and acc have the same scale, oriented  in the same direction, reflect accurate measurements, etc. but nonetheless kalman filtering still produces false (not true to original signal) results. Adjusting the constants of the kalman filtering seem to make the random jumps only more prominent - so I'll just post the code here, and hopefully someone can see my mistake

I'll just post the relevant code (leaving out PID, etc.)
Here is the main file (with setup/loop):

Code:

#include <math.h>

#define GYR_X          0
#define GYR_Y          1

#define ACC_X          2
#define ACC_Y          3
#define ACC_Z          4

const float RAD_DEG = 57.2957795;
const float pi = 3.1415926;
const float QUID_DEG = 0.3515625;
const float DEG_QUID = 2.8444444;

int sensorValue[5] = {0,0,0,0,0};
int sensorZero[5] = {0,0,0,0,0};

float xACC_angle = 0;
float xACC_angleDeg = 0;
float xACC_angleQuid = 0;
int xGYRO_rate = 0;
float xGYRO_angle = 0;

float yACC_angle = 0;
float yACC_angleDeg = 0;
float yACC_angleQuid = 0;
int yGYRO_rate = 0;
float yGYRO_angle = 0;

float xfinAngleQuid = 0;
float xfinAngleDeg = 0;

float yfinAngleQuid = 0;
float yfinAngleDeg = 0;

int PIDcorrect = 0;
int error = 0;

float R = 0;

//alternate timing mechanism...
int STD_LOOP_TIME = 10;
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;

unsigned long timer=0;
unsigned long dtime=0;

void setup() {
  analogReference(EXTERNAL);
  Serial.begin(115200);
  delay(100);
  calibrateSensors();
}

void loop() {
  updateSensors();

//atan2 angle calculation method (single-axis inclination only)
  /*xACC_angle = getAccAngle();
  xACC_angleDeg = int (xACC_angle/2.84444);
  xGYRO_rate = getGyroRate()*-1;*/

//vector angle calculation method
  R = sqrt(pow(sensorValue[ACC_X],2) + pow(sensorValue[ACC_Y],2) + pow(sensorValue[ACC_Z],2));
  xACC_angle = (acos(sensorValue[ACC_X]/R)-pi/2)*-1;
  xACC_angleDeg = xACC_angle * RAD_DEG;
  xACC_angleQuid = xACC_angle * RAD_DEG * DEG_QUID;
  yACC_angle = (acos(sensorValue[ACC_Y]/R)-pi/2)*-1;
  yACC_angleDeg = yACC_angle * RAD_DEG;
  yACC_angleQuid = yACC_angle * RAD_DEG * DEG_QUID;
 
  xGYRO_rate = getGyroRateX();
  xGYRO_angle = xGYRO_angle + xGYRO_rate*lastLoopTime/1000;
  yGYRO_rate = getGyroRateY();
  yGYRO_angle = yGYRO_angle + yGYRO_rate*lastLoopTime/1000;
 
  dtime = millis()-timer;
  timer = millis();
 
 //not working-- fix
  xfinAngleDeg = kalmanCalculateX(xACC_angleDeg, xGYRO_rate, dTime);
  yfinAngleDeg = kalmanCalculateY(yACC_angleDeg, yGYRO_rate, dTime);
 
  //PIDcorrect = updatePid(0, xfinAngleDeg, dtime);
 
  //serialOut_sensor();
  serialOut_timing();
  //processing();
 
  /*lastLoopUsefulTime = millis()-loopStartTime;
  if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);
  lastLoopTime = millis() - loopStartTime;
  loopStartTime = millis();*/
}

and the sensors/calibrate/read code:

Code:

void calibrateSensors() {
   long v;
   for(int n=0; n<5; n++) {
     v=0;
     for(int i=0; i<50; i++)    v += readSensor(n);
       sensorZero[n] = v/50;
   }
  sensorZero[ACC_Z] -= 166;
 }
 
  void updateSensors() {
  long v;
  for(int n=0; n<5; n++) {
    v=0;
    v = readSensor(n)+2*readSensor(n)+3*readSensor(n)+4*readSensor(n)+3*readSensor(n)+2*readSensor(n)+readSensor(n); //tri. avg
    sensorValue[n] = v/16 - sensorZero[n];
  }
 }
 
 int readSensor(int channel) {
  return (analogRead(channel));
 }
 
 //******************************* angle calculations **************************************
 int getAccAngle() {
   return arctan2(-sensorValue[ACC_Z], -sensorValue[ACC_X]) + 256;    // in Quid: 1024/(2*PI))
}

 int getGyroRateX() {                                             // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)
  return int(sensorValue[GYR_X] / 0.775);                 // in quid/sec:(1024/360)/1024 * 3.3/0.002)
}

 int getGyroRateY() {                                             // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)
  return int(sensorValue[GYR_Y] / 0.775);                 // in quid/sec:(1024/360)/1024 * 3.3/0.002)
}

int arctan2(int y, int x) {                                    // http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
   int coeff_1 = 128;                                          // angle in Quids (1024 Quids=360°)
   int coeff_2 = 3*coeff_1;
   float abs_y = abs(y)+1e-10;
   float r, angle;

   if (x >= 0) {
     r = (x - abs_y) / (x + abs_y);
     angle = coeff_1 - coeff_1 * r;
   }  else {
     r = (x + abs_y) / (abs_y - x);
     angle = coeff_2 - coeff_1 * r;
   }
   if (y < 0)      return int(-angle);
   else            return int(angle);
}

Finally, the kalman module (unchanged constants):

Code:

   float Q_angleX  =  0.001;
    float Q_gyroX   =  0.003; 
    float R_angleX  =  0.03; 

    float x_angle = 0;
    float x_bias = 0;
    float PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0;
    float dtX, yX, SX;
    float KX_0, KX_1;

  float kalmanCalculateX(float newAngle, float newRate,int looptime) {
    dtX = float(looptime)/1000;                                    // XXXXXXX arevoir
    x_angle += dtX * (newRate - x_bias);
    PX_00 +=  - dtX * (PX_10 + PX_01) + Q_angleX * dtX;
    PX_01 +=  - dtX * PX_11;
    PX_10 +=  - dtX * PX_11;
    PX_11 +=  + Q_gyroX * dtX;
   
    yX = newAngle - x_angle;
    SX = PX_00 + R_angleX;
    KX_0 = PX_00 / SX;
    KX_1 = PX_10 / SX;
   
    x_angle +=  KX_0 * yX;
    x_bias  +=  KX_1 * yX;
    PX_00 -= KX_0 * PX_00;
    PX_01 -= KX_0 * PX_01;
    PX_10 -= KX_1 * PX_00;
    PX_11 -= KX_1 * PX_01;
   
    return x_angle;
  }
 
  float Q_angleY  =  0.001;
    float Q_gyroY   =  0.003; 
    float R_angleY  =  0.03; 

    float y_angle = 0;
    float y_bias = 0;
    float PY_00 = 0, PY_01 = 0, PY_10 = 0, PY_11 = 0;
    float dtY, yY, SY;
    float KY_0, KY_1;

  float kalmanCalculateY(float newAngle, float newRate,int looptime) {
    dtY = float(looptime)/1000;                                    // XXXXXXX arevoir
    y_angle += dtY * (newRate - y_bias);
    PY_00 +=  - dtY * (PY_10 + PY_01) + Q_angleY * dtY;
    PY_01 +=  - dtY * PY_11;
    PY_10 +=  - dtY * PY_11;
    PY_11 +=  + Q_gyroY * dtY;
   
    yY = newAngle - y_angle;
    SY = PY_00 + R_angleY;
    KY_0 = PY_00 / SY;
    KY_1 = PY_10 / SY;
   
    y_angle +=  KY_0 * yY;
    y_bias  +=  KY_1 * yY;
    PY_00 -= KY_0 * PY_00;
    PY_01 -= KY_0 * PY_01;
    PY_10 -= KY_1 * PY_00;
    PY_11 -= KY_1 * PY_01;
   
    return y_angle;
  }

and just for reference, this is how I'm graphing the data (basically using your processing sketch to graph custom inputs):

Code:

void processing()
{
  Serial.print(xACC_angleDeg);Serial.print("\t");
  Serial.print(0);Serial.print("\t");
 
  Serial.print(yACC_angleDeg);Serial.print("\t");
  Serial.print(0);Serial.print("\t"); 
 
  Serial.print(xfinAngleDeg);Serial.print("\t"); 
  Serial.print(0); Serial.print("\t");
 
  Serial.print(yfinAngleDeg);Serial.print("\t");
  Serial.print(0);Serial.print("\t");
   
  Serial.print("\n");
}

Thanks for your help!

[Reply #235 on:]

#asrinivasan
Hmm, It looks alright. Have you tried to just debug the accelerometer and gyro angles? Just to see if they seem correct?
You could just do something like this:

Code:

Serial.print(xACC_angle, DEC);
Serial.print("\t");
Serial.println(xGYRO_angle, DEC);

Regards
Lauszus

[Reply #236 on:]

@Lauszus

Thanks for your help! I finally figured it out- I'm not surprised I didn't catch this, as it was not syntax or logic error but in fact simply due to the speed of the Arduino system. I'll post my solution for others who experience this problem. Here is the offending code:

Code:

 dtime = millis()-timer;
  timer = millis();

//not working-- fix
  xfinAngleQuid = kalmanCalculateX(xACC_angleQuid, yGYRO_rate, dtime); //note the use of y-gyro for x-axis measurement
  xfinAngleDeg = xfinAngleQuid * QUID_DEG;
  yfinAngleQuid = kalmanCalculateX(yACC_angleQuid, xGYRO_rate, dtime); //here too
  yfinAngleDeg = yfinAngleQuid * QUID_DEG;

This produces unexpectedly inaccurate results- the kalman-filtered reading jumps then settles, resulting in very inaccurate inclination measurements. Here is my problem signal:

[img=http://img96.imageshack.us/img96/5797/scrn5.th.jpg]

As you can see in the problem code, kalmanCalculate is called twice- but as it happens, this module is very resource-intensive, involving matrix calculations and such. Therefore, it takes a significant amount of time return a result (about ~3.5 ms per call according to my experimentation). Therefore, because the timing module measures not only the time of a single loop but also part of the last loop (as you can see it is in the middle of the code, after sensor calibration but before kalman calculation) and hence returns a "dtime" that is much larger than the actual time taken to traverse a single loop. Because of this, depending on the complexity of operations occurring after the 'dtime' measurement, the kalman calculations (and potentially PID loops) tend to behave strangely. The solution to this is simple:

Code:

 dtime = millis()-timer-7; // to compensate for extra kalman calls
  timer = millis();

... and produces this result:

[img=http://img32.imageshack.us/img32/4615/scrn6a.th.jpg]

Alternately, you could simply place the 'timer' variable after kalman calculations:

Code:

  dtime = millis()-timer;
 
  xfinAngleQuid = kalmanCalculateX(xACC_angleQuid, xGYRO_rate, dtime);
  xfinAngleDeg = xfinAngleQuid * QUID_DEG;
  yfinAngleQuid = kalmanCalculateY(yACC_angleQuid, yGYRO_rate, dtime);
  yfinAngleDeg = yfinAngleQuid * QUID_DEG;
 
  timer = millis();//place after kalman calculations
  processing();

This way, you can call kalman as many times as you want without affecting timing.

[Reply #237 on:]

Nice tip. I have never thought about that
So if you already wrote it, but what are you going to use it for?

Regards
Lauszus

[Reply #238 on:]

I'm actually using this for a much larger project- it's a lower-body robotic orthosis (think exoskeleton) that is supposed to be able to dynamically self-balance and require minimal input from the user to operate- in this respect, it is almost autonomous in the way it behaves. It basically facilitates movement of the legs to aid in walking for those with congenital diseases (or simply due to senility/osteoperosis)- as far as I know, this is the first biped orthosis able to self-balance. I know it is a complicated project, but those who know me know that I am prone to such things  (last year's project was a mind-controlled robotic arm http://sites.google.com/site/eegprosthetics/home http://news.cnet.com/2300-11386_3-10008553-8.html). I am entering it for this year's Science Fair (I am a high school sophomore).

It uses several different controlling procedures to facilitate the gait, including an ANN (using back-propagation learning) to control step movements, an IK calculator to determine joint angles, calculation of ZMP (rzero-moment-point), etc. So you can see how frustrated I was when this little part of the project wasn't working properly!  It uses incline sensing as well as body velocity, joint torques, operator inertia, etc. to determine optimal foot placement- since we as humans can balance in place without moving the feet.
Wish me luck! It's only a few weeks away... 

[Reply #239 on:]

Nice, thats sounds really interesting. Just saw the video of your mindcontrolling arm - man you speak fast. Did you speed it up or something? I would really like to see a video of the arm in action. Do you got it on youtube or somewere else :O
How long are you from finishing your project, because it really sounds complicated and how long did you have in total from start to finish?