Finished implementing iLQG. However, the current version diverges in the delta_x value - it needs to be debugged.