📄 sinsshuju.m
字号:
clc;
clear;
%1.设置各种状态参数
i=(1:2000)'; %x axis
m1=(1:2000)';
m2=(1:2000)';
m3=(1:2000)';
m4=(1:2000)';
m5=(1:2000)';
m6=(1:2000)';
m7=(1:2000)';
m8=(1:2000)';
m9=(1:2000)';
m10=(1:2000)';
m11=(1:2000)';
m12=(1:2000)';
m13=(1:2000)';
m14=(1:2000)';
m15=(1:2000)';
m16=(1:2000)';
m17=(1:2000)';
m18=(1:2000)';
%2.读出数据%单位度/秒
Wie=15*pi/(180*3600); %地球自传角速率
Plat0=45*pi/180; %Lat latitude 纬度
Plon0=120*pi/180; %Lon longitude 经度
Ph0=3000; %H altitude
A0=3*pi/180;
A1=5*pi/180;
A2=2000;
B0=5*pi/180;
B1=5*pi/180;
B2=5*pi/180;
w0=2*pi/10;
w1=2*pi/10;
w2=2*pi/10;
w3=2*pi/7200;
Re=6378137;
f=1/298.257;
Dk_gx=sqrt(0.001*pi/180)*randn(1,2000); %刻度系数误差 误差表现为经度、纬度
Dk_gy=sqrt(0.001*pi/180)*randn(1,2000);
Dk_gz=sqrt(0.001*pi/180)*randn(1,2000);
Eb_x=sqrt(0.1*pi/(3600*180))*randn(1,2000); %逐次启动漂移 误差表现为经度、纬度
Eb_y=sqrt(0.1*pi/(3600*180))*randn(1,2000);
Eb_z=sqrt(0.1*pi/(3600*180))*randn(1,2000);
Wr_x=sqrt(0.1*pi/(3600*180))*randn(1,2000); %驱动噪声
Wr_y=sqrt(0.1*pi/(3600*180))*randn(1,2000);
Wr_z=sqrt(0.1*pi/(3600*180))*randn(1,2000);
Wg_x=0.01*pi*randn(1,2000)/180; %白噪声
Wg_y=0.01*pi*randn(1,2000)/180;
Wg_z=0.01*pi*randn(1,2000)/180;
Er_xold=0.05*pi/(3600*180); %初始方差
Er_yold=0.05*pi/(3600*180);
Er_zold=0.05*pi/(3600*180);
Dk_ax=sqrt(0.001)*randn(1,2000); %刻度系数误差 误差表现为经度、纬度 随机常数
Dk_ay=sqrt(0.001)*randn(1,2000);
Dk_az=sqrt(0.001)*randn(1,2000);
Wa_x=0.005*randn(1,2000); %驱动噪声
Wa_y=0.005*randn(1,2000);
Wa_z=0.005*randn(1,2000);
Db_xold=0.001;
Db_yold=0.001;
Db_zold=0.001;
E1old=3*pi/(60*180); %平台角误差东向
E2old=3*pi/(60*180); %北向
E3old=5*pi/(60*180); %天向
E4old=0; %速度误差东向
E5old=0; %北向
E6old=0; %天向
E7old=2*pi/(60*180); %纬度误差
E8old=2*pi/(60*180); %经度误差
E9old=10; %高度
for k=0:1999
%%% 轨迹发生器 %%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%(1)设定姿态角的变化规律为时间的余弦函数（也可根据需要设定为其他函数）
Sata=B0*cos(w0*(k+1)); %Sata 俯仰角
Gama=B1*cos(w1*(k+1)); %Gama 横滚角
Sai=B2*cos(w2*(k+1)); %Sai 航向角
%(2)姿态角的一阶导数
T_sata=-B0*w0*sin(w0*(k+1));
T_gama=-B1*w1*sin(w1*(k+1));
T_sai=-B2*w2*sin(w2*(k+1));
%(3)机体系相对于地理系的转动角速率在机体系中的投影
Wtbx=cos(Gama)*T_sata+sin(Gama)*cos(Sata)*T_sai;
Wtby=T_gama-sin(Sata)*T_sai;
Wtbz=sin(Gama)*T_sata-cos(Gama)*cos(Sata)*T_sai;
%位置（纬度、经度、高度）变化规律为时间的余弦函数，及其一阶、二阶导数
Plat=Plat0+A0*cos(w3*(k+1));
Plon=Plon0+A1*cos(w3*(k+1));
Ph=Ph0+A2*cos(w3*(k+1));
Vlat=-A0*w3*cos(w3*(k+1));
Vlon=-A1*w3*cos(w3*(k+1));
Vh=-A2*w3*cos(w3*(k+1));
Alat=-A0*(w3)^2*cos(w3*(k+1));
Alon=-A1*(w3)^2*cos(w3*(k+1));
Ah=-A2*(w3)^2*cos(w3*(k+1));
%位置在地球直角坐标系的直角坐标值
x=(Re+Ph)*cos(Plat)*cos(Plon); %单位 米
Vx=Vh*cos(Plat)*cos(Plon)-Vlat*(Re+Ph)*sin(Plat)*cos(Plon)-Vlon*(Re+Ph)*cos(Plat)*sin(Plon);
Ax1=Ah*cos(Plat)*cos(Plon)-Vh*Vlat*sin(Plat)*cos(Plon)-Vh*Vlon*cos(Plat)*sin(Plon);
Ax2=-Alat*(Re+Ph)*sin(Plat)*cos(Plon)-Vh*Vlat*sin(Plat)*cos(Plon)-(Re+Ph)*(Vlat)^2*cos(Plat)*cos(Plon)+(Re+Ph)*Vlat*Vlon*sin(Plat)*sin(Plon);
Ax3=-Alon*(Re+Ph)*cos(Plat)*sin(Plon)-Vh*Vlon*cos(Plat)*sin(Plon)-(Re+Ph)*(Vlon)^2*cos(Plat)*cos(Plon)+(Re+Ph)*Vlat*Vlon*sin(Plat)*sin(Plon);
Ax=Ax1+Ax2+Ax3;
y=(Re+Ph)*cos(Plat)*sin(Plon);
Vy=Vh*cos(Plat)*sin(Plon)-Vlat*(Re+Ph)*sin(Plat)*sin(Plon)+Vlon*(Re+Ph)*cos(Plat)*cos(Plon);
Ay1=Ah*cos(Plat)*sin(Plon)-Vh*Vlat*sin(Plat)*sin(Plon)+Vh*Vlon*cos(Plat)*cos(Plon);
Ay2=-Alat*(Re+Ph)*sin(Plat)*sin(Plon)-Vh*Vlat*sin(Plat)*sin(Plon)-(Re+Ph)*(Vlat)^2*cos(Plat)*sin(Plon)-(Re+Ph)*Vlat*Vlon*sin(Plat)*cos(Plon);
Ay3=Alon*(Re+Ph)*cos(Plat)*cos(Plon)+Vh*Vlon*cos(Plat)*cos(Plon)-(Re+Ph)*(Vlon)^2*cos(Plat)*sin(Plon)-(Re+Ph)*Vlat*Vlon*sin(Plat)*cos(Plon);
Ay=Ay1+Ay2+Ay3;
z=(Re+Ph)*sin(Plat);
Vz=Vh*sin(Plat)+Vlat*(Re+Ph)*cos(Plat);
Az1=Ah*sin(Plat)+Vh*Vlat*cos(Plat);
Az2=Alat*(Re+Ph)*cos(Plat)+Vh*Vlat*cos(Plat)-(Re+Ph)*(Vlat)^2*sin(Plat);
Az=Az1+Az2;
%地球系向地理系的坐标转化矩阵Tr
c11=-sin(Plon);
c12=cos(Plon);
c21=-sin(Plat)*cos(Plon);
c22=-sin(Plat)*sin(Plon);
c23=cos(Plat);
c31=cos(Plat)*cos(Plon);
c32=cos(Plat)*sin(Plon);
c33=sin(Plat);
Tr=[c11 c12 0;c21 c22 c23;c31 c32 c33];
%地理系相对于地球系的速度在地理系中的投影Vet
V=[Vx;Vy;Vz];
Vet=Tr*V; %Vet=[Vetx;Vety;Vetz]=[Vete;Vetn;Vetu]
%在地球系中的相对于地球加速度在地理系中的投影Ar,
%在地理系中的相对于地球加速度在地理系中的投影At
A=[Ax;Ay;Az];
Ar=Tr*A; %Ar=[Arx;Ary;Arz]
WV1=(-Vet(1)*Vet(2)*tan(Plat)+Vet(1)*Vet(3))/(Re+Ph);
WV2=(Vet(1)^2*tan(Plat)+Vet(2)*Vet(3))/(Re+Ph);
WV3=(-Vet(1)^2-Vet(2)^2)/(Re+Ph);
WV=[WV1;WV2;WV3];
At=Ar-WV; %At={Ate;Atn;Atu}加速度
%%%%陀螺仪误差、加速度误差、比力值计算%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%陀螺仪误差输出
t11=cos(Gama)*cos(Sai)-sin(Gama)*sin(Sata)*sin(Sai);
t12=cos(Gama)*sin(Sai)+sin(Gama)*sin(Sata)*cos(Sai);
t13=-sin(Gama)*cos(Sata);
t21=-cos(Sata)*sin(Sai);
t22=cos(Sata)*cos(Sai);
t23=sin(Sata);
t31=sin(Gama)*cos(Sai)+cos(Gama)*sin(Sata)*sin(Sai);
t32=sin(Gama)*sin(Sai)-cos(Gama)*sin(Sata)*cos(Sai);
t33=cos(Gama)*cos(Sata);
con_gb=[t11 t12 t13;t21 t22 t23;t31 t32 t33]; %地理系向机体系转化
Er_xnew=exp(-1/300)*Er_xold+Wr_x(k+1);
Er_ynew=exp(-1/300)*Er_yold+Wr_y(k+1);
Er_znew=exp(-1/300)*Er_zold+Wr_z(k+1);
E_x=Eb_x(k+1)+Er_xnew+Dk_gx(k+1)+Wg_x(k+1);
E_y=Eb_y(k+1)+Er_ynew+Dk_gy(k+1)+Wg_y(k+1);
E_z=Eb_z(k+1)+Er_znew+Dk_gz(k+1)+Wg_z(k+1);
Epls=[E_x;E_y;E_z];
E=con_gb'*Epls; %E=[Ee;En;Eu]地理系陀螺仪误差
%加速度误差输出
Db_xnew=exp(-1/300)*Db_xold+Wa_x(k+1);
Db_ynew=exp(-1/300)*Db_yold+Wa_y(k+1);
Db_znew=exp(-1/300)*Db_zold+Wa_z(k+1);
D_x=Db_xnew+Dk_ax(k+1);
D_y=Db_ynew+Dk_ay(k+1);
D_z=Db_znew+Dk_az(k+1);
Delt=[D_x;D_y;D_z];
D=con_gb'*Delt; %D=[De;Dn;Du]地理系加速度误差
%比力输出
g=9.7803+0.051799*sin(Plat)-0.94114*10e-6*Ph;
Wie_x=0; %地球相对于惯性系的自传角速度在地理系中的投影Wie
Wie_y=Wie*cos(Plat);
Wie_z=Wie*sin(Plat);
Wet_x=-Vet(2)/(Re+Ph);
Wet_y=-Vet(1)/(Re+Ph);
Wet_z=-Vet(1)*sin(Plat)/((Re+Ph)*cos(Plat));
F_x=At(1)-(2*Wie_z+Wet_z)*Vet(2)+(2*Wie_y+Wet_y)*Vet(3);
F_y=At(2)+(2*Wie_z+Wet_z)*Vet(1)-(2*Wie_x+Wet_x)*Vet(3);
F_z=At(3)-(2*Wie_y+Wet_y)*Vet(1)+(2*Wie_x+Wet_x)*Vet(2)+g;
F_e=F_x+D(1); %地理系的比力
F_n=F_y+D(2);
F_u=F_z+D(3);
%%%% 误差计算 %%%%
%%%%%%%%%%%%%%%%%%%%%%
%代入误差方程计算误差，加上Plat/Plon/Ph可得纬度、经度和高度
Rm=Re*(1-2*f+3*f*(sin(Plat))^2);
Rn=Re*(1+2*f*(sin(Plat))^2);
Ve=Vet(1);
Vn=Vet(2);
Vu=Vet(3);
Ee=E(1);
En=E(2);
Eu=E(3);
De=D(1);
Dn=D(2);
Du=D(3);
Fe=F_e;
Fn=F_n;
Fu=F_u;
%在每一秒中采样5次
a=k;
b=k+1;
M=100;
h=(b-a)/M;
E1=zeros(1,M+1);
E2=zeros(1,M+1);
E3=zeros(1,M+1);
E4=zeros(1,M+1);
E5=zeros(1,M+1);
E6=zeros(1,M+1);
E7=zeros(1,M+1);
E8=zeros(1,M+1);
E9=zeros(1,M+1);
E1(1)=E1old;
E2(1)=E2old;
E3(1)=E3old;
E4(1)=E4old;
E5(1)=E5old;
E6(1)=E6old;
E7(1)=E7old;
E8(1)=E8old;
E9(1)=E9old;
for j=1:M
kE11=feval('fE12',E2(j),E3(j),E5(j),E9(j),Rm,Rn,Plat,Ph,Ve,Vn,Ee);
kE21=feval('fE22',E1(j),E3(j),E4(j),E7(j),E9(j),Rm,Rn,Plat,Ph,Ve,Vn,En);
kE31=feval('fE32',E1(j),E2(j),E4(j),E7(j),E9(j),Rm,Rn,Plat,Ph,Ve,Vn,Eu);
kE41=feval('fE42',E2(j),E3(j),E4(j),E5(j),E6(j),E7(j),Rn,Plat,Ph,Ve,Vn,Vu,De,Fn,Fu);
kE51=feval('fE52',E1(j),E3(j),E4(j),E5(j),E6(j),E7(j),Rm,Rn,Plat,Ph,Ve,Vu,Vn,Dn,Fe,Fu);
kE61=feval('fE62',E1(j),E2(j),E4(j),E5(j),E7(j),E9(j),Rm,Rn,Plat,Ph,Ve,Vn,Du,Fe,Fn,g);
kE71=feval('fE72',E5(j),E9(j),Rm,Ph,Vn);
kE81=feval('fE82',E4(j),E7(j),E9(j),Rn,Plat,Ph,Ve);
kE91=feval('fE92',E6(j));
kE12=feval('fE12',E2(j)+kE21*h/2,E3(j)+kE31*h/2,E5(j)+kE51*h/2,E9(j)+kE91*h/2,Rm,Rn,Plat,Ph,Ve,Vn,Ee);
kE22=feval('fE22',E1(j)+kE11*h/2,E3(j)+kE31*h/2,E4(j)+kE41*h/2,E7(j)+kE71*h/2,E9(j)+kE91*h/2,Rm,Rn,Plat,Ph,Ve,Vn,En);
kE32=feval('fE32',E1(j)+kE11*h/2,E2(j)+kE31*h/2,E4(j)+kE41*h/2,E7(j)+kE71*h/2,E9(j)+kE91*h/2,Rm,Rn,Plat,Ph,Ve,Vn,Eu);
kE42=feval('fE42',E2(j)+kE21*h/2,E3(j)+kE31*h/2,E4(j)+kE41*h/2,E5(j)+kE51*h/2,E6(j)+kE61*h/2,E7(j)+kE71*h/2,Rn,Plat,Ph,Ve,Vn,Vu,De,Fn,Fu);
kE52=feval('fE52',E1(j)+kE11*h/2,E3(j)+kE31*h/2,E4(j)+kE41*h/2,E5(j)+kE51*h/2,E6(j)+kE61*h/2,E7(j)+kE71*h/2,Rm,Rn,Plat,Ph,Ve,Vu,Vn,Dn,Fe,Fu);
kE62=feval('fE62',E1(j)+kE11*h/2,E2(j)+kE21*h/2,E4(j)+kE41*h/2,E5(j)+kE51*h/2,E7(j)+kE71*h/2,E9(j)+kE91*h/2,Rm,Rn,Plat,Ph,Ve,Vn,Du,Fe,Fn,g);
kE72=feval('fE72',E5(j)+kE51*h/2,E9(j)+kE91*h/2,Rm,Ph,Vn);
kE82=feval('fE82',E4(j)+kE41*h/2,E7(j)+kE71*h/2,E9(j)+kE91*h/2,Rn,Plat,Ph,Ve);
kE92=feval('fE92',E6(j)+kE61*h/2);
kE13=feval('fE12',E2(j)+kE22*h/2,E3(j)+kE32*h/2,E5(j)+kE52*h/2,E9(j)+kE92*h/2,Rm,Rn,Plat,Ph,Ve,Vn,Ee);
kE23=feval('fE22',E1(j)+kE12*h/2,E3(j)+kE32*h/2,E4(j)+kE42*h/2,E7(j)+kE72*h/2,E9(j)+kE92*h/2,Rm,Rn,Plat,Ph,Ve,Vn,En);
kE33=feval('fE32',E1(j)+kE12*h/2,E2(j)+kE32*h/2,E4(j)+kE42*h/2,E7(j)+kE72*h/2,E9(j)+kE92*h/2,Rm,Rn,Plat,Ph,Ve,Vn,Eu);
kE43=feval('fE42',E2(j)+kE22*h/2,E3(j)+kE32*h/2,E4(j)+kE42*h/2,E5(j)+kE52*h/2,E6(j)+kE62*h/2,E7(j)+kE72*h/2,Rn,Plat,Ph,Ve,Vn,Vu,De,Fn,Fu);
kE53=feval('fE52',E1(j)+kE12*h/2,E3(j)+kE32*h/2,E4(j)+kE42*h/2,E5(j)+kE52*h/2,E6(j)+kE62*h/2,E7(j)+kE72*h/2,Rm,Rn,Plat,Ph,Ve,Vu,Vn,Dn,Fe,Fu);
kE63=feval('fE62',E1(j)+kE12*h/2,E2(j)+kE22*h/2,E4(j)+kE42*h/2,E5(j)+kE52*h/2,E7(j)+kE72*h/2,E9(j)+kE92*h/2,Rm,Rn,Plat,Ph,Ve,Vn,Du,Fe,Fn,g);
kE73=feval('fE72',E5(j)+kE52*h/2,E9(j)+kE92*h/2,Rm,Ph,Vn);
kE83=feval('fE82',E4(j)+kE42*h/2,E7(j)+kE72*h/2,E9(j)+kE92*h/2,Rn,Plat,Ph,Ve);
kE93=feval('fE92',E6(j)+kE62*h/2);
kE14=feval('fE12',E2(j)+kE23,E3(j)+kE33,E5(j)+kE53,E9(j)+kE93,Rm,Rn,Plat,Ph,Ve,Vn,Ee);
kE24=feval('fE22',E1(j)+kE13,E3(j)+kE33,E4(j)+kE43,E7(j)+kE73,E9(j)+kE93,Rm,Rn,Plat,Ph,Ve,Vn,En);
kE34=feval('fE32',E1(j)+kE13,E2(j)+kE33,E4(j)+kE43,E7(j)+kE73,E9(j)+kE93,Rm,Rn,Plat,Ph,Ve,Vn,Eu);
kE44=feval('fE42',E2(j)+kE23,E3(j)+kE33,E4(j)+kE43,E5(j)+kE53,E6(j)+kE63,E7(j)+kE73,Rn,Plat,Ph,Ve,Vn,Vu,De,Fn,Fu);
kE54=feval('fE52',E1(j)+kE13,E3(j)+kE33,E4(j)+kE43,E5(j)+kE53,E6(j)+kE63,E7(j)+kE73,Rm,Rn,Plat,Ph,Ve,Vu,Vn,Dn,Fe,Fu);
kE64=feval('fE62',E1(j)+kE13,E2(j)+kE23,E4(j)+kE43,E5(j)+kE53,E7(j)+kE73,E9(j)+kE93,Rm,Rn,Plat,Ph,Ve,Vn,Du,Fe,Fn,g);
kE74=feval('fE72',E5(j)+kE53,E9(j)+kE93,Rm,Ph,Vn);
kE84=feval('fE82',E4(j)+kE43,E7(j)+kE73,E9(j)+kE93,Rn,Plat,Ph,Ve);
kE94=feval('fE92',E6(j)+kE63);
E1(j+1)=E1(j)+(kE11+2*kE12+2*kE13+kE14)*h/6;
E2(j+1)=E2(j)+(kE21+2*kE22+2*kE23+kE24)*h/6;
E3(j+1)=E3(j)+(kE31+2*kE32+2*kE33+kE34)*h/6;
E4(j+1)=E4(j)+(kE41+2*kE42+2*kE43+kE44)*h/6;
E5(j+1)=E5(j)+(kE51+2*kE52+2*kE53+kE54)*h/6;
E6(j+1)=E6(j)+(kE61+2*kE62+2*kE63+kE64)*h/6;
E7(j+1)=E7(j)+(kE71+2*kE72+2*kE73+kE74)*h/6;
E8(j+1)=E8(j)+(kE81+2*kE82+2*kE83+kE84)*h/6;
E9(j+1)=E9(j)+(kE91+2*kE92+2*kE93+kE94)*h/6;
end
E1old=E1(6);
E2old=E2(6);
E3old=E3(6);
E4old=E4(6);
E5old=E5(6);
E6old=E6(6);
E7old=E7(6);
E8old=E8(6);
E9old=E9(6);
Er_xold=Er_xnew;
Er_yold=Er_ynew;
Er_zold=Er_znew;
Db_xold=Db_xnew;
Db_yold=Db_ynew;
Db_zold=Db_znew;
%图形输出
i(k+1)=k+1;
m1(k+1)=E1old;
m2(k+1)=E2old;
m3(k+1)=E3old;
m4(k+1)=E4old;
m5(k+1)=E5old;
m6(k+1)=E6old;
m7(k+1)=E7old;
m8(k+1)=E8old;
m9(k+1)=E9old;
m10(k+1)=Ee;
m11(k+1)=En;
m12(k+1)=Eu;
m13(k+1)=De;
m14(k+1)=Dn;
m15(k+1)=Du;
m16(k+1)=Fe;
m17(k+1)=Fn;
m18(k+1)=Fu;
end
subplot(3,3,1);
plot(i,m1,'r');
xlabel('时间s');
ylabel('rad/s');
title('位置偏差');
subplot(3,3,2);
plot(i,m2,'r');
subplot(3,3,3);
plot(i,m3,'r');
subplot(3,3,4);
plot(i,m4,'r');
subplot(3,3,5);
plot(i,m5,'r');
subplot(3,3,6);
plot(i,m6,'r');
subplot(3,3,7);
plot(i,m7,'r');
subplot(3,3,8);
plot(i,m8,'r');
subplot(3,3,9);
plot(i,m9,'r')⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -