Added mostly complete speed calculations code

It still is incomplete due to lack of optical flow, and a couple small
fixes once I have that.