adding PI-Control for pitch and roll angle