In this paper a nonlinearity compensation algorithm based on the extended Kalman filter is proposed to improve the measurement accuracy of a heterodyne laser interferometer. The heterodyne laser interferometer is used for ultra-precision measurements such as those used in semiconductor manufacturing. However the periodical nonlinearity property caused by frequency-mixing restricts the accuracy of the nanometric measurements. In order to minimize the effect of the nonlinearity, the measurement process of the laser interferometer is modeled as a state equation and the extended Kalman filtering approach is applied to the process. The effectiveness of our proposed algorithm is demonstrated by comparing the results of the algorithm with experimental results for the laser system.