modify estimateKF and test method