Updated matlab code with tracing and velocity plotting