wu_fangzhen_observability_12jie.m

这些程序都是经过本人和许许多多工作过学习过的老师同学

%***************************“速度＋航姿或航向”传递对准12阶状态矩阵的提取**********************

clc
clear all;

format long;

%重力加速度

g=9.80; 

%地速分量

Vx=8.0;
Vy=8.0; 
Vz=0.0; 

%地球半径

R=6371020; 

%纬度角

angle=pi/4;

%地球自转角速度

Wie=7.2921158e-5;  

%所测的比力信息

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;

T=zeros(3,3,30);

for  t=0:1:29
      
%舰船摇摆误差角

pitchx=10*sin(pi*t/4+0.1)/57.3;  %俯仰角

rolly=5*sin(pi*t/5+0.01)/57.3;   %倾斜角即滚动角

yawz=6*sin(pi*t/6+0.02)/57.3;    %偏航角

%求微分方程

dpitchxdt=pi*2.5*cos(pi*t/4+0.1)/57.3; 

drollydt=pi*cos(pi*t/5+0.01)/57.3;   

dyawzdt=pi*cos(pi*t/6+0.02)/57.3;   

%捷联矩阵/由b1坐标系转到n坐标系

T(1,1,t/1+1)=cos(rolly)*cos(yawz)-sin(rolly)*sin(yawz)*sin(pitchx);
T(1,2,t/1+1)=-cos(pitchx)*sin(yawz);
T(1,3,t/1+1)=sin(rolly)*cos(yawz)+cos(rolly)*sin(pitchx)*sin(yawz);
T(2,1,t/1+1)=cos(rolly)*sin(yawz)+sin(rolly)*sin(pitchx)*cos(yawz);
T(2,2,t/1+1)=cos(pitchx)*cos(yawz);
T(2,3,t/1+1)=sin(rolly)*sin(yawz)-cos(rolly)*cos(yawz)*sin(pitchx);
T(3,1,t/1+1)=-sin(rolly)*cos(pitchx);
T(3,2,t/1+1)=sin(pitchx);
T(3,3,t/1+1)=cos(rolly)*cos(pitchx);

end

%求出A阵

A=zeros(12,12,30);

for t=0:1:29

A(1,2,t/1+1)= Vx*tan(angle)/R+Wie*sin(angle);
A(1,3,t/1+1)=-Vx/R-Wie*cos(angle);
A(1,5,t/1+1)=-1/R;
A(1,7,t/1+1)=T(1,1,t/1+1);
A(1,8,t/1+1)=T(1,2,t/1+1);
A(1,9,t/1+1)=T(1,3,t/1+1);

A(2,1,t/1+1)=-Vx*tan(angle)/R-Wie*sin(angle);
A(2,3,t/1+1)=-Vy/R;
A(2,4,t/1+1)=1/R;
A(2,6,t/1+1)=-Wie*sin(angle);
A(2,7,t/1+1)=T(2,1,t/1+1);
A(2,8,t/1+1)=T(2,2,t/1+1);
A(2,9,t/1+1)=T(2,3,t/1+1);

A(3,1,t/1+1)=Vx/R+Wie*cos(angle);
A(3,2,t/1+1)=Vy/R;
A(3,4,t/1+1)=tan(angle)/R;
A(3,6,t/1+1)=Wie*cos(angle)+Vx*sec(angle)^2/R;
A(3,7,t/1+1)=T(3,1,t/1+1);
A(3,8,t/1+1)=T(3,2,t/1+1);
A(3,9,t/1+1)=T(3,3,t/1+1);

A(4,2,t/1+1)=-fz;
A(4,3,t/1+1)=fy; 
A(4,4,t/1+1)=(Vy*tan(angle)-Vz)/R;
A(4,5,t/1+1)=2*Wie*sin(angle)+Vx*tan(angle)/R; 
A(4,6,t/1+1)=2*Wie*sin(angle)*Vz+(2*Wie*cos(angle)+Vx*sec(angle)^2/R)*Vy;
A(4,10,t/1+1)=T(1,1,t/1+1);
A(4,11,t/1+1)=T(1,2,t/1+1);
A(4,12,t/1+1)=T(1,3,t/1+1);

A(5,1,t/1+1)=fz;
A(5,3,t/1+1)=-fx;
A(5,4,t/1+1)=-2*Wie*sin(angle)-2*Vx*tan(angle)/R;
A(5,5,t/1+1)=-Vz/R;
A(5,6,t/1+1)=-2*Wie*cos(angle)*Vx-Vx^2*sec(angle)^2/R;
A(5,10,t/1+1)=T(2,1,t/1+1);
A(5,11,t/1+1)=T(2,2,t/1+1);
A(5,12,t/1+1)=T(2,3,t/1+1);

A(6,5,t/1+1)=1/R;

end

A1=zeros(12,30);A2=zeros(18,30);A3=zeros(12,30);A4=zeros(12,30);A5=zeros(12,30);
A6=zeros(12,30);A7=zeros(12,30);A8=zeros(12,30);A9=zeros(12,30);A10=zeros(12,30);
A11=zeros(12,30);A12=zeros(12,30);
A1=A(1,:,:);A2=A(2,:,:);A3=A(3,:,:);A4=A(4,:,:);A5=A(5,:,:);
A6=A(6,:,:);A7=A(7,:,:);A8=A(8,:,:);A9=A(9,:,:);A10=A(10,:,:);
A11=A(11,:,:);A12=A(12,:,:);

