Final version attached to bachelor thesis.
Oops, bug in manipulator corrected (PI).
Merge pull request #5 from xproko/master

Final version attached to bachelor thesis.