?? calculate_teleout_diff12.m
字號(hào):
function Delta=Calculate_TeleOut_Diff
% 求若干整秒時(shí)刻的遙外差，要求采樣點(diǎn)與計(jì)算環(huán)境函數(shù)矩陣時(shí)選用的數(shù)據(jù)點(diǎn)對(duì)齊
% 求得的遙外差中未包含慣導(dǎo)工具誤差的補(bǔ)償量（地面測試值）
% 選取時(shí)間點(diǎn)為16s~63s,73s~128s,138~182s,192s~281s
% 考慮潛艇初速補(bǔ)償（于外測速度中扣除）
% 此函數(shù)包括三個(gè)子函數(shù)：外測數(shù)據(jù)的坐標(biāo)轉(zhuǎn)換、樣條積分和梯形積分函數(shù)
load chwx.dat; % 載入遙測視速度
load chvx.dat; % 載入發(fā)射系中的外測速度
load chx.dat; % 載入發(fā)射系中的外測位置
% 遙測參數(shù)導(dǎo)入，數(shù)據(jù)時(shí)間間隔為0.05s
tn_tele=chwx(:,1); % 遙測時(shí)間數(shù)組
wx_tele=chwx(:,2); % X方向的遙測視速度
wy_tele=chwx(:,3); % Y方向的遙測視速度
wz_tele=chwx(:,4); % Z方向的遙測視速度
% 外測參數(shù)導(dǎo)入，數(shù)據(jù)時(shí)間間隔為0.05s
% 注意到速度與位置數(shù)據(jù)的采樣時(shí)間點(diǎn)是一致的，統(tǒng)一為tn_out0
tn_out0=chvx(:,1); % 外測時(shí)間數(shù)組
vx_out0=chvx(:,2); % 發(fā)射系中X方向的外測速度
vy_out0=chvx(:,3); % 發(fā)射系中Y方向的外測速度
vz_out0=chvx(:,4); % 發(fā)射系中Z方向的外測速度
x_out0=chx(:,2); % 發(fā)射系中X方向的外測位置
y_out0=chx(:,3); % 發(fā)射系中Y方向的外測位置
z_out0=chx(:,4); % 發(fā)射系中Z方向的外測位置
format long;
disp('開始計(jì)算遙外差，來一杯咖啡慢慢品嘗......');
%At=LaunchPara(4); % 發(fā)射方位角
At=277.0627*pi/180; % 發(fā)射方位角
% ship_VN=LaunchPara(5); % 潛艇北向速度
% ship_VR=LaunchPara(6); % 潛艇天向速度
% ship_VE=LaunchPara(7); % 潛艇東向速度
ship_VN=-0.4167; % 潛艇北向速度
ship_VR=0.0; % 潛艇天向速度
ship_VE=1.53304; % 潛艇東向速度
% 北天東坐標(biāo)系到發(fā)射坐標(biāo)系的轉(zhuǎn)換矩陣
G_NRE=[cos(-At) 0 -sin(-At)
0 1 0
sin(-At) 0 cos(-At) ];
% 計(jì)算潛艇在發(fā)射坐標(biāo)系中的速度
ship_VG=G_NRE*[ship_VN;ship_VR;ship_VE];
ship_Vx=ship_VG(1);
ship_Vy=ship_VG(2);
ship_Vz=ship_VG(3);
%初始位置零點(diǎn)補(bǔ)償
ship_P=[-154.94;-0.01;-372.166];
% 遙測參數(shù)時(shí)間段為0s~286s，外測數(shù)據(jù)時(shí)間段為10.45s~287s，補(bǔ)償數(shù)據(jù)時(shí)間段為0s~300s，統(tǒng)一選取時(shí)間段為0s~286s
% 數(shù)據(jù)的時(shí)間點(diǎn)間隔均為0.05s
tn_out1=0:0.05:10.40;
kk=(286-10.45)/0.05+1;
% 注意初始時(shí)刻導(dǎo)彈坐標(biāo)分量均為0
% 用潛艇速度近似代替導(dǎo)彈初速
% 使用四種插值方法補(bǔ)齊所缺數(shù)據(jù)
interp_method=1;
switch interp_method
case 1 % 線性插值
vx_out1=interp1([0;tn_out0(1:50)],[ship_Vx;vx_out0(1:50)],tn_out1','linear');
vy_out1=interp1([0;tn_out0(1:50)],[ship_Vy;vy_out0(1:50)],tn_out1','linear');
vz_out1=interp1([0;tn_out0(1:50)],[ship_Vz;vz_out0(1:50)],tn_out1','linear');
x_out1=interp1([0;tn_out0(1:50)],[ship_P(1);x_out0(1:50)],tn_out1','linear');
y_out1=interp1([0;tn_out0(1:50)],[ship_P(2);y_out0(1:50)],tn_out1','linear');
z_out1=interp1([0;tn_out0(1:50)],[ship_P(3);z_out0(1:50)],tn_out1','linear');
case 2 % 樣條插值
vx_out1=interp1([0;tn_out0(1:50)],[ship_Vx;vx_out0(1:50)],tn_out1','spline');
vy_out1=interp1([0;tn_out0(1:50)],[ship_Vy;vy_out0(1:50)],tn_out1','spline');
vz_out1=interp1([0;tn_out0(1:50)],[ship_Vz;vz_out0(1:50)],tn_out1','spline');
x_out1=interp1([0;tn_out0(1:50)],[ship_P(1);x_out0(1:50)],tn_out1','spline');
y_out1=interp1([0;tn_out0(1:50)],[ship_P(2);y_out0(1:50)],tn_out1','spline');
z_out1=interp1([0;tn_out0(1:50)],[ship_P(3);z_out0(1:50)],tn_out1','spline');
case 3 % 立方插值
vx_out1=interp1([0;tn_out0(1:50)],[ship_Vx;vx_out0(1:50)],tn_out1','cubic');
vy_out1=interp1([0;tn_out0(1:50)],[ship_Vy;vy_out0(1:50)],tn_out1','cubic');
vz_out1=interp1([0;tn_out0(1:50)],[ship_Vz;vz_out0(1:50)],tn_out1','cubic');
x_out1=interp1([0;tn_out0(1:50)],[ship_P(1);x_out0(1:50)],tn_out1','cubic');
y_out1=interp1([0;tn_out0(1:50)],[ship_P(2);y_out0(1:50)],tn_out1','cubic');
z_out1=interp1([0;tn_out0(1:50)],[ship_P(3);z_out0(1:50)],tn_out1','cubic');
case 4 % 最近點(diǎn)插值
vx_out1=interp1([0;tn_out0(1:50)],[ship_Vx;vx_out0(1:50)],tn_out1','nearest');
vy_out1=interp1([0;tn_out0(1:50)],[ship_Vy;vy_out0(1:50)],tn_out1','nearest');
vz_out1=interp1([0;tn_out0(1:50)],[ship_Vz;vz_out0(1:50)],tn_out1','nearest');
x_out1=interp1([0;tn_out0(1:50)],[0;x_out0(1:50)],tn_out1','nearest');
y_out1=interp1([0;tn_out0(1:50)],[0;y_out0(1:50)],tn_out1','nearest');
z_out1=interp1([0;tn_out0(1:50)],[0;z_out0(1:50)],tn_out1','nearest');
otherwise
error('請(qǐng)選擇正確的插值方法');
end
% 重新組合后的發(fā)射系外測數(shù)據(jù)
% 此時(shí)的遙測與外測數(shù)據(jù)在所有時(shí)間點(diǎn)上對(duì)齊，tn_out與tn_tele起點(diǎn)、間隔一致
tn_out=[tn_out1';tn_out0(1:kk)];
vx_out=[vx_out1;vx_out0(1:kk)];
vy_out=[vy_out1;vy_out0(1:kk)];
vz_out=[vz_out1;vz_out0(1:kk)];
x_out=[x_out1;x_out0(1:kk)];
y_out=[y_out1;y_out0(1:kk)];
z_out=[z_out1;z_out0(1:kk)];
% 計(jì)算遙測視位置
x_tele=(trapezia_intergral(tn_tele,wx_tele))';
y_tele=(trapezia_intergral(tn_tele,wy_tele))';
z_tele=(trapezia_intergral(tn_tele,wz_tele))';
% 計(jì)算視位置的遙外差補(bǔ)償值
data_out=Coordinate_Transform(tn_out,vx_out,vy_out,vz_out,x_out,y_out,z_out); % 得到發(fā)射慣性系中的外測數(shù)據(jù)
% 發(fā)射慣性系中的外測視速度
wx_out_A=data_out(:,2);
wy_out_A=data_out(:,3);
wz_out_A=data_out(:,4);
% 發(fā)射慣性系中的外測視位置
x_out_A=data_out(:,5);
y_out_A=data_out(:,6);
z_out_A=data_out(:,7);
% % 計(jì)算整秒時(shí)刻遙外差
interval=tn_out(2)-tn_out(1); % 遙外測數(shù)據(jù)采樣時(shí)間間隔
fre=1.0/interval; % 遙外測數(shù)據(jù)采樣時(shí)間頻率
len=ceil(length(tn_out)/fre);
Delta_matrix=zeros(6,len);
Delta_matrix(1,:)=wx_tele(1:fre:end)'-wx_out_A(1:fre:end)';
Delta_matrix(2,:)=wy_tele(1:fre:end)'-wy_out_A(1:fre:end)';
Delta_matrix(3,:)=wz_tele(1:fre:end)'-wz_out_A(1:fre:end)';
Delta_matrix(4,:)=x_tele(1:fre:end)'-x_out_A(1:fre:end)';
Delta_matrix(5,:)=y_tele(1:fre:end)'-y_out_A(1:fre:end)';
Delta_matrix(6,:)=z_tele(1:fre:end)'-z_out_A(1:fre:end)';
% % 選取時(shí)間點(diǎn)為50s~63s,73s~128s,138s~182s,192s~281s的整秒時(shí)刻遙外差
% len1=length(find(tn_out<16));
% len2=length(find(tn_out<=60));
% len3=length(find(tn_out<73));
% len4=length(find(tn_out<=128));
% len5=length(find(tn_out<155));
% len6=length(find(tn_out<=281));
%
%
% N=(60-15)+(128-73+1)+(281-155+1);
% Delta_matrix=zeros(6,N);
%
% %16至60秒數(shù)據(jù)
% Delta_matrix(1,1:(len2-len1-1)/fre+1)=wx_tele(len1+1:fre:len2)'-wx_out_A(len1+1:fre:len2)';
% Delta_matrix(2,1:(len2-len1-1)/fre+1)=wy_tele(len1+1:fre:len2)'-wy_out_A(len1+1:fre:len2)';
% Delta_matrix(3,1:(len2-len1-1)/fre+1)=wz_tele(len1+1:fre:len2)'-wz_out_A(len1+1:fre:len2)';
% Delta_matrix(4,1:(len2-len1-1)/fre+1)=x_tele(len1+1:fre:len2)'-x_out_A(len1+1:fre:len2)';
% Delta_matrix(5,1:(len2-len1-1)/fre+1)=y_tele(len1+1:fre:len2)'-y_out_A(len1+1:fre:len2)';
% Delta_matrix(6,1:(len2-len1-1)/fre+1)=z_tele(len1+1:fre:len2)'-z_out_A(len1+1:fre:len2)';
%
% %73秒至128秒數(shù)據(jù)
% b=(len2-len1-1)/fre+1;
% Delta_matrix(1,b+1:b+(len4-len3-1)/fre+1)=wx_tele(len3+1:fre:len4)'-wx_out_A(len3+1:fre:len4)';
% Delta_matrix(2,b+1:b+(len4-len3-1)/fre+1)=wy_tele(len3+1:fre:len4)'-wy_out_A(len3+1:fre:len4)';
% Delta_matrix(3,b+1:b+(len4-len3-1)/fre+1)=wz_tele(len3+1:fre:len4)'-wz_out_A(len3+1:fre:len4)';
% Delta_matrix(4,b+1:b+(len4-len3-1)/fre+1)=x_tele(len3+1:fre:len4)'-x_out_A(len3+1:fre:len4)';
% Delta_matrix(5,b+1:b+(len4-len3-1)/fre+1)=y_tele(len3+1:fre:len4)'-y_out_A(len3+1:fre:len4)';
% Delta_matrix(6,b+1:b+(len4-len3-1)/fre+1)=z_tele(len3+1:fre:len4)'-z_out_A(len3+1:fre:len4)';
%
% %%155秒至281數(shù)據(jù)
% b=b+(len4-len3-1)/fre+1;
% Delta_matrix(1,b+1:b+(len6-len5-1)/fre+1)=wx_tele(len5+1:fre:len6)'-wx_out_A(len5+1:fre:len6)';
% Delta_matrix(2,b+1:b+(len6-len5-1)/fre+1)=wy_tele(len5+1:fre:len6)'-wy_out_A(len5+1:fre:len6)';
% Delta_matrix(3,b+1:b+(len6-len5-1)/fre+1)=wz_tele(len5+1:fre:len6)'-wz_out_A(len5+1:fre:len6)';
% Delta_matrix(4,b+1:b+(len6-len5-1)/fre+1)=x_tele(len5+1:fre:len6)'-x_out_A(len5+1:fre:len6)';
% Delta_matrix(5,b+1:b+(len6-len5-1)/fre+1)=y_tele(len5+1:fre:len6)'-y_out_A(len5+1:fre:len6)';
% Delta_matrix(6,b+1:b+(len6-len5-1)/fre+1)=z_tele(len5+1:fre:len6)'-z_out_A(len5+1:fre:len6)';
%
% 將遙外差信息寫成(6*N)*1的向量
Delta=reshape(Delta_matrix,6*len,1);
disp('遙外差計(jì)算完畢......');
%-----------------------------------------------------------------------------------------------------------------------
%-----------------------------------------------------------------------------------------------------------------------
function Waice_data=Coordinate_Transform(tn_out,vx_out,vy_out,vz_out,x_out,y_out,z_out)
% 將外測數(shù)據(jù)由發(fā)射坐標(biāo)系轉(zhuǎn)換到發(fā)射慣性系
% 將速度轉(zhuǎn)化為視速度
?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號(hào)
Ctrl + =
減小字號(hào)
Ctrl + -