Go Down

Topic: IMU and Gyro balance bot code (Read 14455 times) previous topic - next topic

sirbow2

Jan 20, 2012, 12:17 am Last Edit: Jan 20, 2012, 05:23 am by sirbow2 Reason: 1
i ve been working on this for the last few days. im using a L3G4200 gyro and a LSM303 Acc in an attempt to balance two wheeled robot. it first reads the gyro and acc, then combines them using Khalman equations(to find a final angle of the IMU) and then PID to adjust the servo output to stabilize the bot according to the angle.

i wasnt really expecting the PID to work because i have to change Kp Ki and Kd for fine stabilization. but for some reason the Khalman equations didnt combine the GYRO and ACC correctly and the values were way off. if i hold the imu in one place, the value will stay at once place. but if i move the imu, the value jumps, and then stabilizes to value around what it should be, ish.

heres the code:
Code: [Select]
//-----Libraries------------------------------------------------//
#include <Wire.h>
#include <LSM303.h> //ACC, MAG(not used)
#include <L3G4200D.h> //gyro
#include <IRremote.h> //IR library
#include <Servo.h> //Servo support

//-----Robot Stuff------------------------------------------------//
int LmtrSpd=0; //stores motor speed value(0-180)
int RmtrSpd=0;
#define LServoMtrPin 2 
#define RServoMtrPin 3
Servo LServoMtr;
Servo RServoMtr;

//--PID------------------------------------------------------------
//awesome explanation: http://www.x-firm.com/?page_id=193
#define offset 90 //motor center

//change for stability
#define Kp 7   //proportional
#define Ki 1   //Integral
#define Kd -5   //derivative

//for storing calculated values later on
int Error = 0; //Error
int Proportion = 0; //value to store the Proportional value from PID equation
int Integral = 0; //value to store the ... value from PID equation
int Derivative = 0; //value to store the ... value from PID equation
int Output = 0; //ouput based on PID
int PrevError = 0; //stores error for next loop + PID

//Loop refresh rate------------------------------------------------
int TimeTwo = 0;
int TimeOne = 0;
int LoopDelay = 0;  //later is modified to equal additional delay needed to make loop 30ms long
#define SetDelay 30 //how long loop should be

//IMU------------------------------------------------------------
//http://www.pololu.com/picture/view/0J3680
LSM303 compass;
L3G4200D gyro;

int GyroRate = 0;
int AccAngle = 0;
int CalibSensor[3] = {0,0,0}; //for storing IMU centering values during setup, we only need gyro Y, ACC X and Z
int PlatformAngle = 0; //stores final angle in degree of the platform, used as input for PID
int CenterVal = 0; //balancing value for IMU

// --- Kalman filter module  ----------------------------------------------------------
float Q_angle  =  0.001; //0.001
float Q_gyro   =  0.003;  //0.003
float R_angle  =  0.03;  //0.03

float x_angle = 0;
float x_bias = 0;
float y = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float dt, z, S;
float K_0, K_1;

//---Setup---------------------------------------------------------//
void setup()
{
  Serial.begin(9600);

  //LM303 acc setup
  Wire.begin();
  compass.init();
  compass.enableDefault(); //enables acc LSM303 readings
  gyro.enableDefault(); //enables gyro L3G44200D readings
 
  LServoMtr.attach(LServoMtrPin); //attach then set servos to center
  LServoMtr.write(90);
  RServoMtr.attach(RServoMtrPin);
  RServoMtr.write(90);

  IMUCenter(); //center IMU at current position

}

//--Loop------------------------------------------------------------
void loop()
{
  TimeOne = millis(); //recordes time at beginning of loop

  gyro.read(); //reads gyro
  compass.read(); //reads compass and acc(only acc used)

  PlatformAngle = IMUCalc(); //final current angle of the bot

  Output = PIDCalc(); //calculates how much plaform has tilted, smoothes and adjusts motor output accordingly

  Serial.println(PlatformAngle);
 
  LServoMtr.write(Output + offset);
  RServoMtr.write(Output + offset);

  TimeTwo = millis(); //recordes time at end of loop
  LoopDelay = SetDelay - (TimeTwo - TimeOne); //calculates delay needed to maintain SetDelay
  delay(LoopDelay); //delays needed time
}

