Last Cost Function is wrong. Calculation of IntAbsUdq and IntUdq are corrected.