Lauszus, I was stuck on page 17 and thought you never replied! I am so silly!
Anyway, thanks for the support. Look at what I have till now :
I am using complementary filter.
I modified the values to take more of the gyro output because if I use the defaults
the robot gets lazy and needs big tilt before trying to react.
How can I also modify the kalman filter to take more out of the gyro?
I think the secret is here... Right?
float Q_angle = 0.001; //0.001
float Q_gyro = 0.003; //0.003
float R_angle = 0.03; //0.03
But which is which???