int IMUCalc()
{
  //IMU------------------------------------------------------------ 
  AccAngle = arctan2(((int)compass.a.z - CalibSensor[3]), ((int)compass.a.x - CalibSensor[2]));  //calculates acc angle                                       
  GyroRate = (int)gyro.g.y - CalibSensor[1];   //reads gyro rate from IMU using library                                       
  return kalmanCalculate(AccAngle, GyroRate, SetDelay); // calculate combined Angle
}

void IMUCenter()
{
  gyro.read(); //reads gyro
  compass.read(); //reads compass and acc(only acc used)
  CalibSensor[1] = (int)gyro.g.y; //store values for centering
  CalibSensor[2] = (int)compass.a.x;
  CalibSensor[3] = (int)compass.a.z;

}

int PIDCalc()
{
  //PID-------------------------------------------------------------
  Error = CenterVal - PlatformAngle; //finds error between the wanted value and what it currently is.
  Proportion = Error; //"makes a change to the output that is proportional to the current error value"
  Integral += Error; //"if you aren't quite getting to target, keep increasing over a period of time until you get there"
  Derivative = (Error - PrevError); //"how fast a change in angle occurs"

  //calculates the final adjusted output based on Ki,Kp,Kd set in the beginning of
  //the program and the resulting values from Proportion, Integral, and Derivative calculations
  Output = (Kp * Proportion) + (Ki * Integral) + (Kd * Derivative);
  PrevError = Error; //puts current error in another value for next loop to use.
  return Output;
}

int arctan2(int z, int x) //finds Acc angle
{
  int coeff_1 = 128;                                       
  int coeff_2 = 3*coeff_1;
  float abs_z = abs(z)+1e-10;
  float r, angle;

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

float kalmanCalculate(float newAngle, float newRate,int looptime) //combines Gyro rate and Acc angle, no clue how it works :)
{
  dt = float(SetDelay)/1000;
  x_angle += dt * (newRate - x_bias);
  P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
  P_01 +=  - dt * P_11;
  P_10 +=  - dt * P_11;
  P_11 +=  + Q_gyro * dt;

  y = newAngle - x_angle;
  S = P_00 + R_angle;
  K_0 = P_00 / S;
  K_1 = P_10 / S;

  x_angle +=  K_0 * y;
  x_bias  +=  K_1 * y;
  P_00 -= K_0 * P_00;
  P_01 -= K_0 * P_01;
  P_10 -= K_1 * P_00;
  P_11 -= K_1 * P_01;

  return x_angle;
}
http://dduino.blogspot.com all my Arduino/electronic projects!!!

{NEW} Getting Started, Learning, Reference + FAQ PDF!!:
[url]http://ar

cyclegadget


Are you saying that this is a working code? If so, good job!

I am interested in the code because I bought the same IMU and have had some issues compiling the code examples.

In my case it get an error saying LSM303 compass; L3G4200D gyro; "does not define a type". I guess I have a problem with the library installation.

Have you had any problems getting the code to work?

Thanks for sharing!
Good links: Eagle tutorial= http://www.youtube.com/playlist?list=PLDE1858BD83D19C70
General Arduion tutorials = http://tronixstuff.wordpress.com
http://www.gammon.com.au/forum/bbshowpost.php?bbtopic_id=123

sirbow2

#2
Jan 20, 2012, 05:22 am Last Edit: Jan 20, 2012, 05:25 am by sirbow2 Reason: 1
you have to get the LSM303 and L3G4200D libraries from here: https://github.com/pololu/L3G4200D and https://github.com/pololu/LSM303

yes it reads the sensors correctly, but thats the easy part; its not combining the Gyro and ACC together correctly. <-harder part. i was going to implement the I2C my self, but im already learning enough new stuff in this project; i think ill wait till after the code works as it is, then go back and do I2C without the libraries(for gyro and ACC).

as of now i think the arctan and khalman functions are NOT functioning improperly. im sure someone out there in the wonderful world of arduino (lol) knows what im doing wrong. could you help?
http://dduino.blogspot.com all my Arduino/electronic projects!!!

{NEW} Getting Started, Learning, Reference + FAQ PDF!!:
[url]http://ar

cyclegadget


I can't personally help much but, maybe this link will help. P.S. it is spelled Kalman or Kalman Filter, I had to look it up myself.

http://arduino.cc/forum/index.php/topic,58048.0.html
Good links: Eagle tutorial= http://www.youtube.com/playlist?list=PLDE1858BD83D19C70
General Arduion tutorials = http://tronixstuff.wordpress.com
http://www.gammon.com.au/forum/bbshowpost.php?bbtopic_id=123

sren

Hello,
I am working with the MinIMU-9. It has got the same sensors.

On http://www.pololu.com/catalog/product/1264
you can get the librarys and a sample code + AHRS software.

I only invested some hours because I had not enough time for it, but my first results were looking promising.

Perhaps this code can help you ..

best regards

sirbow2



I can't personally help much but, maybe this link will help. P.S. it is spelled Kalman or Kalman Filter, I had to look it up myself.

http://arduino.cc/forum/index.php/topic,58048.0.html

hmm yes i saw that. but i have question. do i need to do the sensitivity stuff since im using a digital sensor?

thanks. it helped
http://dduino.blogspot.com all my Arduino/electronic projects!!!

{NEW} Getting Started, Learning, Reference + FAQ PDF!!:
[url]http://ar

cyclegadget

If we could assume for a moment that your code is working, there is a few things to try. Typically, when you get excessive response from a PID loop that is called overshoot. It can be solved in your code with these values:
Code: [Select]
//change for stability
#define Kp 7   //proportional
#define Ki 1   //Integral
#define Kd -5   //derivative


Try starting out with large changes in the values. I don't know what is allowed, but I would put a 30 in for Kp and then try a 60. If you see a change in performance it will give you a direction to go in.

Here is a description of each tuning parameter.:
Code: [Select]
Proportion = Error; //"makes a change to the output that is proportional to the current error value"
  Integral += Error; //"if you aren't quite getting to target, keep increasing over a period of time until you get there"
  Derivative = (Error - PrevError); //"how fast a change in angle occurs"


You might also want to google "how to tune a PID loop".
Good links: Eagle tutorial= http://www.youtube.com/playlist?list=PLDE1858BD83D19C70
General Arduion tutorials = http://tronixstuff.wordpress.com
http://www.gammon.com.au/forum/bbshowpost.php?bbtopic_id=123

sirbow2

#7
Jan 20, 2012, 10:15 pm Last Edit: Jan 20, 2012, 10:30 pm by sirbow2 Reason: 1
thanks for the explanation on PID tuning, but that doesn't matter right now as the the correct values arent getting to the PID loop.

thanks for the tuning tips though! :) i believe ive made some progress with the ACC angle calculations though. anyone feel free to comment :)

