Fixed a (major) bug involving the use of the initial condition in the calculation of Psi. This means that, unless we are very lucky, the QP in optimality_fctn is non-constrained qualified. The rest are minor (mostly aesthetical) changes.