improved the math, now integrates properly
work in progress, try to fix gyro data
now interprets gyro data as sin(rho/2), also plots rot angle in degree