Neigungskompensation bei Abstandsmessung

An einem kleinen Quadrokopter habe ich einen Ultraschall-Abstandsmesser, 
der die Flughöhe messen soll.
Das Klappt prinzipiell ganz gut, aaaaber: Um mich vorwärts zu bewegen, 
muss ich das Gerät ja neigen, der gemessene Abstand wird als bei 
gleicher Flughöhe größer.
Aus dem Kompass bekomme ich Pitch- und Rollwinkel.
Solange ich nur vorwärts fliege, kann ich ganz einfach mit dem Cosinus 
meine tatsächliche Höhe über dem Boden berechnen. Klappt schon gut.

Nun möchte ich aber auch in der Kurve meine Flughöhe korrekt bestimmen, 
wenn das Gerät um Pitch- und Rollachse geneigt ist.
Da habe ich gerade ein Problem.

Wie muss ich da vorgehen? In Sachen Vektorrechnung bin ich leider nicht 
mehr ganz fit, ist schon lange her.
Im Grunde muss ich wohl den Schnittwinkel zwischen der Pitch-Roll-Ebene 
und der Senkrechten (in richtung Erdmittelpunkt) bestimmen. Aber da 
stehe ich mal eben auf dem Schlauch.
Ich finde auch keine Beispiele, evtl. suche ich aber auch noch nach dem 
Falschen Begriff.

Für einen ordentlichen Schubs in die richtige Richtung, evtl. mit 
Beispiel wäre ich sehr dankbar!