got crude yaw,pitch,roll working
got yaw, pitch, roll working slightly better
got yaw,pitch,roll rotation slightly better after thresholding the rotations allowed