High speed height control of a surface of a substrate within a wafer inspection system includes positioning a substrate on a substrate stage of a dynamically adjustable substrate stage assembly, actuating the substrate perpendicular to the surface of the substrate, measuring a height error value of the surface of the substrate at a position of inspection of the surface, measuring a displacement value perpendicular to the surface of the substrate at the location of the substrate stage assembly, generating a displacement target from the height error value and the displacement value, and adjusting an actuation state of the actuator using the measured height error value and the generated displacement target in order to maintain the substrate surface at an imaging plane of a detector of the inspection system or a focus of illumination of the inspection system.