Offline
Newbie
Karma: 0
Posts: 39


« Reply #390 on: February 11, 2013, 02:41:44 pm » 
Hi Lauszus,
I've been learning a lot from your code, thanks very much for putting it on here.
I have a board with single axis gyro with 3 axis accelerometer, analogue outputs.
I'm looking to find the Roll angle, so the single gyro board will need to be orientated up on its end so that the Roll axis goes through the middle of the gyro. The accelerometers with then be: X=up/down, Y=left/right, Z=forward/backward.
In this configuration which combination of the X,Y and Z accelerometers should I be using in the code?
I am guessing that it's the X and Y as the Z axis accelerometer is not effected by roll when the board is mounted on it's end
Your code uses X and Z and Y and Z in the arctan function, so I should use X and Y, correct?



Logged






Offline
Newbie
Karma: 0
Posts: 39


« Reply #392 on: February 11, 2013, 06:29:15 pm » 
Hi Lauszus,
I have your code running ok with the single gyro and x,y,z accelerometers.
I'll dump the intermediate angle calculation values out and see if I can figure which pair I need to combine with the gyro.
Cheers,
Richard.



Logged





Offline
Newbie
Karma: 0
Posts: 39


« Reply #393 on: February 12, 2013, 04:32:01 am » 
Hi Lauszus,
All sorted out now. I had everything coded correctly but missed the initial offset from zero for the down axis accelerometer due to gravity.
It works very well and rejects linear movements really well.
Just need to feed the angle into my motor controller now.
Thanks for everything!



Logged





morocco
Offline
Newbie
Karma: 0
Posts: 14


« Reply #394 on: February 12, 2013, 03:34:42 pm » 
please , can you share with us all the program?



Logged





Offline
Newbie
Karma: 0
Posts: 12


« Reply #395 on: February 18, 2013, 11:03:27 am » 
i was able to implement the filter thanks for this guide it was really helpful...thank you very much but i dont know how to relate these values to motor's rpm of a quadcopter so i will stabilize, can anybody post links to guides like this?



Logged





Offline
Newbie
Karma: 0
Posts: 12


« Reply #396 on: February 18, 2013, 11:41:36 am » 
i was able to implement the filter thanks for this guide it was really helpful...thank you very much but i dont know how to relate these values to motor's rpm of a quadcopter so i will stabilize, can anybody post links to guides like this?
You will need to use a PID controller to give roll,pitch,yaw outputs when the orientation deviates from 0,0,0. However, I have used these filters on a quadcopter and they suffer from the problem of linear acceleration affecting the pitch and roll. The filters are fast enough, smooth and very stable (calculation time was 80micros for twin Kalmans on an Arduino Due) but you'd probably be better off using Mahonys DCM or Madgwicks filter  they're less sensitive to linear acceleration but a bit more prone to noise (vibration from the motors). You'll find plenty of links via Google for any of this stuff.


« Last Edit: February 18, 2013, 11:46:50 am by AntR »

Logged





Offline
Newbie
Karma: 0
Posts: 6


« Reply #397 on: February 18, 2013, 12:12:20 pm » 
i want to work with MPU6050Triple Axis Accelerometer Gyro can it be done



Logged





Offline
Newbie
Karma: 0
Posts: 6


« Reply #398 on: February 18, 2013, 12:30:23 pm » 
i want the same guide for MPU6050Triple Axis Accelerometer Gyro



Logged





morocco
Offline
Newbie
Karma: 0
Posts: 14


« Reply #399 on: February 20, 2013, 07:52:34 am » 
the same think , so my ,i'd like this idea and i would like someone to share us the GUIDE of IMU 6050 and arduino



Logged





Denmark
Offline
Sr. Member
Karma: 10
Posts: 287


« Reply #400 on: February 20, 2013, 08:44:56 am » 
Hi, The setup is very similar just note that if you are using an Arduino running at 5V you might damage the IMU as it's only running at 3.3V, but some breakout boards have a logic level circuit builtin. If not use have to use a logic level converter like this one: https://www.sparkfun.com/products/8745. To connect the Arduino with the MPU6050, you need to connect it as it says in the following comment: https://www.sparkfun.com/products/11028#comment50b3c165ce395f484d000000. Hopes this helps you out for now, as I havn't got time to write a guide at the moment. Regards Lauszus


« Last Edit: March 28, 2013, 12:01:38 pm by Lauszus »

Logged





Offline
Newbie
Karma: 1
Posts: 48


