Das hört sich nach der Regelung eines instabilen Systems an.
Die üblichen Werkzeuge dafür sind PID-Regler und, wenn schon die Messwert-Erfassung Probleme macht, Kalman-Filter.
Auch musst du Messwert-Erfassung und Regelung gedanklich trennen und zunächst die Messwert-Erfassung sauber in den Griff kriegen. Dann klären, wie du die Stellgröße (Servo-Ansteuerung) auf dein mechanisches System rüberbringst, möglichst direkt und ohne Totzonen und -zeiten. Danach kannst du Regelungstechnik studieren, P und I Anteile ermitteln und deinem "Balancer" das Balancieren beibringen. Viel Spaß !