A method and apparatus are provided for improving the focusing of a substrate such as a wafer during the photolithography imaging procedure of a semiconductor manufacturing process. The invention is particularly useful for step-and-scan system and the CD of two features in each exposure field are measured in fields exposed at varying focus to form at least two Bossung curves. Exposure focus instructions are calculated based on the intersection point of the curves and the wafer is then scanned and imaged based on the calculated exposure focus instructions. In another aspect of the invention, when multiple wafers are being processed operational variances may cause a drift in the focus. The focus drift can be easily corrected by measuring the critical dimension of each of the features and comparing the difference to determine if any focus offset is needed to return the focus to the original calculated focus value.