mufun_4sps_2ccs.m

基于四元数的机构综合建立模型的MATLAB程序

function f=mufun_4sps_2ccs(x)
x=[-0.977154	0.090038	0.169880	-0.090580	0.426559	-8.664720	2.665800	-8.214840	7.324790	3.027880];
q0=x(1);q1=x(2);q2=x(3);q3=x(4);g0=x(5);g1=x(6);g2=x(7);g3=x(8);x1=x(9);x2=x(10);
f1 =q0^2+q1^2+q2^2+q3^2-1;
f2 =g0*q0+g1*q1+g2*q2+g3*q3;
f3 =(g0*q0+g1*q1+g2*q2+g3*q3)^2-(-g0*q1+g1*q0-g2*q3+g3*q2-8700260925737183/9007199254740992*x1)*(g0*q1-g1*q0+g2*q3-g3*q2+8700260925737183/9007199254740992*x1)-(-g0*q2+g2*q0-g3*q1+g1*q3+25883/100000*x1)*(g0*q2-g2*q0+g3*q1-g1*q3-25883/100000*x1)-(-g0*q3+g3*q0-g1*q2+g2*q1)*(g0*q3-g3*q0+g1*q2-g2*q1)-6769252513509535/70368744177664;
f4 =-8700260925737183/9007199254740992*g0*q1+8700260925737183/9007199254740992*g1*q0-8700260925737183/9007199254740992*g2*q3+8700260925737183/9007199254740992*g3*q2-792281792178586086494825984942062691129/792281625142643375935439503360000000000*x1+25883/100000*g0*q2-25883/100000*g2*q0+25883/100000*g3*q1-25883/100000*g1*q3;
f5 =(g0*q0+g1*q1+g2*q2+g3*q3+q0*(-2*q1+36049/10000*q2+69/1000*q3)-q1*(-2*q0-36049/10000*q3+69/1000*q2)-q2*(36049/10000*q0-69/1000*q1-2*q3)-q3*(69/1000*q0+2*q2+36049/10000*q1))^2-(-g0*q1+g1*q0-g2*q3+g3*q2+q0*(-2*q0-36049/10000*q3+69/1000*q2)+q1*(-2*q1+36049/10000*q2+69/1000*q3)+q2*(69/1000*q0+2*q2+36049/10000*q1)-q3*(36049/10000*q0-69/1000*q1-2*q3)+18301/10000-3/5*x2)*(g0*q1-g1*q0+g2*q3-g3*q2-q0*(-2*q0-36049/10000*q3+69/1000*q2)-q1*(-2*q1+36049/10000*q2+69/1000*q3)-q2*(69/1000*q0+2*q2+36049/10000*q1)+q3*(36049/10000*q0-69/1000*q1-2*q3)-18301/10000+3/5*x2)-(-g0*q2+g2*q0-g3*q1+g1*q3+q0*(36049/10000*q0-69/1000*q1-2*q3)+q2*(-2*q1+36049/10000*q2+69/1000*q3)+q3*(-2*q0-36049/10000*q3+69/1000*q2)-q1*(69/1000*q0+2*q2+36049/10000*q1)-31699/10000-4/5*x2)*(g0*q2-g2*q0+g3*q1-g1*q3-q0*(36049/10000*q0-69/1000*q1-2*q3)-q2*(-2*q1+36049/10000*q2+69/1000*q3)-q3*(-2*q0-36049/10000*q3+69/1000*q2)+q1*(69/1000*q0+2*q2+36049/10000*q1)+31699/10000+4/5*x2)-(-g0*q3+g3*q0-g1*q2+g2*q1+q0*(69/1000*q0+2*q2+36049/10000*q1)+q3*(-2*q1+36049/10000*q2+69/1000*q3)+q1*(36049/10000*q0-69/1000*q1-2*q3)-q2*(-2*q0-36049/10000*q3+69/1000*q2)+1/2)*(g0*q3-g3*q0+g1*q2-g2*q1-q0*(69/1000*q0+2*q2+36049/10000*q1)-q3*(-2*q1+36049/10000*q2+69/1000*q3)-q1*(36049/10000*q0-69/1000*q1-2*q3)+q2*(-2*q0-36049/10000*q3+69/1000*q2)-1/2)-4139320421175937/35184372088832;
f6=-71893/50000-3/5*g0*q1+3/5*g1*q0-3/5*g2*q3+3/5*g3*q2+3/5*q0*(-2*q0-36049/10000*q3+69/1000*q2)+3/5*q1*(-2*q1+36049/10000*q2+69/1000*q3)+3/5*q2*(69/1000*q0+2*q2+36049/10000*q1)-3/5*q3*(36049/10000*q0-69/1000*q1-2*q3)-x2-4/5*g0*q2+4/5*g2*q0-4/5*g3*q1+4/5*g1*q3+4/5*q0*(36049/10000*q0-69/1000*q1-2*q3)+4/5*q2*(-2*q1+36049/10000*q2+69/1000*q3)+4/5*q3*(-2*q0-36049/10000*q3+69/1000*q2)-4/5*q1*(69/1000*q0+2*q2+36049/10000*q1);
f7=(g0*q0+g1*q1+g2*q2+g3*q3+q0*(-8349/10000*q1+54247/10000*q2-9359/10000*q3)-q1*(-8349/10000*q0-54247/10000*q3-9359/10000*q2)-q2*(54247/10000*q0+9359/10000*q1-8349/10000*q3)-q3*(-9359/10000*q0+8349/10000*q2+54247/10000*q1))^2-(-g0*q1+g1*q0-g2*q3+g3*q2+q0*(-8349/10000*q0-54247/10000*q3-9359/10000*q2)+q1*(-8349/10000*q1+54247/10000*q2-9359/10000*q3)+q2*(-9359/10000*q0+8349/10000*q2+54247/10000*q1)-q3*(54247/10000*q0+9359/10000*q1-8349/10000*q3)-31699/10000)*(g0*q1-g1*q0+g2*q3-g3*q2-q0*(-8349/10000*q0-54247/10000*q3-9359/10000*q2)-q1*(-8349/10000*q1+54247/10000*q2-9359/10000*q3)-q2*(-9359/10000*q0+8349/10000*q2+54247/10000*q1)+q3*(54247/10000*q0+9359/10000*q1-8349/10000*q3)+31699/10000)-(-g0*q2+g2*q0-g3*q1+g1*q3+q0*(54247/10000*q0+9359/10000*q1-8349/10000*q3)+q2*(-8349/10000*q1+54247/10000*q2-9359/10000*q3)+q3*(-8349/10000*q0-54247/10000*q3-9359/10000*q2)-q1*(-9359/10000*q0+8349/10000*q2+54247/10000*q1)-6659754243969463/562949953421312)*(g0*q2-g2*q0+g3*q1-g1*q3-q0*(54247/10000*q0+9359/10000*q1-8349/10000*q3)-q2*(-8349/10000*q1+54247/10000*q2-9359/10000*q3)-q3*(-8349/10000*q0-54247/10000*q3-9359/10000*q2)+q1*(-9359/10000*q0+8349/10000*q2+54247/10000*q1)+6659754243969463/562949953421312)-(-g0*q3+g3*q0-g1*q2+g2*q1+q0*(-9359/10000*q0+8349/10000*q2+54247/10000*q1)+q3*(-8349/10000*q1+54247/10000*q2-9359/10000*q3)+q1*(54247/10000*q0+9359/10000*q1-8349/10000*q3)-q2*(-8349/10000*q0-54247/10000*q3-9359/10000*q2)-1/2)*(g0*q3-g3*q0+g1*q2-g2*q1-q0*(-9359/10000*q0+8349/10000*q2+54247/10000*q1)-q3*(-8349/10000*q1+54247/10000*q2-9359/10000*q3)-q1*(54247/10000*q0+9359/10000*q1-8349/10000*q3)+q2*(-8349/10000*q0-54247/10000*q3-9359/10000*q2)+1/2)-8604334031308961/70368744177664;
f8=(g0*q0+g1*q1+g2*q2+g3*q3+q0*(31651/10000*q1+11367/2000*q2+3/100*q3)-q1*(31651/10000*q0-11367/2000*q3+3/100*q2)-q2*(11367/2000*q0-3/100*q1+31651/10000*q3)-q3*(3/100*q0-31651/10000*q2+11367/2000*q1))^2-(-g0*q1+g1*q0-g2*q3+g3*q2+q0*(31651/10000*q0-11367/2000*q3+3/100*q2)+q1*(31651/10000*q1+11367/2000*q2+3/100*q3)+q2*(3/100*q0-31651/10000*q2+11367/2000*q1)-q3*(11367/2000*q0-3/100*q1+31651/10000*q3)-68301/10000)*(g0*q1-g1*q0+g2*q3-g3*q2-q0*(31651/10000*q0-11367/2000*q3+3/100*q2)-q1*(31651/10000*q1+11367/2000*q2+3/100*q3)-q2*(3/100*q0-31651/10000*q2+11367/2000*q1)+q3*(11367/2000*q0-3/100*q1+31651/10000*q3)+68301/10000)-(-g0*q2+g2*q0-g3*q1+g1*q3+q0*(11367/2000*q0-3/100*q1+31651/10000*q3)+q2*(31651/10000*q1+11367/2000*q2+3/100*q3)+q3*(31651/10000*q0-11367/2000*q3+3/100*q2)-q1*(3/100*q0-31651/10000*q2+11367/2000*q1)-6659754243969463/562949953421312)*(g0*q2-g2*q0+g3*q1-g1*q3-q0*(11367/2000*q0-3/100*q1+31651/10000*q3)-q2*(31651/10000*q1+11367/2000*q2+3/100*q3)-q3*(31651/10000*q0-11367/2000*q3+3/100*q2)+q1*(3/100*q0-31651/10000*q2+11367/2000*q1)+6659754243969463/562949953421312)-(-g0*q3+g3*q0-g1*q2+g2*q1+q0*(3/100*q0-31651/10000*q2+11367/2000*q1)+q3*(31651/10000*q1+11367/2000*q2+3/100*q3)+q1*(11367/2000*q0-3/100*q1+31651/10000*q3)-q2*(31651/10000*q0-11367/2000*q3+3/100*q2)+1)*(g0*q3-g3*q0+g1*q2-g2*q1-q0*(3/100*q0-31651/10000*q2+11367/2000*q1)-q3*(31651/10000*q1+11367/2000*q2+3/100*q3)-q1*(11367/2000*q0-3/100*q1+31651/10000*q3)+q2*(31651/10000*q0-11367/2000*q3+3/100*q2)-1)-1459811760851227/8796093022208;
f9=(g0*q0+g1*q1+g2*q2+g3*q3+q0*(21651/5000*q1+16731/5000*q2-4483/5000*q3)-q1*(21651/5000*q0-16731/5000*q3-4483/5000*q2)-q2*(16731/5000*q0+4483/5000*q1+21651/5000*q3)-q3*(-4483/5000*q0-21651/5000*q2+16731/5000*q1))^2-(-g0*q1+g1*q0-g2*q3+g3*q2+q0*(21651/5000*q0-16731/5000*q3-4483/5000*q2)+q1*(21651/5000*q1+16731/5000*q2-4483/5000*q3)+q2*(-4483/5000*q0-21651/5000*q2+16731/5000*q1)-q3*(16731/5000*q0+4483/5000*q1+21651/5000*q3)-6659754243969463/562949953421312)*(g0*q1-g1*q0+g2*q3-g3*q2-q0*(21651/5000*q0-16731/5000*q3-4483/5000*q2)-q1*(21651/5000*q1+16731/5000*q2-4483/5000*q3)-q2*(-4483/5000*q0-21651/5000*q2+16731/5000*q1)+q3*(16731/5000*q0+4483/5000*q1+21651/5000*q3)+6659754243969463/562949953421312)-(-g0*q2+g2*q0-g3*q1+g1*q3+q0*(16731/5000*q0+4483/5000*q1+21651/5000*q3)+q2*(21651/5000*q1+16731/5000*q2-4483/5000*q3)+q3*(21651/5000*q0-16731/5000*q3-4483/5000*q2)-q1*(-4483/5000*q0-21651/5000*q2+16731/5000*q1)-31699/10000)*(g0*q2-g2*q0+g3*q1-g1*q3-q0*(16731/5000*q0+4483/5000*q1+21651/5000*q3)-q2*(21651/5000*q1+16731/5000*q2-4483/5000*q3)-q3*(21651/5000*q0-16731/5000*q3-4483/5000*q2)+q1*(-4483/5000*q0-21651/5000*q2+16731/5000*q1)+31699/10000)-(-g0*q3+g3*q0-g1*q2+g2*q1+q0*(-4483/5000*q0-21651/5000*q2+16731/5000*q1)+q3*(21651/5000*q1+16731/5000*q2-4483/5000*q3)+q1*(16731/5000*q0+4483/5000*q1+21651/5000*q3)-q2*(21651/5000*q0-16731/5000*q3-4483/5000*q2))*(g0*q3-g3*q0+g1*q2-g2*q1-q0*(-4483/5000*q0-21651/5000*q2+16731/5000*q1)-q3*(21651/5000*q1+16731/5000*q2-4483/5000*q3)-q1*(16731/5000*q0+4483/5000*q1+21651/5000*q3)+q2*(21651/5000*q0-16731/5000*q3-4483/5000*q2))-3279920802732479/35184372088832;
f10=(g0*q0+g1*q1+g2*q2+g3*q3+q0*(11651/5000*q1+2589/10000*q2+9659/10000*q3)-q1*(11651/5000*q0-2589/10000*q3+9659/10000*q2)-q2*(2589/10000*q0-9659/10000*q1+11651/5000*q3)-q3*(9659/10000*q0-11651/5000*q2+2589/10000*q1))^2-(-g0*q1+g1*q0-g2*q3+g3*q2+q0*(11651/5000*q0-2589/10000*q3+9659/10000*q2)+q1*(11651/5000*q1+2589/10000*q2+9659/10000*q3)+q2*(9659/10000*q0-11651/5000*q2+2589/10000*q1)-q3*(2589/10000*q0-9659/10000*q1+11651/5000*q3)-10)*(g0*q1-g1*q0+g2*q3-g3*q2-q0*(11651/5000*q0-2589/10000*q3+9659/10000*q2)-q1*(11651/5000*q1+2589/10000*q2+9659/10000*q3)-q2*(9659/10000*q0-11651/5000*q2+2589/10000*q1)+q3*(2589/10000*q0-9659/10000*q1+11651/5000*q3)+10)-(-g0*q2+g2*q0-g3*q1+g1*q3+q0*(2589/10000*q0-9659/10000*q1+11651/5000*q3)+q2*(11651/5000*q1+2589/10000*q2+9659/10000*q3)+q3*(11651/5000*q0-2589/10000*q3+9659/10000*q2)-q1*(9659/10000*q0-11651/5000*q2+2589/10000*q1))*(g0*q2-g2*q0+g3*q1-g1*q3-q0*(2589/10000*q0-9659/10000*q1+11651/5000*q3)-q2*(11651/5000*q1+2589/10000*q2+9659/10000*q3)-q3*(11651/5000*q0-2589/10000*q3+9659/10000*q2)+q1*(9659/10000*q0-11651/5000*q2+2589/10000*q1))-(-g0*q3+g3*q0-g1*q2+g2*q1+q0*(9659/10000*q0-11651/5000*q2+2589/10000*q1)+q3*(11651/5000*q1+2589/10000*q2+9659/10000*q3)+q1*(2589/10000*q0-9659/10000*q1+11651/5000*q3)-q2*(11651/5000*q0-2589/10000*q3+9659/10000*q2)+1)*(g0*q3-g3*q0+g1*q2-g2*q1-q0*(9659/10000*q0-11651/5000*q2+2589/10000*q1)-q3*(11651/5000*q1+2589/10000*q2+9659/10000*q3)-q1*(2589/10000*q0-9659/10000*q1+11651/5000*q3)+q2*(11651/5000*q0-2589/10000*q3+9659/10000*q2)-1)-1351047901746639/8796093022208;
f=[f1;f2;f3;f4;f5;f6;f7;f8;f9;f10];
%disp(num2str(f'))
%%%%%%%%%%%%%%2008.01.08%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%