« Reply #401 on: March 13, 2013, 12:48:40 pm » 
Hi Lauszus, I make a little customisation of your source for digital IMUs so it can work with Minimu9 board , hope you don't mind.But there is something wrong here is what i get: ....(the board is on the ground and i don't touch it the whole time) X: 180.00 Y: 180.01 X: 180.05 Y: 180.06 X: 180.22 Y: 180.14 X: 180.51 Y: 180.16 X: 180.82 Y: 180.11 X: 181.07 Y: 180.04 X: 181.42 Y: 179.92 X: 181.79 Y: 179.80 X: 182.22 Y: 179.65 X: 182.72 Y: 179.46 X: 183.23 Y: 179.28 X: 183.85 Y: 179.08 X: 184.54 Y: 178.80 X: 185.26 Y: 178.48 X: 185.99 Y: 178.02 X: 186.60 Y: 177.45 X: 187.54 Y: 176.95 X: 188.54 Y: 176.43 X: 189.64 Y: 175.72 X: 190.82 Y: 175.03 X: 192.13 Y: 174.37 ...... X: 280.48 Y: 86.86 X: 280.43 Y: 86.66 X: 280.32 Y: 86.91 X: 280.19 Y: 87.02 X: 280.09 Y: 87.10 X: 279.97 Y: 87.32 X: 279.81 Y: 87.84 X: 279.71 Y: 87.88 X: 279.60 Y: 88.26 X: 279.51 Y: 88.09 X: 279.38 Y: 88.20 X: 279.27 Y: 88.09 X: 279.10 Y: 88.24 X: 278.96 Y: 88.26 X: 278.80 Y: 88.56 ...(finally the X stopped growing and the Y stopped getting lower) X: 278.61 Y: 89.11 X: 278.44 Y: 89.23 X: 278.29 Y: 89.41 X: 278.25 Y: 88.81 X: 278.07 Y: 89.24 X: 277.85 Y: 89.62 X: 277.68 Y: 89.88 X: 277.53 Y: 90.03 X: 277.36 Y: 90.22 X: 277.24 Y: 90.48 X: 277.12 Y: 90.17 X: 276.99 Y: 90.08 X: 276.90 Y: 89.87 X: 276.76 Y: 89.84 X: 276.61 Y: 89.97 X: 276.44 Y: 90.35 X: 276.26 Y: 90.62 X: 276.12 Y: 90.84 X: 276.02 Y: 90.76 X: 275.78 Y: 91.38 X: 275.67 Y: 91.39 X: 275.47 Y: 91.74 X: 275.32 Y: 91.92 X: 275.16 Y: 92.18 X: 275.07 Y: 92.02 X: 274.95 Y: 92.17 X: 274.83 Y: 92.30 X: 274.68 Y: 92.31 X: 274.52 Y: 92.53 X: 274.43 Y: 92.53 X: 274.27 Y: 92.90 X: 274.18 Y: 92.90 X: 274.03 Y: 92.95 ..(and all over again) and here is my source #include <Wire.h> #include <L3G.h> #include <LSM303.h>
L3G gyro; LSM303 compass; double zeroValue[5] = {200, 44, 660, 18.5, 52.3}; // Found by experimenting
/* All the angles start at 180 degrees */ double gyroXangle = 180; double gyroYangle = 180;
double compAngleX = 180; double compAngleY = 180;
unsigned long timer; /* Kalman filter variables and constants */ const double Q_angleX = 0.001; // Process noise covariance for the accelerometer  Sw const double Q_gyroX = 0.003; // Process noise covariance for the gyro  Sw const double R_angleX = 0.03; // Measurement noise covariance  Sv
double angleX = 180; // The angle output from the Kalman filter double biasX = 0; // The gyro bias calculated by the Kalman filter double PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0; double dtX, yX, SX; double KX_0, KX_1;
double kalmanX(double newAngle, double newRate, double dtime) { // KasBot V2  Kalman filter module  http://www.arduino.cc/cgibin/yabb2/YaBB.pl?num=1284738418 // See also http://www.xfirm.com/?page_id=145 // with slightly modifications by Kristian Lauszus // See http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf and // http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf for more information dtX = dtime / 1000000; // Convert from microseconds to seconds
// Discrete Kalman filter time update equations  Time Update ("Predict") // Update xhat  Project the state ahead angleX += dtX * (newRate  biasX);
// Update estimation error covariance  Project the error covariance ahead 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;
// Discrete Kalman filter measurement update equations  Measurement Update ("Correct") // Calculate Kalman gain  Compute the Kalman gain SX = PX_00 + R_angleX; KX_0 = PX_00 / SX; KX_1 = PX_10 / SX;
// Calculate angle and resting rate  Update estimate with measurement zk yX = newAngle  angleX; angleX += KX_0 * yX; biasX += KX_1 * yX;
// Calculate estimation error covariance  Update the error covariance 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 angleX; } /* Kalman filter variables and constants */ const double Q_angleY = 0.001; // Process noise covariance for the accelerometer  Sw const double Q_gyroY = 0.003; // Process noise covariance for the gyro  Sw const double R_angleY = 0.03; // Measurement noise covariance  Sv
double angleY = 180; // The angle output from the Kalman filter double biasY = 0; // The gyro bias calculated by the Kalman filter double PY_00 = 0, PY_01 = 0, PY_10 = 0, PY_11 = 0; double dtY, yY, SY; double KY_0, KY_1;
double kalmanY(double newAngle, double newRate, double dtime) { // KasBot V2  Kalman filter module  http://www.arduino.cc/cgibin/yabb2/YaBB.pl?num=1284738418 // See also http://www.xfirm.com/?page_id=145 // with slightly modifications by Kristian Lauszus // See http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf and // http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf for more information dtY = dtime / 1000000; // Convert from microseconds to seconds
// Discrete Kalman filter time update equations  Time Update ("Predict") // Update xhat  Project the state ahead angleY += dtY * (newRate  biasY);
// Update estimation error covariance  Project the error covariance ahead 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;
// Discrete Kalman filter measurement update equations  Measurement Update ("Correct") // Calculate Kalman gain  Compute the Kalman gain SY = PY_00 + R_angleY; KY_0 = PY_00 / SY; KY_1 = PY_10 / SY;
// Calculate angle and resting rate  Update estimate with measurement zk yY = newAngle  angleY; angleY += KY_0 * yY; biasY += KY_1 * yY;
// Calculate estimation error covariance  Update the error covariance 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 angleY; } int readGyroX(void) { int data; data =gyro.g.x;
return data; } int readGyroY(void) { int data; data = gyro.g.y;
return data; } double getXangle() { double accXval = (double)readAccX()zeroValue[0]; double accZval = (double)readAccZ()zeroValue[2]; double angle = (atan2(accXval,accZval)+PI)*RAD_TO_DEG; return angle; } double getYangle() { double accYval = (double)readAccY()zeroValue[1]; double accZval = (double)readAccZ()zeroValue[2]; double angle = (atan2(accYval,accZval)+PI)*RAD_TO_DEG; return angle; } int readAccX(void) { int data; data = compass.a.x; return data; } int readAccY(void) { int data; data = compass.a.y; return data; } int readAccZ(void) { int data; data =compass.a.z; return data;
}
void setup() { Serial.begin(9600); Wire.begin(); if (!gyro.init()) { Serial.println("Failed to autodetect gyro type!"); while (1); }
gyro.enableDefault(); compass.init(); compass.enableDefault(); timer = micros(); }
void loop() { gyro.read(); compass.read(); double gyroXrate = (((double)readGyroX()zeroValue[3])/14.375); gyroXangle += gyroXrate*((double)(micros()timer)/1000000); // Without any filter double gyroYrate = (((double)readGyroY()zeroValue[4])/14.375); gyroYangle += gyroYrate*((double)(micros()timer)/1000000); // Without any filter double accXangle = getXangle(); double accYangle = getYangle();
compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()timer)/1000000)))+(0.07*accXangle); compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()timer)/1000000)))+(0.07*accYangle); double xAngle = kalmanX(accXangle, gyroXrate, (double)(micros()  timer)); // calculate the angle using a Kalman filter double yAngle = kalmanY(accYangle, gyroYrate, (double)(micros()  timer)); // calculate the angle using a Kalman filter timer = micros();
Serial.print("X: ");Serial.print(xAngle);Serial.print("\t"); Serial.print("Y: ");Serial.print(yAngle);Serial.print("\t"); Serial.print("\n"); delay(10);
}
Could you please take a look of my source and tell me what I am doing wrong.I want to thank you in advance for your time. Stanislav.



