@NRicola ich hab mir gerade deine Formeln durchgeschaut (für die einfache Linearisierung) und schon mal keinen Fehler gefunden. Abgesehen davon lässt sie sich sogar sehr einfach berechnen, sofern der µC sinus und cosinus funktionen sowie die umkehrfunktionen davon kann.
Den, den Block (B²+D²-C²)/(2BD) kann man recht einfach offline vorberechnen bzw. einmal berechnen und immer einsetzten.