complementary filter for gyro/accellero, might need some tweaking for use in the final setup.
add simple Vec3 class, for x,y,z (only implement the needed operators)