Laser interferometry has been widely used in the field of nano-metrology due to its superior properties, such as its almost unlimited resolution and its flexible set up. In practical measurements with a heterodyne laser interferometer, however, some obstacles, such as environmental and nonlinearity errors, limit its measurement accuracy. In this paper, we compensate for nonlinearity errors by applying the recursive weighted least-squares method and the robust observer based Kalman filter algorithm, consecutively. To demonstrate the performance of the two proposed algorithms, we performed simulations and experiments. The experimental results show the improved accuracy obtained by using the proposed algorithms.