ILQR Steering implemented. Compiles but has issues with shared pointers. TODO: Debug the shared pointer issues