Fixed RV2COE for getting classical orbital elements from position and velocity. Starting to work on COE equations of motion.