Logged





Denmark
Offline
Sr. Member
Karma: 10
Posts: 287


« Reply #402 on: March 13, 2013, 06:42:06 pm » 
@stunito0o0 It looks like you are reading the magnetometer (compass) and not the accelerometer. Btw the newest version of the code can be found here: https://github.com/TKJElectronics/KalmanFilter.



Logged





Offline
Newbie
Karma: 1
Posts: 48


« Reply #403 on: March 14, 2013, 04:31:07 am » 
Hi again, I thought too that it might be the compass data I am reading so i opened the example for the "LSM303.h" libraly and here is the source: #include <Wire.h> #include <LSM303.h>
LSM303 compass;
void setup() { Serial.begin(9600); Wire.begin(); compass.init(); compass.enableDefault(); }
void loop() { compass.read();
Serial.print("A "); Serial.print("X: "); Serial.print((int)compass.a.x); Serial.print(" Y: "); Serial.print((int)compass.a.y); Serial.print(" Z: "); Serial.print((int)compass.a.z);
Serial.print(" M "); Serial.print("X: "); Serial.print((int)compass.m.x); Serial.print(" Y: "); Serial.print((int)compass.m.y); Serial.print(" Z: "); Serial.println((int)compass.m.z);
delay(100); } According to the example "compass.a.x","compass.a.y" and "compass.a.z" are for reading the acc data.I will really appreciate if you take another look of my code.I think that when i was customising your source i might have deleted something important.Thank you again for your time. P.S. Now i rememberd that i deleted part of the source for getting the data of acc and gyro.For example "readAccX" function was something like this: int readAccX(void) { int data; data = i2cRead(adxlAddress, 0x32); data = i2cRead(adxlAddress,0x33)<<8; return data; } and i didn't understand what "data = i2cRead(adxlAddress,0x33)<<8;" is for so i deleted it. now it looks like this: int readAccX(void) { int data; data = compass.a.x; return data; }


« Last Edit: March 14, 2013, 04:33:43 am by stunito0o0 »

Logged






