Pre- smoothing gyro before Kalman filter?

I copy pasted the code from

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

I copy pasted the code from

You are wasting your time with that link, as the system model is wrong. The code simply can't work properly (and the author admits that he doesn't understand the material). A critique of an earlier, even worse attempt is posted here, but basically, the gyro is handled completely incorrectly.

You should look for code that uses a complementary filter, as for a balance bot, a viable Kalman filter alternative in C/C++ doesn't seem to be readily available.