also, i may have had the pid loop incorrectly set at the time i noticed this, but what does it mean when the PID output lowers each time PID is ran even when the input value is the same?
http://dduino.blogspot.com all my Arduino/electronic projects!!!

{NEW} Getting Started, Learning, Reference + FAQ PDF!!:
[url]http://ar

cyclegadget


 
Code: [Select]
int GyroRate = 0;

I believe Gyro's return a figure based on rate of rotation. It is based on degrees rotation per second or some other time angle base. The result is when your device is not rotating the Gyro Rate is going to go to 0. Your PID uses both the GYRO and ACC to figure correction factor.

To put very simply it would be similar to saying PID = rate of rotation "Gyro" + angle of board based on ACC.
                                                                 
Good links: Eagle tutorial= http://www.youtube.com/playlist?list=PLDE1858BD83D19C70
General Arduion tutorials = http://tronixstuff.wordpress.com
http://www.gammon.com.au/forum/bbshowpost.php?bbtopic_id=123

PeterH


i wasnt really expecting the PID to work because i have to change Kp Ki and Kd for fine stabilization. but for some reason the Khalman equations didnt combine the GYRO and ACC correctly and the values were way off. if i hold the imu in one place, the value will stay at once place. but if i move the imu, the value jumps, and then stabilizes to value around what it should be, ish.


Does Kahlman assume the system is a linear system? I imagine that there are highly non-linear things going on inside a balance bot.

sirbow2



 
Code: [Select]
int GyroRate = 0;

I believe Gyro's return a figure based on rate of rotation. It is based on degrees rotation per second or some other time angle base. The result is when your device is not rotating the Gyro Rate is going to go to 0. Your PID uses both the GYRO and ACC to figure correction factor.

To put very simply it would be similar to saying PID = rate of rotation "Gyro" + angle of board based on ACC.
                                                                   

yeah um but its kalman that combines gyro+acc. PID finds the difference between what the value should be and what is and the last value to smooth output.
http://dduino.blogspot.com all my Arduino/electronic projects!!!

{NEW} Getting Started, Learning, Reference + FAQ PDF!!:
[url]http://ar

cyclegadget


  Sorry, I messed up. I meant to say... To put very simply it would be similar to saying error = rate of rotation "Gyro" + angle of board based on ACC. As the error decreases so does the PID correction.

I am going to load the code and try to test it with my board. I will see if I can figure this out.

I would like someone else to join in, as I am not a code expert. Most of my experience has been with Yokogawa, and Rosemont sensors, transmitters, and controllers. They do not require me to understand code at least for the things that I do.
Good links: Eagle tutorial= http://www.youtube.com/playlist?list=PLDE1858BD83D19C70
General Arduion tutorials = http://tronixstuff.wordpress.com
http://www.gammon.com.au/forum/bbshowpost.php?bbtopic_id=123

sirbow2

this is so weird! i copied the code for getting the y angle from here: http://arduino.cc/forum/index.php/topic,58048.0.html

Code: [Select]
R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector
  accYangle = acos(accYval/R)*RAD_TO_DEG-90;


since im using digital acc, it is already in raw g(gravity) output from the sensor, so if i put the x,y, and z values into the code above, it should give me a halfway(drift etc) accurate reading of the y angle!

here is the full code(calculates y angle and thats it, no servo PID or kalman code in it):
Code: [Select]
//-----Libraries------------------------------------------------//
#include <Wire.h>
#include <LSM303.h> //ACC, MAG(not used)
#include <L3G4200D.h> //gyro
#include <IRremote.h> //IR library
#include <Servo.h> //Servo support

//-----Robot Stuff------------------------------------------------//
int LmtrSpd=0; //stores motor speed value(0-180)
int RmtrSpd=0;
#define LServoMtrPin 2 
#define RServoMtrPin 3
Servo LServoMtr;
Servo RServoMtr;

//Loop refresh rate------------------------------------------------
int TimeTwo = 0;
int TimeOne = 0;
int LoopDelay = 0;  //later is modified to equal additional delay needed to make loop 30ms long
#define SetDelay 30 //how long loop should be

//IMU------------------------------------------------------------
//http://www.pololu.com/picture/view/0J3680
LSM303 compass;
L3G4200D gyro;

int Gyro_Y_Rate = 0;
int Acc_Y_Angle = 0;
int CalibSensor[4] = {
  0,0,0,0}; //for storing IMU centering values during setup, we only need gyro Y, ACC X and Z
int PlatformAngle = 0; //stores final angle in degree of the platform, used as input for PID
int CenterVal = 0; //balancing value for IMU
int yAngle =0; //used in y axis degree calculations

//---Setup---------------------------------------------------------//
void setup()
{
  Serial.begin(9600);

  //LM303 acc setup
  Wire.begin();
  compass.init();
  compass.enableDefault(); //enables acc LSM303 readings
  gyro.enableDefault(); //enables gyro L3G44200D readings

  IMUCenter(); //center IMU at current position

}

//--Loop------------------------------------------------------------
void loop()
{
  TimeOne = millis(); //recordes time at beginning of loop

  gyro.read(); //reads gyro
  compass.read(); //reads compass and acc(only acc used)

  IMUCalc(); //final current angle of the bot

  Serial.println(Acc_Y_Angle);

  TimeTwo = millis(); //recordes time at end of loop
  LoopDelay = SetDelay - (TimeTwo - TimeOne); //calculates delay needed to maintain SetDelay
  delay(LoopDelay); //delays needed time
}

void IMUCenter()
{
  gyro.read(); //reads gyro
  compass.read(); //reads compass and acc(only acc used)
 
  //store values for centering
  CalibSensor[0] = (int)gyro.g.y;
  CalibSensor[1] = (int)compass.a.x;
  CalibSensor[2] = (int)compass.a.y;
  CalibSensor[3] = (int)compass.a.z;
}

