Extract generally usable functions from Matlab code

[stiber]

Right now, the Matlab code is generally separated into functions and examples, but more work needs to be done to make the functions reusable.