I copy pasted the code fromhttp://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
Overall it looks good. But when i turn on the motor, both acc and gyro raw readings oscillate pretty dramatically. The more serious is the gyro, because, according to what i understand, the kalman impl basically tries to reconcile/ fuse the curve from acc and gyro. So it can't be smoother than the gyro (granted that the acc is usually much worse in high frequency noise). But now my gyro alsi has such a bad noise when motor is on (wuth the high freq noise from gears), even the integration results from gyro show more than 5 degrees of oscillation. This means even when the robot is still, it still thinks it's +-5 degrees tilted. So i cant maintain the balance angle within 5 degrees.
I am now trying to do either mechanical damping (foam buffer) or a pre- filtering step on gyro before kalman
Did u find similar problem with gyro high- freq noise? Mine is mpu6050, used a sparkfun itg thing before, seems to have better noise but that one triggers system freeze when i run motor at high speed