int IMUCalc()
{
  //IMU------------------------------------------------------------ 
  Acc_Y_Angle = YAngleCalc(((int)compass.a.x - CalibSensor[1]), ((int)compass.a.y - CalibSensor[2]), ((int)compass.a.z - CalibSensor[3]));  //calculates acc angle                                       
  Gyro_Y_Rate = (int)gyro.g.y - CalibSensor[1];   //reads gyro rate from IMU using library                                       
}

int YAngleCalc(int x, int y, int z) //finds Acc angle
{
   
  yAngle = acos(y/(sqrt(pow(x,2)+pow(y,2)+pow(z,2))))*RAD_TO_DEG-90; //force vector + degree format

  if(z < 0)//360 degrees
  {   
    if(yAngle < 0)
    {
      yAngle = -180-yAngle;
    }
    else
    {
      yAngle = 180-yAngle;
    }
  }
  return yAngle;
}
http://dduino.blogspot.com all my Arduino/electronic projects!!!

{NEW} Getting Started, Learning, Reference + FAQ PDF!!:
[url]http://ar

cyclegadget


I ran your sketch without the IR library and I found some confusing output. I added some Serial.print to show both platform angle "PA" and Output "OP". I looks kind of like a roll over problems using INT, I am not sure. Look at how the output jumps form - to +. I am holding the board steady the whole time. This is after a long count up in PA and OP.
Code: [Select]
PA 337
OP -31504
 
PA 333
OP -31819
 
PA 328
OP -32117
 
PA 325
OP -32411
 
PA 321
OP -32709
 
PA 318
OP 32535
 
PA 312
OP 32250
 
PA 307
OP 31983
 
PA 302
Good links: Eagle tutorial= http://www.youtube.com/playlist?list=PLDE1858BD83D19C70
General Arduion tutorials = http://tronixstuff.wordpress.com
http://www.gammon.com.au/forum/bbshowpost.php?bbtopic_id=123

cyclegadget


Here is your code with the IR part commented out and some Serial.prints added to see PA an OP. I also raised the baud rate up to 56700.
Code: [Select]
//-----Libraries------------------------------------------------//
#include <Wire.h>
#include <LSM303.h> //ACC, MAG(not used)
#include <L3G4200D.h> //gyro
//#include <IRremote.h> //IR library
#include <Servo.h> //Servo support

//-----Robot Stuff------------------------------------------------//
int LmtrSpd=0; //stores motor speed value(0-180)
int RmtrSpd=0;
#define LServoMtrPin 2 
#define RServoMtrPin 3
Servo LServoMtr;
Servo RServoMtr;

//--PID------------------------------------------------------------
//awesome explanation: http://www.x-firm.com/?page_id=193
#define offset 90 //motor center

//change for stability
#define Kp 7   //proportional
#define Ki 1   //Integral
#define Kd -5   //derivative

//for storing calculated values later on
int Error = 0; //Error
int Proportion = 0; //value to store the Proportional value from PID equation
int Integral = 0; //value to store the ... value from PID equation
int Derivative = 0; //value to store the ... value from PID equation
int Output = 0; //ouput based on PID
int PrevError = 0; //stores error for next loop + PID

//Loop refresh rate------------------------------------------------
int TimeTwo = 0;
int TimeOne = 0;
int LoopDelay = 0;  //later is modified to equal additional delay needed to make loop 30ms long
#define SetDelay 30 //how long loop should be

//IMU------------------------------------------------------------
//http://www.pololu.com/picture/view/0J3680
LSM303 compass;
L3G4200D gyro;

int GyroRate = 0;
int AccAngle = 0;
int CalibSensor[3] = {0,0,0}; //for storing IMU centering values during setup, we only need gyro Y, ACC X and Z
int PlatformAngle = 0; //stores final angle in degree of the platform, used as input for PID
int CenterVal = 0; //balancing value for IMU

// --- Kalman filter module  ----------------------------------------------------------
float Q_angle  =  0.001; //0.001
float Q_gyro   =  0.003;  //0.003
float R_angle  =  0.03;  //0.03

float x_angle = 0;
float x_bias = 0;
float y = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float dt, z, S;
float K_0, K_1;

