New version without kalman filter