fun3.m

希望每一个使用本程序的人都能做到保密

function f=fun3(time,yyy)
Vx=8.0+0.5*randn(1);Vy=8.0+0.5*randn(1);Vz=0.0;
ax=0.0;ay=0.0;az=0.0;
angle=(45+0.1*randn(1))*pi/180;
R=6371020.0+50*randn(1);
Wie=7.2921158e-5;
g=9.80;
fx=(2*Wie*cos(angle)+Vx/R)*Vz-(2*Wie*sin(angle)+Vx*tan(angle)/R)*Vy;
fy=(2*Wie*sin(angle)+Vx*tan(angle)/R)*Vx+Vy*Vz/R;    
fz=-(2*Wie*cos(angle)+Vx/R)*Vx-Vy^2/R+g;
A=zeros(17);
A(1,2)= Vx*tan(angle)/R+Wie*sin(angle);
A(1,3)=-Vx/R-Wie*cos(angle);
A(1,8)=-1/R;
A(1,10)=-1;
A(2,1)=-Vx*tan(angle)/R-Wie*sin(angle);
A(2,3)=-Vy/R;
A(2,7)=1/R;
A(2,9)=-Wie*sin(angle);
A(2,11)=-1;
A(3,1)=Vx/R+Wie*cos(angle);
A(3,2)=Vy/R;
A(3,7)=tan(angle)/R;
A(3,9)=Wie*cos(angle)+Vx*sec(angle)^2/R;
A(3,12)=-1;
A(4,10)=1;
A(4,13)=1;
A(5,11)=1;
A(5,14)=1;
A(6,12)=1;
A(6,15)=1;
A(7,2)=-fz;
A(7,3)=fy; 
A(7,7)=(Vy*tan(angle)-Vz)/R;
A(7,8)=2*Wie*sin(angle)+Vx*tan(angle)/R; 
A(7,9)=2*Wie*sin(angle)*Vz+(2*Wie*cos(angle)+Vx*sec(angle)^2/R)*Vy;
A(7,16)=1;
A(8,1)=fz;
A(8,3)=-fx;
A(8,7)=-2*Wie*sin(angle)-2*Vx*tan(angle)/R;
A(8,8)=-Vz/R;
A(8,9)=-2*Wie*cos(angle)*Vx-Vx^2*(sec(angle))^2/R;
A(8,17)=1;
A(9,8)=1/R;
f=A*yyy;