updated for vision
fixed numerical integration, still needs gyro
fixed contours issue