Added velocity estimation code