fixing the algorithm, problem is that (dh,dq)=nan