Ich habe ja auch mit in meinem DIYSegway einen MPU6050 drin ich habe sehr gute Erfahrungen mit dem Kalmanfilter aus dieser Quelle.
KalmanFilter
Da gibt es auch im Sketch Serielle ausgaben die man einstellen kann probiere es damit aus dann siehst du ob es an deiner Hardware liegt oder an der Software.
Gruß
DerDani