📄 transfer_alignment_modify.m
字号:
%本程序为传递对准精度评估的蒙特卡洛法仿真程序的主程序
%程序中采用飞机蛇行机动飞行
function transfer_alignment_modify(hxj,sd,jd,wd,gd,aq,jcx,jcy,jcz,tcx,tcy,tcz,ncx,ncy,ncz)%输入参数依次为航向角(度)、机头速度(米/秒)、初始经度(度)、纬度(度)、高度(米),选择变量,加速度计误差白噪声方差(X,Y,Z),陀螺误差白噪声方差(X,Y,Z),挠曲变形误差白噪声方差(X,Y,Z),
%clc;
%clear; %清除内存中变量和函数
%第一部分:仿真用初始数据
%
fid = fopen('trans_data.txt','w');%清空文件内容
fprintf(fid,' ');
fclose(fid);
fid = fopen('trans_data.txt','a+');
Tm=0.02; %采样时间，单位：秒
%飞机的初始状态设置
vx0=sd; %飞机纵向初速，单位：米/秒
h0=gd; %飞机飞行高度，单位：米
psi0=hxj*pi/180;%航向角初始值，单位：弧度
gama0=0; %倾斜角初始值，单位：弧度
theta0=0; %俯仰角初始值，单位：弧度
L0=wd*pi/180; %纬度初始值，单位：弧度
Lo0=jd; %经度初始值，单位：度
xz=aq;%选择变量.
aax=jcx;%加速度计误差白噪声方差X轴
aay=jcy;%加速度计误差白噪声方差Y轴
aaz=jcz;%加速度计误差白噪声方差Z轴
abx=tcx;%陀螺误差白噪声方差X轴
aby=tcy;%陀螺误差白噪声方差Y轴
abz=tcz;%陀螺误差白噪声方差Z轴
acx=ncx;%曲变形误差白噪声方差X轴
acy=ncy;%曲变形误差白噪声方差Y轴
acz=ncz;%曲变形误差白噪声方差Z轴
ssx=0;
ssy=0;
ssz=0;
betax=12; %飞机机翼挠曲变形参数, 单位：1/秒
betay=12;
betaz=12;
rx=1;ry=-1;rz=4;%导弹偏离飞机重心距离，单位：米
wie=7.2921151467*10^(-5);%地球自转角速度，单位：弧度/秒
g0=9.7803267714*(1+0.00193185138639*sin(L0)^2)/sqrt(1-0.00669437999013*sin(L0)^2);%起始点重力加速度, 单位:米/(秒*秒)
G=zeros(15,9);%系统噪声系数阵
G(1,1)=1;
G(2,2)=1;
G(3,3)=1;
G(4,4)=1;
G(5,5)=1;
G(6,6)=1;
G(7,7)=1;
G(8,8)=1;
G(9,9)=1;
g=g0;
P0=zeros(15,15);%协方差初始阵P0
P0(1,1)=2^2;
P0(2,2)=2^2;
P0(3,3)=2^2;
P0(4,4)=(0.3*pi/180)^2;
P0(5,5)=(0.3*pi/180)^2;
P0(6,6)=(0.3*pi/180)^2;
P0(7,7)=(0.15*pi/180)^2;
P0(8,8)=(0.1*pi/180)^2;
P0(9,9)=(0.1*pi/180)^2;
P0(10,10)=(1*pi/180/3600)^2;
P0(11,11)=(1*pi/180/3600)^2;
P0(12,12)=(1*pi/180/3600)^2;
P0(13,13)=(0.2*10^(-3)*g)^2;
P0(14,14)=(0.2*10^(-3)*g)^2;
P0(15,15)=(0.2*10^(-3)*g)^2;
SWDX=10^(-4)*g0; %加速度计中间变量参数
SWDY=10^(-4)*g0;
SWDZ=10^(-4)*g0;
SWEX=2*pi/180/3600; %陀螺中间变量参数
SWEY=2*pi/180/3600;
SWEZ=2*pi/180/3600;
SWPX=sqrt(2*betax*(0.05*pi/180)^2/Tm); %挠曲变形中间变量参数
SWPY=sqrt(2*betay*(0.05*pi/180)^2/Tm);
SWPZ=sqrt(2*betaz*(0.05*pi/180)^2/Tm);
%第二部分:飞机的飞行轨迹
%计算采样时刻飞机的姿态角,地速在地理坐标系上的分量和飞行高度
%计算采样时刻飞机的线加速度dv在地理坐标系上的分量,机体坐标系b相对地理坐标系t的角速度wtb
if xz==1 %水平匀速飞行
i=1;
g=g0; %飞机起飞点的重力加速度, 单位:米/(秒*秒)
dgama0=0 %倾斜角的微分，单位：弧度/秒 原值为70
dpsi0=0 %航向角的微分，单位：弧度/秒
dtheta0=0; %俯仰角的微分，单位：弧度/秒
for t=0:Tm:10.08
dgama=dgama0;
dpsi=dpsi0;
dtheta=dtheta0;
if (i<=1)
gama(i)=gama0; %第一次采样时刻飞机的姿态角
psi(i)=psi0;
theta(i)=theta0;
else
gama(i)=gama(i-1)+dgama*Tm;%第二次采样及以后的姿态角
psi(i)=psi(i-1)+dpsi*Tm;
theta(i)=0;
end
wtbx(i)=dtheta*cos(psi(i))+dgama*cos(theta(i))*sin(psi(i));
wtby(i)=-dtheta*sin(psi(i))+dgama*cos(theta(i))*cos(psi(i));
wtbz(i)=dgama*sin(theta(i))-dpsi;
vx(i)=vx0*sin(psi(i));
vy(i)=vx0*cos(psi(i));
vz(i)=0;
sx(i)=vx(i)*Tm;%单位周期内东向运动距离
sy(i)=vy(i)*Tm;%单位周期内北向运动距离
sz(i)=vz(i)*Tm;%单位周期内天向运动距离
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h0;
%end of if
ssx=ssx+sx(i);%飞行距离累计
ssy=ssy+sy(i);
ssz=ssz+sz(i);
%format long;%设置高精度显示
%disp('传递对准采样时刻飞机/导弹的纬度(度)=');%完全参照飞机，是近似结果
lat=ssy/6378137;
lat1(i)=(lat+L0)*180/pi;%当前纬度近似计算
%disp(lat1(i));
%disp('传递对准采样时刻飞机/导弹的经度(度)=');
long1(i)=ssx/(6378137*cos(lat+L0))*180/pi+Lo0;%当前经度近似计算
%disp(long1(i));
%disp('传递对准采样时刻飞机/导弹的高度(米)=');
%format short;%恢复原来精度
%disp(h(i));
i=i+1;
end
elseif xz==2%水平匀加速飞行
i=1;
g=g0; %飞机起飞点的重力加速度, 单位:米/(秒*秒)
dgama0=0 %倾斜角的微分，单位：弧度/秒 原值为70
dpsi0=0 %航向角的微分，单位：弧度/秒
dtheta0=0; %俯仰角的微分，单位：弧度/秒
for t=0:Tm:10.08
if t<=5.0 %水平匀速过程前5秒
dvx2=g0*sqrt(2)/2
dvy2=dvx2
dgama=dgama0;
dpsi=dpsi0;
dtheta=dtheta0;
if (i<=1)
gama(i)=gama0; %第一次采样时刻飞机的姿态角
psi(i)=psi0;
theta(i)=theta0;
else
gama(i)=gama(i-1)+dgama*Tm;%第二次采样及以后的姿态角
psi(i)=psi(i-1)+dpsi*Tm;
theta(i)=0;
end
wtbx(i)=dtheta*cos(psi(i))+dgama*cos(theta(i))*sin(psi(i));
wtby(i)=-dtheta*sin(psi(i))+dgama*cos(theta(i))*cos(psi(i));
wtbz(i)=dgama*sin(theta(i))-dpsi;
if(i<=1)
vx(i)=vx0*sin(psi(i))
vy(i)=vx0*cos(psi(i))
else
vx(i)=vx(i-1)+dvx2*sin(psi(i))*Tm;
vy(i)=vy(i-1)+dvx2*cos(psi(i))*Tm;
end
vz(i)=0;
sx(i)=vx(i)*Tm+0.5*dvx2*sin(psi(i))*Tm*Tm;%单位周期内东向运动距离
sy(i)=vy(i)*Tm+0.5*dvy2*cos(psi(i))*Tm*Tm;%单位周期内北向运动距离
sz(i)=vz(i)*Tm;%单位周期内天向运动距离
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h0;
elseif (t>5.0)&(t<=10.08) %后5.08秒匀加速飞行
dgama=0;
dpsi=0;
dtheta=dtheta0;
gama(i)=gama(i-1)+dgama*Tm;
psi(i)=psi(i-1)+dpsi*Tm;
theta(i)=0;
wtbx(i)=dtheta*cos(psi(i))+dgama*cos(theta(i))*sin(psi(i));
wtby(i)=-dtheta*sin(psi(i))+dgama*cos(theta(i))*cos(psi(i));
wtbz(i)=dgama*sin(theta(i))-dpsi;
vx(i)=vx(i-1)*sin(psi(i));
vy(i)=vx(i-1)*cos(psi(i));
vz(i)=0;
sx(i)=vx(i)*Tm;%单位周期内东向运动距离
sy(i)=vy(i)*Tm;%单位周期内北向运动距离
sz(i)=vz(i)*Tm;%单位周期内天向运动距离
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h0;
end
ssx=ssx+sx(i);%飞行距离累计
ssy=ssy+sy(i);
ssz=ssz+sz(i);
%format long;%设置高精度显示
%disp('传递对准采样时刻飞机/导弹的纬度(度)=');%完全参照飞机，是近似结果
lat=ssy/6378137;
lat1(i)=(lat+L0)*180/pi;%当前纬度近似计算
%disp(lat1(i));
%disp('传递对准采样时刻飞机/导弹的经度(度)=');
long1(i)=ssx/(6378137*cos(lat+L0))*180/pi+Lo0;%当前经度近似计算
%disp(long1(i));
%disp('传递对准采样时刻飞机/导弹的高度(米)=');
%format short;%恢复原来精度
%disp(h(i));
i=i+1;
end
u=2
elseif xz==3 %蛇行机动
i=1;
g=g0; %飞机起飞点的重力加速度, 单位:米/(秒*秒)
dgama0=50*pi/180; %倾斜角的微分，单位：弧度/秒 原值为70
dpsi0=6.59*pi/180; %航向角的微分，单位：弧度/秒
dtheta0=0; %俯仰角的微分，单位：弧度/秒
for t=0:Tm:10.08
if t<=0.80 %序号为01的飞行状态：右盘旋
dgama=dgama0;
dpsi=dpsi0;
dtheta=dtheta0;
if (i<=1)
gama(i)=gama0; %第一次采样时刻飞机的姿态角
psi(i)=psi0;
theta(i)=theta0;
else
gama(i)=gama(i-1)+dgama*Tm;%第二次采样及以后的姿态角
psi(i)=psi(i-1)+dpsi*Tm;
theta(i)=0;
end
wtbx(i)=dtheta*cos(psi(i))+dgama*cos(theta(i))*sin(psi(i)); %采样时刻b对t的角速度在t中的分量
wtby(i)=-dtheta*sin(psi(i))+dgama*cos(theta(i))*cos(psi(i));
wtbz(i)=dgama*sin(theta(i))-dpsi;
vx(i)=vx0*sin(psi(i)); %采样时刻地速在t中的分量
vy(i)=vx0*cos(psi(i));
vz(i)=0;
sx(i)=vx(i)*Tm;%单位周期内东向运动距离
sy(i)=vy(i)*Tm;%单位周期内北向运动距离
sz(i)=vz(i)*Tm;%单位周期内天向运动距离
dvx(i)=-wtbz(i)*vy(i); %采样时刻线加速度在t中的分量
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h0; %采样时刻的飞行高度
elseif (t>0.80)&(t<=1.60) %序号为02的飞行状态：右盘旋
dgama=0;
dpsi=dpsi0;
dtheta=dtheta0;
gama(i)=gama(i-1)+dgama*Tm;
psi(i)=psi(i-1)+dpsi*Tm;
theta(i)=0;
wtbx(i)=dtheta*cos(psi(i))+dgama*cos(theta(i))*sin(psi(i));
wtby(i)=-dtheta*sin(psi(i))+dgama*cos(theta(i))*cos(psi(i));
wtbz(i)=dgama*sin(theta(i))-dpsi;
vx(i)=vx0*sin(psi(i));
vy(i)=vx0*cos(psi(i));
vz(i)=0;
sx(i)=vx(i)*Tm;%单位周期内东向运动距离
sy(i)=vy(i)*Tm;%单位周期内北向运动距离
sz(i)=vz(i)*Tm;%单位周期内天向运动距离
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h0;
elseif (t>1.60)&(t<=2.40) %序号为03的飞行状态：右盘旋
dgama=-dgama0;
dpsi=dpsi0;
dtheta=dtheta0;
gama(i)=gama(i-1)+dgama*Tm;
psi(i)=psi(i-1)+dpsi*Tm;
theta(i)=0;
wtbx(i)=dtheta*cos(psi(i))+dgama*cos(theta(i))*sin(psi(i));
wtby(i)=-dtheta*sin(psi(i))+dgama*cos(theta(i))*cos(psi(i));
wtbz(i)=dgama*sin(theta(i))-dpsi;
vx(i)=vx0*sin(psi(i));
vy(i)=vx0*cos(psi(i));
vz(i)=0;
sx(i)=vx(i)*Tm;%单位周期内东向运动距离
sy(i)=vy(i)*Tm;%单位周期内北向运动距离
sz(i)=vz(i)*Tm;%单位周期内天向运动距离
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h0;
elseif (t>2.40)&(t<=4.00) %序号为04的飞行状态：左盘旋
dgama=-dgama0;
dpsi=-dpsi0;
dtheta=dtheta0;
gama(i)=gama(i-1)+dgama*Tm;
psi(i)=psi(i-1)+dpsi*Tm;
theta(i)=0;
wtbx(i)=dtheta*cos(psi(i))+dgama*cos(theta(i))*sin(psi(i));
wtby(i)=-dtheta*sin(psi(i))+dgama*cos(theta(i))*cos(psi(i));
wtbz(i)=dgama*sin(theta(i))-dpsi;
vx(i)=vx0*sin(psi(i));
vy(i)=vx0*cos(psi(i));
vz(i)=0;
sx(i)=vx(i)*Tm;%单位周期内东向运动距离
sy(i)=vy(i)*Tm;%单位周期内北向运动距离
sz(i)=vz(i)*Tm;%单位周期内天向运动距离
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h0;
elseif (t>4.00)&(t<=5.60) %序号为05的飞行状态：左盘旋
dgama=0;
dpsi=-dpsi0;
dtheta=dtheta0;
gama(i)=gama(i-1)+dgama*Tm;
psi(i)=psi(i-1)+dpsi*Tm;
theta(i)=0;
wtbx(i)=dtheta*cos(psi(i))+dgama*cos(theta(i))*sin(psi(i));
wtby(i)=-dtheta*sin(psi(i))+dgama*cos(theta(i))*cos(psi(i));
wtbz(i)=dgama*sin(theta(i))-dpsi;
vx(i)=vx0*sin(psi(i));
vy(i)=vx0*cos(psi(i));
vz(i)=0;
sx(i)=vx(i)*Tm;%单位周期内东向运动距离
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -