[New feature] Plotting function for position + velocity + orientation history

[gaffarelj]

To help visualize the orientation of a vehicle over time, a plotting function with the following capabilities can be added:

• Plot the position history of a body in 2D (let user select what pair to use from x, y, z).
• Plot the velocity of the body at each point as a vector going from the point to the direction of the velocity, with the size of the vector scaled to represent the magnitude of the velocity (add a scaling factor to let the user tune how big/small the magnitudes of the vectors should be).
• Optionally, also add the orientation of the body itself at some points in time. This would require the user to specify an image for the body (making sure to have x and y/z as required on the picture). Then, the orientation of the body can be extracted and plotted on a set of points as indicated by the user. Scaling of the body should also be made possible to allow the user to fine-tune the figure.