I know you are going to get swarmed for your code for the Kalman, but I am really struggling getting this to work. Can you detail how you went about this? Was the programming intensive? ...
Hi Gibby623, sorry, I just got your PM's
If your robot rolls about the x axis, you only need GYR_X, ACC_Y and ACC_Z
I notice that signal quality is much better when the sensor is upward, apparently, the IMU does not appreciate to be up side down
I will publish the full code pretty soon
here is the Kalman module:
// Kalman filter module
float Q_angle = 0.001;
float Q_gyro = 0.003;
float R_angle = 0.03;
float x_angle = 0;
float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float dt, y, S;
float K_0, K_1;
float kalmanCalculate(float newAngle, float newRate,int looptime) {
dt = float(looptime)/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;
}
The function parameters:
- newAngle = ACC_angle (Accelerometers angle)
- newRate = GYRO_rate (Gyro angle rate)
- looptime = lastLoopTime (time between sensors measurement)
More info this week end, lets keep it public