Finished and tested the prediction stage, including the addition of a possible speed in the z direction, in case it is needed.