Hello all
I started a little gimbal project using LSM303DLHC accelerometer & magnetometer, and L3GD20 gyroscope...

So far, the roll and pitch axis works perfect i should say, except with the yaw axis, wich tends to drift by +1 degree every second
Tryied diferent sampleing variants, and the result is the same, i even try to use only the accelerometer values in the imu filters, and the result is the same.

Here is my filter code using MadgwickAHRS

void MadgwickAHRSupdateIMU22(float gx, float gy, float gz, float ax, float ay, float az ) {

float sampleFreq 200.0f;

float recipNorm;
float s0, s1, s2, s3;
float s0a, s1a, s2a, s3a;
float az1,ay1,ax1;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;

// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);


// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

// Normalise accelerometer measurement

recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax1 = ax*recipNorm;
ay1 = ay*recipNorm;
az1 = az*recipNorm;

// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;

// Gradient decent algorithm corrective step

s0 = (_4q0 * pow(q2,2)) + (_2q2 * ax1) + (_4q0 * pow(q1,2)) -( _2q1 * ay1);
s1 = _4q1 * pow(q3,2) - _2q3 * ax1 + 4.0f * pow(q0,2) * q1 -( _2q0 * ay1) - _4q1 + (_8q1 * pow(q1,2)) +( _8q1 * pow(q2,2)) +( _4q1 * az1);
s2 = (4.0f * pow(q0,2)* q2) + _2q0 * ax1 + _4q2 * pow(q3,2) -( _2q3 * ay1) - _4q2 + (_8q2 * pow(q1,2)) + (_8q2 * pow(q2,2)) +( _4q2 * az1);
s3 = (4.0f * pow(q1,2)* q3)- _2q1 * ax1 + 4.0f * pow(q2,2) * q3 - _2q2 * ay1;
recipNorm = invSqrt(pow(s0,2)+pow(s1,2)+pow(s2,2)+pow(s3,2)); // normalise step magnitude
s0a = s0*recipNorm;
s1a = s1*recipNorm;
s2a = s2*recipNorm;
s3a = s3*recipNorm;

// Apply feedback step
qDot1 -= beta * s0a;
qDot2 -= beta * s1a;
qDot3 -= beta * s2a;
qDot4 -= beta * s3a;
}

// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 *(1.0f / sampleFreq);
q1 += qDot2 *(1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);

// Normalise quaternion
float qp01=pow(q0,2)+pow(q1,2);
float qp23=pow(q2,2)+pow(q3,2);
recipNorm = invSqrt(qp01+qp23);

q0x = q0*recipNorm;
q1x = q1*recipNorm;
q2x = q2*recipNorm;
q3x = q3*recipNorm;

}



and my main code loop is bellow

/* 0 - x, 1 -y, 2 - z */
float fgyroXYZ[3];
float faccXYZ[3];

timer1 @ 100hz

L3gd20ReadAngRate(fgyroXYZ); // reads gyro axes
Lsm303dlhcAccReadAcc(faccXYZ); // reads acc axes

timer2 @200hz

MadgwickAHRSupdateIMU22(fgyroXYZ[0]*3.141592/180.0, fgyroXYZ[1]*3.141592/180.0, fgyroXYZ[2]*3.141592/180.0, faccXYZ[0], faccXYZ[1], faccXYZ[2],0.02);


froll = atan2( 2 * (q0x * q1x + q2x * q3x), pow(q0x,2) - pow(q1x,2) - pow(q2x,2) + pow(q3x,2) );
fpitch = -asin( 2 * (q1x * q3x - q0x * q2x) );
fyaw = atan2( 2 * (q1x * q2x) + (q0x * q3x), pow(q0x,2)+pow(q1x,2) - pow(q2x,2) - pow(q3x,2));


I cant explain why froll and fpitch works well and the fyaw dont.... they all three derive from the same quaternion...
Any ideeas?