Da hast du ja einen schönen Balancer gebaut!
Mittlerweile verwende ich auch den MPU6050. Interessant ist die DMP Berechnung die im Sensor inbegriffen ist. Damit kann man die Filterung direkt am Sensor machen und spart das dann am Controller ein.
Welchen Motor hast du da verbaut? Das erkennt man am Video so schlecht.