Added Matlab functions for IK

Some basic functions to computer inverse kinematics in 2D. Should be
fairly self explanatory but I can explain if needed.

Need to replace fsolve with our own function though.