//---Setup---------------------------------------------------------//
void setup()
{
  Serial.begin(57600);
  delay(1000);

  //LM303 acc setup
  Wire.begin();
  compass.init();
  compass.enableDefault(); //enables acc LSM303 readings
  gyro.enableDefault(); //enables gyro L3G44200D readings
  delay(500);
  LServoMtr.attach(LServoMtrPin); //attach then set servos to center
  LServoMtr.write(90);
  RServoMtr.attach(RServoMtrPin);
  RServoMtr.write(90);

  IMUCenter(); //center IMU at current position
  delay(2000);

}

//--Loop------------------------------------------------------------
void loop()
{
  TimeOne = millis(); //recordes time at beginning of loop

  gyro.read(); //reads gyro
  compass.read(); //reads compass and acc(only acc used)

  PlatformAngle = IMUCalc(); //final current angle of the bot

  Output = PIDCalc(); //calculates how much plaform has tilted, smoothes and adjusts motor output accordingly
  Serial.println("  ");
  Serial.print("PA ");
  Serial.println(PlatformAngle);
  Serial.print("OP ");
  Serial.println(Output);
 
 
  LServoMtr.write(Output + offset);
  RServoMtr.write(Output + offset);

  TimeTwo = millis(); //recordes time at end of loop
  LoopDelay = SetDelay - (TimeTwo - TimeOne); //calculates delay needed to maintain SetDelay
  delay(LoopDelay); //delays needed time
}

int IMUCalc()
{
  //IMU------------------------------------------------------------ 
  AccAngle = arctan2(((int)compass.a.z - CalibSensor[3]), ((int)compass.a.x - CalibSensor[2]));  //calculates acc angle                                       
  GyroRate = (int)gyro.g.y - CalibSensor[1];   //reads gyro rate from IMU using library                                       
  return kalmanCalculate(AccAngle, GyroRate, SetDelay); // calculate combined Angle
}

void IMUCenter()
{
  gyro.read(); //reads gyro
  compass.read(); //reads compass and acc(only acc used)
  CalibSensor[1] = (int)gyro.g.y; //store values for centering
  CalibSensor[2] = (int)compass.a.x;
  CalibSensor[3] = (int)compass.a.z;
}

int PIDCalc()
{
  //PID-------------------------------------------------------------
  Error = CenterVal - PlatformAngle; //finds error between the wanted value and what it currently is.
  Proportion = Error; //"makes a change to the output that is proportional to the current error value"
  Integral += Error; //"if you aren't quite getting to target, keep increasing over a period of time until you get there"
  Derivative = (Error - PrevError); //"how fast a change in angle occurs"

  //calculates the final adjusted output based on Ki,Kp,Kd set in the beginning of
  //the program and the resulting values from Proportion, Integral, and Derivative calculations
  Output = (Kp * Proportion) + (Ki * Integral) + (Kd * Derivative);
  PrevError = Error; //puts current error in another value for next loop to use.
  return Output;
}

int arctan2(int z, int x) //finds Acc angle
{
  int coeff_1 = 128;                                       
  int coeff_2 = 3*coeff_1;
  float abs_z = abs(z)+1e-10;
  float r, angle;

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

float kalmanCalculate(float newAngle, float newRate,int looptime) //combines Gyro rate and Acc angle, no clue how it works :)
{
  dt = float(SetDelay)/1000;
  x_angle += dt * (newRate - x_bias);
  P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
  P_01 +=  - dt * P_11;
  P_10 +=  - dt * P_11;
  P_11 +=  + Q_gyro * dt;

  y = newAngle - x_angle;
  S = P_00 + R_angle;
  K_0 = P_00 / S;
  K_1 = P_10 / S;

  x_angle +=  K_0 * y;
  x_bias  +=  K_1 * y;
  P_00 -= K_0 * P_00;
  P_01 -= K_0 * P_01;
  P_10 -= K_1 * P_00;
  P_11 -= K_1 * P_01;

  return x_angle;
}
Good links: Eagle tutorial= http://www.youtube.com/playlist?list=PLDE1858BD83D19C70
General Arduion tutorials = http://tronixstuff.wordpress.com
http://www.gammon.com.au/forum/bbshowpost.php?bbtopic_id=123

Go Up