Abstract: A new algorithm is developed to incorporate the radial velocity measurement into Kalman filter for LFM waveform. An analysis is given about the statistical property of the range and radial velocity measurements. The covariance of position measurement noises and radial velocity measurement noises is given. An equivalent radial velocity measurement equation is derived and associated measurement noise is uncorrelated with position measurement noises. Monte Carlo simulation results show that the new algorithm can not only remove the bias caused by the range-Doppler coupling but also improve estimation accuracy and its performance is superior to conventional EKF.