📄 simulation.m
字号:
%%%%%导航 坐标系到 载体坐标
Tnb1=[Tnb11(1) Tnb12(1) Tnb13(1)
Tnb21(1) Tnb22(1) Tnb23(1)
Tnb31(1) Tnb32(1) Tnb33(1)];
Tnb2=[Tnb11(2) Tnb12(2) Tnb13(2)
Tnb21(2) Tnb22(2) Tnb23(2)
Tnb31(2) Tnb32(2) Tnb33(2)];
Tnb3=[Tnb11(3) Tnb12(3) Tnb13(3)
Tnb21(3) Tnb22(3) Tnb23(3)
Tnb31(3) Tnb32(3) Tnb33(3)];
winb1=Tnb1*winn;
winb2=Tnb2*winn;
winb3=Tnb3*winn;
wibb1=winb1+wnbb1;
wibb2=winb2+wnbb2;
wibb3=winb3+wnbb3;
%%%%%%%%%%%%%%%%%%%%%%
fn(1,1) = a(1) - (2*wien(3)+wenn(3))*v0(2);
fn(2,1) = a(2) + (2*wien(3)+wenn(3))*v0(1);
fn(3,1) = a(3) + (2*wien(1)+wenn(1))*v0(2) - (2*wien(2)+wenn(2))*v0(1) + g1;
%%%%%%%%%%%%%%%%
fbb3=Tnb3*fn;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%__________---------
%%%%%%%%%----
wib1=Tms*wibb1+G_Drift+0.1*G_Drift*randn;
wib2=Tms*wibb2+G_Drift+0.1*G_Drift*randn;
wib3=Tms*wibb3+G_Drift+0.1*G_Drift*randn;
fb3=Tms*fbb3+A_Bias+0.1*A_Bias*randn;
%%%%%%%%%%%%%%
q0 = q;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
wpbb1=wib1-inv(T)*(wepp+wiep);
wpbb2=wib2-inv(T)*(wepp+wiep);
wpbb3=wib3-inv(T)*(wepp+wiep);
omiga1=[0 -wpbb1(1) -wpbb1(2) -wpbb1(3)
wpbb1(1) 0 wpbb1(3) -wpbb1(2)
wpbb1(2) -wpbb1(3) 0 wpbb1(1)
wpbb1(3) wpbb1(2) -wpbb1(1) 0 ];
omiga2=[0 -wpbb2(1) -wpbb2(2) -wpbb2(3)
wpbb2(1) 0 wpbb2(3) -wpbb2(2)
wpbb2(2) -wpbb2(3) 0 wpbb2(1)
wpbb2(3) wpbb2(2) -wpbb2(1) 0 ];
omiga3=[0 -wpbb3(1) -wpbb3(2) -wpbb3(3)
wpbb3(1) 0 wpbb3(3) -wpbb3(2)
wpbb3(2) -wpbb3(3) 0 wpbb3(1)
wpbb3(3) wpbb3(2) -wpbb3(1) 0 ];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
k1 = 1/2*omiga1*q;
q=q0+k1*Hn/2;
k2=1/2*omiga2*q;
q=q0+k2*Hn/2;
k3=1/2*omiga2*q;
q=q0+k3*Hn;
k4=1/2*omiga3*q;
q=q0+(k1+2*k2+2*k3+k4)/6*Hn;
q=q/sqrt(q(1)^2+q(2)^2+q(3)^2+q(4)^2);
T=[q(1)^2+q(2)^2-q(3)^2-q(4)^2 2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3))
2*(q(2)*q(3)+q(1)*q(4)) q(1)^2-q(2)^2+q(3)^2-q(4)^2 2*(q(3)*q(4)-q(1)*q(2))
2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) q(1)^2-q(2)^2-q(3)^2+q(4)^2];
fp=T*fb3;
f_vx = fp(1) + (2*wiep(3)+wepp(3))*v(2);
f_vy = fp(2) - (2*wiep(3)+wepp(3))*v(1);
v(1) = v(1) + f_vx*Hn;
v(2) = v(2) + f_vy*Hn;
wepp = [-v(2)/Ryp;
v(1)/Rxp;
v(1)*tan(phi)/Rxp];
phi = phi+v(2)*Hn/Ryp;
wiep = [0; wie*cos(phi); wie*sin(phi)];
g1 = g0+0.051799*sin(phi)*sin(phi);
temp_omega=inv(Tbn)*T*wpbb3;
temp_T=[0 -temp_omega(3) temp_omega(2)
temp_omega(3) 0 -temp_omega(1)
-temp_omega(2) temp_omega(1) 0];
Da=inv(T)*Tbn*(temp_T);
%Dv=Tbn*(inv(Tbn)*T*wpbb3);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A =[0 wiep(3)+wepp(3) T(1,3)*fb3(2)-T(1,2)*fb3(3) -T(1,3)*fb3(1)+T(1,1)*fb3(3) T(1,2)*fb3(1)-T(1,1)*fb3(3) -T(1,3)*fb3(2)+T(1,2)*fb3(3) T(1,3)*fb3(1)-T(1,1)*fb3(3) -T(1,2)*fb3(1)+T(1,1)*fb3(3) 0
-(wiep(3)+wepp(3)) 0 T(2,3)*fb3(2)-T(2,2)*fb3(3) -T(2,3)*fb3(1)+T(2,1)*fb3(3) T(2,2)*fb3(1)-T(2,1)*fb3(3) -T(2,3)*fb3(2)+T(2,2)*fb3(3) T(2,3)*fb3(1)-T(2,1)*fb3(3) -T(2,2)*fb3(1)+T(2,1)*fb3(3) 0
0 0 0 wpbb3(3) -wpbb3(2) 0 -wpbb3(3) wpbb3(2) 0
0 0 -wpbb3(3) 0 wpbb3(1) wpbb3(3) 0 -wpbb3(1) 0
0 0 wpbb3(2) -wpbb3(1) 0 -wpbb3(2) wpbb3(1) 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0];
B=eye(9,9);
B(1,1)=T(1,1);
B(1,2)=T(1,2);
B(2,1)=T(2,1);
B(2,2)=T(2,2);
H= [1 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 -Da(2,3)
0 0 0 1 0 0 0 0 Da(1,3)
0 0 0 0 1 0 0 0 -Da(1,2)];
% H= [1 0 0 0 0 0 0 0 0
% 0 1 0 0 0 0 0 0 0
% 0 0 1 0 0 0 0 0 0
% 0 0 0 1 0 0 0 0 0
% 0 0 0 0 1 0 0 0 0];
[F,G] = c2d(A,B,Hn);
FT = F';
GT = G';
HT=H';
delta_T=inv(T)*Tbn;
% Z=[v(1)-v0(1)
% v(2)-v0(2)
% delta_T(2,3)
% delta_T(3,1)
% delta_T(1,2)];
% %
Z=[v(1)-v0(1)+0.0001*randn
v(2)-v0(2)+0.0001*randn
delta_T(2,3)+0.0001*rad_deg*randn
delta_T(3,1)+0.0001*rad_deg*randn
delta_T(1,2)+0.0001*rad_deg*randn];
temp1(k)=delta_T(2,3)/rad_deg*60;
temp2(k)=delta_T(3,1)/rad_deg*60;
temp3(k)=delta_T(1,2)/rad_deg*60;
% r=3;
% %%%% r=0.4，0.5，0.6，0.7，0.8，0.9，1，2，3可以，
% L=eye(8);
% Rr=zeros(13);
% Rr(1,1)=1;
% Rr(2,2)=1;
% Rr(3,3)=1;
% Rr(4,4)=1;
% Rr(5,5)=1;
% Rr(6,6)= -r^2 ;Rr(7,7)= -r^2 ;Rr(8,8)= -r^2 ;Rr(9,9)= -r^2 ;
% Rr(10,10)= -r^2 ;Rr(11,11)= -r^2 ;Rr(12,12)= -r^2 ;Rr(13,13)= -r^2 ;
% HL=[H;
% L];
% HLT=[H' L'];
% Rek=Rr+HL*P*HLT;
% P=F*P*FT+G*GT-F*P*HLT*inv(Rek)*HL*P*FT;
% I=eye(5);
% KK=P*HT*inv(I+H*P*HT);
% X=F*X+KK*(Z-H*F*X);
Xk = F*X;
Zk = H*Xk;
Pk = F*P*FT + G*Q*GT;
K = Pk*HT*inv(H*Pk*HT+R);
P = (eye(9)-K*H)*Pk;
X = Xk + K*(Z-Zk);
datx1(k) = X(1);
datx2(k) = X(2);
datx3(k) = X(3)/rad_deg*60;
datx4(k) = X(4)/rad_deg*60;
datx5(k) = X(5)/rad_deg*60;
datx6(k) = X(6)/rad_deg*60;
datx7(k) = X(7)/rad_deg*60;
datx8(k) = X(8)/rad_deg*60;
datx9(k)=X(9);
dvx(k)=Z(1);
dvy(k)=Z(2);
end
j=1:length(datx1);
figure(1)
subplot(2,1,1)
plot(j/10,(datx1(j)))
subplot(2,1,2)
plot(j/10,(datx2(j)))
Xlabel('time/s');
figure(2)
subplot(3,1,1)
plot(j/10,(datx3(j)))
subplot(3,1,2)
plot(j/10,(datx4(j)))
subplot(3,1,3)
plot(j/10,(datx5(j)))
Xlabel('time/s');
figure(3)
subplot(3,1,1)
plot(j/10,temp1(j))
subplot(3,1,2)
plot(j/10,temp2(j))
subplot(3,1,3)
plot(j/10,temp3(j))
Xlabel('time/s');
figure(4)
subplot(3,1,1)
plot(j/10,datx6(j)-dp)
subplot(3,1,2)
plot(j/10,datx7(j)-dr)
subplot(3,1,3)
plot(j/10,datx8(j)-dy)
Xlabel('time/s');
Ylabel('角分');
j=1:200;
figure(5)
plot(j/10,datx9(j));
hold on;
plot(j/10,delay_time(j),'r');
% plot(delay_time-datx9);
hold off;
Xlabel('time/s');
figure(6)
plot(delay_time-datx9);⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -