hello. i can help you with this project since i am mechanical engineer. there is no need for matlab to do that. :D it can be calculated manualy. i can write you all procedure in doc file.
so if you are interested, please contact me.
thank you.
as i see, this:
x=r1*sin(theta)-d0*sin(phi0);
y=r1*cos(theta)-d0*cos(phi0);
angle=atan(abs(y/x));
this is oyler equatins for some vector. i mean this is projections on x and y axis. and ATAN is angle between projection and vector.
so, i think i know what you are doing but until i dont get all data i cant help.
so if you are interested, please contact me.
as consern matrix i have it in my books. that is assighnment from power transmision probably. maybe some piston mehanism or something similar with that...