Балансировщик

> тут даже дело не в матапарате, а в том чтобы быстро считать и непонятно, чорт побери как он узнает в каком положении элементы.

Чтобы узнавать положения элементов, достаточно в узлах поставить потенциометры (или для полной крутизы, чтоб по-взрослому - датчик угловой скорости). Быстро считать - сейчас с этим явно проблем нету. Так что дело тут именно в мат.аппарате. Потому что система - существенно нелинейная и довольно высокого порядка.