global L1 L2 XD YD;
L1 = 0.5;
L2 = 0.5;
XD = 0.5;
YD = 0.5;
a0 = [ 0.0, 0.0 ];
a = fminunc('cost',a0)
% should be a = 0.0000 1.5708
a0 = [ 1.5 -1.5 ];
a = fminunc('cost',a0)
% should be a = 1.5708   -1.5708
XD = 1.0;
YD = 1.0;
a = fminunc('cost',a0)
% should be a = 0.7854 0.0000