Improve default implementation of CostFunction::GetGradient()
For any cost function that does not have an explicit gradient, the GetGradient(v) method should use sampling to give a decent approximation. This default implementation can be found in CostFunction::GetGradient(v).
However, the current version is incorrect. A better solution would be to take a number of sample points around the query velocity v, compute their costs, and fit a 3D plane through these points using some least-squares optimizer.
@ajovane You said you had an idea/implementation for this, right? :)