2 Wheel Self balance + MPU6050 IMU: filter angle without using DMP ?

Hello everyone ! I'm doing my final year's thesis about self balance robot, using Arduino Uno and IMU MPU6050. I've tried PID controller and Kalman filter to filter the raw readings from the sensor, but still could not get my robot to balance at all. I've done some research on google, but as from what i read, most people use the DMP in the Arduino sample code to get angle. I've tried using the angle from DMP too, and it was indeed successful, but when i switch back to my Kalman-filtered angle, the robot failed. The PID code is still the same, so i guess the problem is that the angle isn't filtered correctly. The Kalman filter i used is based on this article of Kristian Sloth Lauszus:

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

Can anyone give me the source code, or some guide to get the angle correct without using the DMP on MPU6050 ? Many thanks !

That particular "Kalman filter" code is simply wrong and although it does work (rather poorly) in some situations, no one is quite sure why. Read more about it here.

For absolute orientation, RTIMUlib works very well, even on an Arduino, and I believe it accommodates your sensor.

I used dmp instead of kalman filtering. See the full balancing code in attachment. Ask for any doubts, I’ll help you.

arnital.ino (12.5 KB)