?? transfer_alignment_modify.m
字號:
sy(i)=vy(i)*Tm;%單位周期內(nèi)北向運動距離
sz(i)=vz(i)*Tm;%單位周期內(nèi)天向運動距離
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h0;
elseif (t>5.60)&(t<=7.20) %序號為06的飛行狀態(tài)：左盤旋
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;%單位周期內(nèi)東向運動距離
sy(i)=vy(i)*Tm;%單位周期內(nèi)北向運動距離
sz(i)=vz(i)*Tm;%單位周期內(nèi)天向運動距離
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h0;
elseif (t>7.20)&(t<=8.00) %序號為07的飛行狀態(tài)：右盤旋
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;%單位周期內(nèi)東向運動距離
sy(i)=vy(i)*Tm;%單位周期內(nèi)北向運動距離
sz(i)=vz(i)*Tm;%單位周期內(nèi)天向運動距離
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h0;
elseif (t>8.00)&(t<=8.80) %序號為08的飛行狀態(tài)：右盤旋
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;%單位周期內(nèi)東向運動距離
sy(i)=vy(i)*Tm;%單位周期內(nèi)北向運動距離
sz(i)=vz(i)*Tm;%單位周期內(nèi)天向運動距離
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h0;
elseif (t>8.80)&(t<=9.60) %序號為09的飛行狀態(tài)：右盤旋
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;%單位周期內(nèi)東向運動距離
sy(i)=vy(i)*Tm;%單位周期內(nèi)北向運動距離
sz(i)=vz(i)*Tm;%單位周期內(nèi)天向運動距離
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h0;
elseif (t>9.60)&(t<=10.08) %序號為10的飛行狀態(tài)：平飛
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)=vx0*sin(psi(i));
vy(i)=vx0*cos(psi(i));
vz(i)=0;
sx(i)=vx(i)*Tm;%單位周期內(nèi)東向運動距離
sy(i)=vy(i)*Tm;%單位周期內(nèi)北向運動距離
sz(i)=vz(i)*Tm;%單位周期內(nèi)天向運動距離
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h0;
end %end of if
ssx=ssx+sx(i);%飛行距離累計
ssy=ssy+sy(i);
ssz=ssz+sz(i);
%format long;%設(shè)置高精度顯示
%disp('傳遞對準采樣時刻飛機/導(dǎo)彈的緯度(度)=');%完全參照飛機，是近似結(jié)果
lat=ssy/6378137;
lat1(i)=(lat+L0)*180/pi;%當前緯度近似計算
%disp(lat1(i));
%disp('傳遞對準采樣時刻飛機/導(dǎo)彈的經(jīng)度(度)=');
long1(i)=ssx/(6378137*cos(lat+L0))*180/pi+Lo0;%當前經(jīng)度近似計算
%disp(long1(i));
%disp('傳遞對準采樣時刻飛機/導(dǎo)彈的高度(米)=');
%format short;%恢復(fù)原來精度
%disp(h(i));
i=i+1;
end
elseif xz==4 % 爬高飛行
i=1;
g=g0; %飛機起飛點的重力加速度, 單位:米/(秒*秒)
dgama0=0 %傾斜角的微分，單位：弧度/秒 原值為70
dpsi0=0 %航向角的微分，單位：弧度/秒
for t=0:Tm:10.08
dgama=dgama0;
dpsi=dpsi0;
dtheta=0
if t<=1.00 %序號為01的飛行狀態(tài) 平飛
dgama=dgama0;
dpsi=dpsi0;
dtheta=0;
if (i<=1)
gama(i)=gama0; %第一次采樣時刻飛機的姿態(tài)角
psi(i)=psi0;
theta(i)=0;
else
gama(i)=gama(i-1)+dgama*Tm;%第二次采樣及以后的姿態(tài)角
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;%單位周期內(nèi)東向運動距離
sy(i)=vy(i)*Tm;%單位周期內(nèi)北向運動距離
sz(i)=vz(i)*Tm;%單位周期內(nèi)天向運動距離
dvx(i)=-wtbz(i)*vy(i); %采樣時刻線加速度在t中的分量
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h0; %采樣時刻的飛行高度
elseif (t>1.00)&(t<=4.00) %序號為02的飛行狀態(tài)：右盤旋
dgama=0;
dpsi=dpsi0;
dtheta=45*pi/180/150;
gama(i)=gama(i-1)+dgama*Tm;
psi(i)=psi(i-1)+dpsi*Tm;
theta(i)=theta(i-1)+dtheta*Tm;
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))*cos(theta(i));
vy(i)=vx0*cos(psi(i))*cos(theta(i));
vz(i)=vx0*sin(theta(i));
sx(i)=vx(i)*Tm;%單位周期內(nèi)東向運動距離
sy(i)=vy(i)*Tm;%單位周期內(nèi)北向運動距離
sz(i)=vz(i)*Tm;%單位周期內(nèi)天向運動距離
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h(i-1)+sz(i);
elseif (t>4.00)&(t<=6.00)
dgama=0;
dpsi=dpsi0;
dtheta=0;
gama(i)=gama(i-1)+dgama*Tm;
psi(i)=psi(i-1)+dpsi*Tm;
theta(i)=theta(i-1)+dtheta*Tm;
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)=vx0*sin(theta(i));
sx(i)=vx(i)*Tm;%單位周期內(nèi)東向運動距離
sy(i)=vy(i)*Tm;%單位周期內(nèi)北向運動距離
sz(i)=vz(i)*Tm;%單位周期內(nèi)天向運動距離
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h(i-1)+sz(i);
elseif (t>6.00)&(t<=9.00)
dgama=0;
dpsi=dpsi0;
dtheta=-45*pi/180/150;
gama(i)=gama(i-1)+dgama*Tm;
psi(i)=psi(i-1)+dpsi*Tm;
theta(i)=theta(i-1)+dtheta*Tm;
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))*cos(theta(i));
vy(i)=vx0*cos(psi(i))*cos(theta(i));
vz(i)=vx0*sin(theta(i));
sx(i)=vx(i)*Tm;%單位周期內(nèi)東向運動距離
sy(i)=vy(i)*Tm;%單位周期內(nèi)北向運動距離
sz(i)=vz(i)*Tm;%單位周期內(nèi)天向運動距離
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h(i-1)+sz(i);
elseif (t>9.00)&(t<=10.08)
dgama=0;
dpsi=dpsi0;
dtheta=0;
gama(i)=gama(i-1)+dgama*Tm;
psi(i)=psi(i-1)+dpsi*Tm;
theta(i)=theta(i-1)+dtheta*Tm;
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;%單位周期內(nèi)東向運動距離
sy(i)=vy(i)*Tm;%單位周期內(nèi)北向運動距離
sz(i)=vz(i)*Tm;%單位周期內(nèi)天向運動距離
dvx(i)=-wtbz(i)*vy(i);
dvy(i)=wtbz(i)*vx(i);
dvz(i)=0;
h(i)=h(i-1)+sz(i);
end
%end of if
ssx=ssx+sx(i);%飛行距離累計
ssy=ssy+sy(i);
ssz=ssz+sz(i);
%format long;%設(shè)置高精度顯示
%disp('傳遞對準采樣時刻飛機/導(dǎo)彈的緯度(度)=');%完全參照飛機，是近似結(jié)果
lat=ssy/6378137;
lat1(i)=(lat+L0)*180/pi;%當前緯度近似計算
%disp(lat1(i));
%disp('傳遞對準采樣時刻飛機/導(dǎo)彈的經(jīng)度(度)=');
long1(i)=ssx/(6378137*cos(lat+L0))*180/pi+Lo0;%當前經(jīng)度近似計算
%disp(long1(i));
%disp('傳遞對準采樣時刻飛機/導(dǎo)彈的高度(米)=');
%format short;%恢復(fù)原來精度
%disp(h(i));
i=i+1;
end
elseif xz==5 %實測數(shù)據(jù)驗證
load scsj.txt
long1=scsj(:,1);%經(jīng)度數(shù)組
lat1=scsj( :,2);%緯度數(shù)組
h=scsj( :,3);%高度數(shù)組
vx=scsj( :,4);%東向速度數(shù)組
vy=scsj( :,5);%北向速度數(shù)組
vz=scsj( :,6);%天向速度數(shù)組
dvx=scsj( :,7);%東向速度數(shù)組微分
dvy=scsj( :,8);%北向速度數(shù)組微分
dvz=scsj( :,9);%天向速度數(shù)組微分
gama=scsj( :,10);%航向角數(shù)組
psi=scsj( :,11);%傾斜角數(shù)組
theta=scsj( :,12);%俯仰角數(shù)組
wtbx=scsj( :,13);%采樣時刻b對t的角速度在t中的X軸分量
wtby=scsj( :,14);%采樣時刻b對t的角速度在t中的Y軸分量
wtbz=scsj( :,15);%采樣時刻b對t的角速度在t中的Z軸分量
end
%第三部分:開始仿真x1x2x3
i=1;
L=L0;
g=g0;
%xi為地理系t偏離初始時刻地理系t0的偏角，初始時刻偏角為0, 單位:弧度
xix=0;
xiy=0;
xiz=0;
I=eye(15);
%卡爾曼濾波器狀態(tài)估計初始值
x0=zeros(15,1);
g=g0;
x0(1,1)=10;
x0(2,1)=10;
x0(3,1)=10;%準速度誤差初始值
x0(4,1)=1*pi/180;
x0(5,1)=1*pi/180;
x0(6,1)=1.5*pi/180;%平臺誤差角初始值
x0(7,1)=1.5*pi/180;
x0(8,1)=1*pi/180;
x0(9,1)=1*pi/180;%撓曲變形角誤差初始值
x0(10,1)=4*pi/180/3600;
x0(11,1)=4*pi/180/3600;
x0(12,1)=4*pi/180/3600;%陀螺漂移誤差初始值
x0(13,1)=10^(-3)*g0;
x0(14,1)=10^(-3)*g0;
x0(15,1)=10^(-3)*g0;%加速度計零偏誤差初始值
%反饋控制用
xfeed=x0;
%保存各狀態(tài)量估計
%速度誤差 單位:米/秒
x1(1)=x0(1,1); %X軸
x2(1)=x0(2,1); %Y軸
x3(1)=x0(3,1); %Z軸
%平臺誤差角 單位:角分
x4(1)=x0(4,1); %X軸
x5(1)=x0(5,1); %Y軸
x6(1)=x0(6,1); %Z軸
%撓曲變形角 單位:角分
x7(1)=x0(7,1); %X軸
x8(1)=x0(8,1); %Y軸
x9(1)=x0(9,1); %Z軸
%陀螺漂移 單位:度/小時
x10(1)=x0(10,1); %X軸
x11(1)=x0(11,1); %Y軸
x12(1)=x0(12,1); %Z軸
%加速度計誤差 單位:米/(秒*秒)
x13(1)=x0(13,1); %X軸
x14(1)=x0(14,1); %Y軸
x15(1)=x0(15,1); %Z軸
CMS=[];%45度安裝導(dǎo)彈姿態(tài)陣累計
CMS2=[];%0度安裝導(dǎo)彈姿態(tài)陣累計
for t=0:Tm:10.08
%計算地球曲率半徑，單位：米
Rtxh=(6378.4*10^3)*(1+sin(L)^2/298.257)+h(i); %垂直子午面Rtxh=Rtx+h
Rtyh=(6378.4*10^3)*(1-(2-3*sin(L)^2)/298.257)+h(i); %子午面Rtyh=Rty+h
%計算地球自轉(zhuǎn)角速度，單位：弧度/秒
wiex=0;
?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -