Added velocity estimation