Robust optimisation

First optimise with full cost function before starting iterative cost function

Also, fixed the cost function for DWI