|
楼主 |
发表于 2009-5-17 23:14
|
显示全部楼层
回复 8楼 ChaChing 的帖子
a(4*600),A1(4*600),F(4*4)是在已求出的矩阵
T=600;
U=eye(1,T);
Xo=zeros(1,T);
Yo=zeros(1,T);
Vo=3.0;
global a;
global A1;
global F;
for k=1:T-1
Xo(k+1)=Xo(k)+Vo*sin(U(k));
Yo(k+1)=Yo(k)+Vo*cos(U(k));
end
Xt=zeros(1,T);
Yt=1e+4*eye(1,T);
Vt=5.0;
for k=1:T-1
Xt(k+1)=Xt(k)+Vt*cos(pi/3);
Yt(k+1)=Yt(k)-Vt*sin(pi/3);
end
rx=zeros(1,T);
ry=zeros(1,T);
r=zeros(1,T);
for k=1:T
rx(k)=Xt(k)-Xo(k);
ry(k)=Yt(k)-Yo(k);
r(k)=sqrt(rx(k)^2+ry(k)^2);
end
B=atan2(rx,ry);
a=zeros(4,T);
for k=1:T
a(:,k)=1/r(k)*[cos(B(k)) -sin(B(k)) 0 0]';
end
c=0.5/180*pi;
G=[1 0 1 0;0 1 0 1;0 0 1 0;0 0 0 1];
F=zeros(4);
for k=1:T
F=G.'*F*G+1/c/c*a(:,k)*a(:,k).';
end
for w=1:T
Xtt=Xt(w);
Xoo=Xo(w);
Ytt=Yt(w);
Yoo=Yo(w);
aa1 =-1/2/(Xtt^2-2*Xtt*Xoo+Xoo^2+Ytt^2-2*Ytt*Yoo+Yoo^2)^(3/2)/(1+(Xtt-Xoo)^2/(Ytt-Yoo)^2)^(1/2)*(-2*Xtt+2*Xoo)+1/(Xtt^2-2*Xtt*Xoo+Xoo^2+Ytt^2-2*Ytt*Yoo+Yoo^2)^(1/2)/(1+(Xtt-Xoo)^2/(Ytt-Yoo)^2)^(3/2)*(Xtt-Xoo)/(Ytt-Yoo)^2;
aa2 =1/2/(Xtt^2-2*Xtt*Xoo+Xoo^2+Ytt^2-2*Ytt*Yoo+Yoo^2)^(3/2)*(Xtt-Xoo)/(Ytt-Yoo)/(1+(Xtt-Xoo)^2/(Ytt-Yoo)^2)^(1/2)*(-2*Xtt+2*Xoo)+1/(Xtt^2-2*Xtt*Xoo+Xoo^2+Ytt^2-2*Ytt*Yoo+Yoo^2)^(1/2)/(Ytt-Yoo)/(1+(Xtt-Xoo)^2/(Ytt-Yoo)^2)^(1/2)-1/(Xtt^2-2*Xtt*Xoo+Xoo^2+Ytt^2-2*Ytt*Yoo+Yoo^2)^(1/2)*(Xtt-Xoo)^2/(Ytt-Yoo)^3/(1+(Xtt-Xoo)^2/(Ytt-Yoo)^2)^(3/2);
aa3 =-1/2/(Xtt^2-2*Xtt*Xoo+Xoo^2+Ytt^2-2*Ytt*Yoo+Yoo^2)^(3/2)/(1+(Xtt-Xoo)^2/(Ytt-Yoo)^2)^(1/2)*(-2*Ytt+2*Yoo)-1/(Xtt^2-2*Xtt*Xoo+Xoo^2+Ytt^2-2*Ytt*Yoo+Yoo^2)^(1/2)*(Xtt-Xoo)^2/(Ytt-Yoo)^3/(1+(Xtt-Xoo)^2/(Ytt-Yoo)^2)^(3/2);
aa4 =1/2/(Xtt^2-2*Xtt*Xoo+Xoo^2+Ytt^2-2*Ytt*Yoo+Yoo^2)^(3/2)*(Xtt-Xoo)/(Ytt-Yoo)/(1+(Xtt-Xoo)^2/(Ytt-Yoo)^2)^(1/2)*(-2*Ytt+2*Yoo)-1/(Xtt^2-2*Xtt*Xoo+Xoo^2+Ytt^2-2*Ytt*Yoo+Yoo^2)^(1/2)*(Xtt-Xoo)/(Ytt-Yoo)^2/(1+(Xtt-Xoo)^2/(Ytt-Yoo)^2)^(1/2)+1/(Xtt^2-2*Xtt*Xoo+Xoo^2+Ytt^2-2*Ytt*Yoo+Yoo^2)^(1/2)*(Xtt-Xoo)^3/(Ytt-Yoo)^4/(1+(Xtt-Xoo)^2/(Ytt-Yoo)^2)^(3/2);
A1(:,w)=[aa1,aa2,0,0]';
A2(:,w)=[aa3,aa4,0,0]';
end |
|