To run:
1) start matlab
2) type in "main"

The model is made up, so you need to fix it based on your model.

The angle is in radians, not encoder units.