fid1= fopen('E:\孙国伟\work\data61.txt','w');
    fprintf(fid1,'%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\r\n',A1(:,:));%,C(2,:),C(3,:),C(1,4,:),C(1,5,:),C(1,6,:),C(1,7,:),C(1,,:),C(1,9,:),C(1,10,:),C(1,11,:),C(1,12,:),C(1,13,:),C(1,14,:));
fclose(fid1);

fid2= fopen('E:\孙国伟\work\data62.txt','w');
    fprintf(fid2,'%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\r\n',A2(:,:));%,C(2,2,:),C(2,3,:),C(2,4,:),C(2,5,:),C(2,6,:),C(2,7,:),C(2,,:),C(2,9,:),C(2,10,:),C(2,11,:),C(2,12,:),C(2,13,:),C(2,14,:));
fclose(fid2);

fid3= fopen('E:\孙国伟\work\data63.txt','w');
    fprintf(fid3,'%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\r\n',A3(:,:));%,C3(2,:),C3(3,:),C3(4,:),C3(5,:),C3(6,:),C3(7,:),C3(8,:),C3(9,:),C3(10,:),C3(11,:),C3(12,:),C3(13,:),C3(14,:));
fclose(fid3);

fid4= fopen('E:\孙国伟\work\data64.txt','w');
    fprintf(fid4,'%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\r\n',A4(:,:));%,C(4,2,:),C(4,3,:),C(4,4,:),C(4,5,:),C(4,6,:),C(4,7,:),C(4,8,:),C(4,9,:),C(4,10,:),C(4,11,:),C(4,12,:),C(4,13,:),C(4,14,:));
fclose(fid4);

fid5= fopen('E:\孙国伟\work\data65.txt','w');
    fprintf(fid5,'%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\r\n',A5(:,:));%,C(5,2,:),C(5,3,:),C(5,4,:),C(5,5,:),C(5,6,:),C(5,7,:),C(5,8,:),C(5,9,:),C(5,10,:),C(5,11,:),C(5,12,:),C(5,13,:),C(5,14,:));
fclose(fid5);

fid6= fopen('E:\孙国伟\work\data66.txt','w');
    fprintf(fid6,'%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\r\n',A6(:,:));%,C(2,:),C(3,:),C(1,4,:),C(1,5,:),C(1,6,:),C(1,7,:),C(1,,:),C(1,9,:),C(1,10,:),C(1,11,:),C(1,12,:),C(1,13,:),C(1,14,:));
fclose(fid6);

fid7= fopen('E:\孙国伟\work\data67.txt','w');
    fprintf(fid7,'%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\r\n',A7(:,:));%,C(2,:),C(3,:),C(1,4,:),C(1,5,:),C(1,6,:),C(1,7,:),C(1,,:),C(1,9,:),C(1,10,:),C(1,11,:),C(1,12,:),C(1,13,:),C(1,14,:));
fclose(fid7);

fid8= fopen('E:\孙国伟\work\data68.txt','w');
    fprintf(fid8,'%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\r\n',A8(:,:));%,C(2,:),C(3,:),C(1,4,:),C(1,5,:),C(1,6,:),C(1,7,:),C(1,,:),C(1,9,:),C(1,10,:),C(1,11,:),C(1,12,:),C(1,13,:),C(1,14,:));
fclose(fid8);

fid9= fopen('E:\孙国伟\work\data69.txt','w');
    fprintf(fid9,'%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\r\n',A9(:,:));%,C(2,:),C(3,:),C(1,4,:),C(1,5,:),C(1,6,:),C(1,7,:),C(1,,:),C(1,9,:),C(1,10,:),C(1,11,:),C(1,12,:),C(1,13,:),C(1,14,:));
fclose(fid9);

fid10= fopen('E:\孙国伟\work\data610.txt','w');
    fprintf(fid10,'%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\r\n',A10(:,:));%,C(2,:),C(3,:),C(1,4,:),C(1,5,:),C(1,6,:),C(1,7,:),C(1,,:),C(1,9,:),C(1,10,:),C(1,11,:),C(1,12,:),C(1,13,:),C(1,14,:));
fclose(fid10);

fid11= fopen('E:\孙国伟\work\data611.txt','w');
    fprintf(fid11,'%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\r\n',A11(:,:));%,C(2,:),C(3,:),C(1,4,:),C(1,5,:),C(1,6,:),C(1,7,:),C(1,,:),C(1,9,:),C(1,10,:),C(1,11,:),C(1,12,:),C(1,13,:),C(1,14,:));
fclose(fid11);

fid12= fopen('E:\孙国伟\work\data612.txt','w');
    fprintf(fid12,'%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\r\n',A12(:,:));%,C(2,:),C(3,:),C(1,4,:),C(1,5,:),C(1,6,:),C(1,7,:),C(1,,:),C(1,9,:),C(1,10,:),C(1,11,:),C(1,12,:),C(1,13,:),C(1,14,:));
fclose(fid12);