Added the final version with Kalman FIlter