Come volevasi dimostrare, ti mancano gli strumenti matematici, ma anche la fisica, per una cosa di questo genere, tieni presente che ottenere l'asseto 3D, anche se parliamo solamente dell'inclinazione, è una cosa che richiede tanta matematica di livello superiore.
Ti consiglio di "rovistare" dentro MultiWii, software per quadricotteri, che supporta i sensori della tua IMU ed estrapolare le sue routine per integrarle nel tuo software, non sono un gran che visto che implementano solo la parte generica dei vari algoritmi, ovvero non sono ottimizzate, però funzionano decentemente e per il tuo uso dovrebbero bastare-