Hello all, i am trying to build a quadcopter using stm32F407 32bit microcontroler.
My issue is kinda strange since the roll and pitch axis are working corect .
I use L3GD20 gyro sensor and LSM303DLHC accelerometer (i`m not using the magnetometer)
The gyro output rate is at 400hz and 250DPS
The accelerometer rate is at 400hz and 2G
I sample both the acc and gyro with a timer at 200hz ( 0.005 seconds),
then i sample the Madgwick with gyro and acc values in another timer at 200hz
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
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(pow(ax,2)+pow(ay,2) + pow(az,2));
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;
//q0q0 = q0 * q0;
//q1q1 = q1 * q1;
//q2q2 = q2 * q2;
//q3q3 = q3 * q3;
// 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 *dt;
q1 += qDot2 *dt;
q2 += qDot3 *dt;
q3 += qDot4 *dt;
// Normalise quaternion
recipNorm = invSqrt(pow(q0,2)+pow(q1,2)+pow(q2,2)+pow(q3,2));
q0x = q0*recipNorm;
q1x = q1*recipNorm;
q2x = q2*recipNorm;
q3x = q3*recipNorm;
}
//====================================================================================================
// END OF CODE FOR IMU FILTER
//====================================================================================================
And in my main program i have the followings
0 is X
1 is Y
2 is Z
L3gd20ReadAngRate(fgyroXYZ);
Lsm303dlhcAccReadAcc(faccXYZ);
MadgwickAHRSupdateIMU(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));
Why is the fyaw axis drifts with about 1 degree per second, at some point it starts to decrees and so on...
The same thing happens if i put only the acelerometer values in the IMU filter...
What could make such yaw drift?
Tryied even the IMU version of Mahonys with same result, even the variants with magnetic inputs...
Normaly wouldn`t the yaw be ok? if my pitch and roll are ok? Since they all three come from the same quaternion ?
Thanks
Replies
You have only gyro and acc. Acc is going to be used for compensating the drifts along roll and pitch axis, while for yaw axis, there is no information available for compensation. No doubt it will show a drift since actually you are just use gyro-z for integration, even the smallest error at the beginning will be enlarged after several seconds.
You can add a 3-axis magnetometer for yaw drift compensation or you can just fly using gyro and acc, but in that way, you cannot lock the head of your copter.
Thank you for the info, i managed to do some offset calibration for the gyroscope and now it rates 1 degree/second at 5 minutes, much better...
I was thinking to compensate with a kalman filter
does anyone has some good reference for a kalman filter implementation?
if you have the mathematical basis about matrix theory and statistics, you can google "kalman filter UNC" which may help you. Otherwise, if you don't wanna go deep into mathematics, maybe MadgwickAHRS is good enough for you to build a Quadcopter.
One more thing to mention that, when using MadgwickAHRS, you need to consider what should be done if there is a constant and large enough linear acceleration. The normal MadgwickAHRS does not take that into consideration, which may cause problem if you make your Quadcopter fly for several seconds...