ai(1)/2)*cos(sit(1)/2)*cos(gam(1)/2)-sin(fai(1)/2)*sin(sit(1)/2)*sin(gam(1)/2);Рcos(fai(1)/2)*sin(sit(1)/2)*cos(gam(1)/2)-sin(fai(1)/2)*cos(sit(1)/2)*sin(gam(1)/2);Рcos(fai(1)/2)*cos(sit(1)/2)*sin(gam(1)/2)+sin(fai(1)/2)*sin(sit(1)/2)*cos(gam(1)/2);Рsin(fai(1)/2)*cos(sit(1)/2)*cos(gam(1)/2)+cos(fai(1)/2)*sin(sit(1)/2)*sin(gam(1)/2);]; %求取四元数的初值Рq0=q(1,1);q1=q(2,1);q2=q(3,1);q3=q(4,1);РCtb=[q0^2+q1^2-q2^2-q3^2 2*(q1*q2+q0*q3) 2*(q1*q3-q0*q2); %求取状态转移矩阵Р2*(q1*q2-q0*q3) q0^2-q1^2+q2^2-q3^2 2*(q2*q3+q0*q1);Р2*(q1*q3+q0*q2) 2*(q2*q3-q0*q1) q0^2-q1^2-q2^2+q3^2];Рfor i=1:48000 РRx(i)=re/(1-e*(sin(L(i)))^2); %求取两个半径РRy(i)=re/(1+2*e-3*e*(sin(L(i)))^2);РWitt=[-Vy(i)/Ry(i);Wie*cos(L(i))+Vx(i)/Rx(i);Wie*sin(L(i))+Vx(i)*tan(L(i))/Rx(i)]; %指令角速度计算РWitb=Ctb*Witt; %将指令角速度转换到平台坐标系РWibb=[Wx(i);Wy(i);Wz(i)];
全文预览
