Fixed Utility Function

Fixed bug with optimization function that occurred when the angle was 0.