First draft of optimisation completed - gradient matrix appear to be incorrect from numerical testing