Hi Lauszus,
I make a little customisation of your source for digital IMUs so it can work with Minimu-9 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/cgi-bin/yabb2/YaBB.pl?num=1284738418
// See also http://www.x-firm.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/cgi-bin/yabb2/YaBB.pl?num=1284738418
// See also http://www.x-firm.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.