📄 transfer_alignment_modify.m
字号:
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>5.60)&(t<=7.20) %序号为06的飞行状态：左盘旋
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>7.20)&(t<=8.00) %序号为07的飞行状态：右盘旋
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>8.00)&(t<=8.80) %序号为08的飞行状态：右盘旋
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>8.80)&(t<=9.60) %序号为09的飞行状态：右盘旋
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>9.60)&(t<=10.08) %序号为10的飞行状态：平飞
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;%单位周期内东向运动距离
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 %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==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的飞行状态 平飞
dgama=dgama0;
dpsi=dpsi0;
dtheta=0;
if (i<=1)
gama(i)=gama0; %第一次采样时刻飞机的姿态角
psi(i)=psi0;
theta(i)=0;
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>1.00)&(t<=4.00) %序号为02的飞行状态：右盘旋
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;%单位周期内东向运动距离
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)=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;%单位周期内东向运动距离
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)=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;%单位周期内东向运动距离
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)=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;%单位周期内东向运动距离
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)=h(i-1)+sz(i);
end
%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==5 %实测数据验证
load scsj.txt
long1=scsj(:,1);%经度数组
lat1=scsj( :,2);%纬度数组
h=scsj( :,3);%高度数组
vx=scsj( :,4);%东向速度数组
vy=scsj( :,5);%北向速度数组
vz=scsj( :,6);%天向速度数组
dvx=scsj( :,7);%东向速度数组微分
dvy=scsj( :,8);%北向速度数组微分
dvz=scsj( :,9);%天向速度数组微分
gama=scsj( :,10);%航向角数组
psi=scsj( :,11);%倾斜角数组
theta=scsj( :,12);%俯仰角数组
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);
%卡尔曼滤波器状态估计初始值
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;
%保存各状态量估计
%速度误差 单位:米/秒
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度安装导弹姿态阵累计
CMS2=[];%0度安装导弹姿态阵累计
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
%计算地球自转角速度，单位：弧度/秒
wiex=0;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -