?? 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 + -