Solved a problem with the prediction function. Also the ego-motion has been included into the generation of the final speed of the obstacles.