📄 calculate_teleout_diff12.m
字号:
function Delta=Calculate_TeleOut_Diff
% 求若干整秒时刻的遥外差，要求采样点与计算环境函数矩阵时选用的数据点对齐
% 求得的遥外差中未包含惯导工具误差的补偿量（地面测试值）
% 选取时间点为16s~63s,73s~128s,138~182s,192s~281s
% 考虑潜艇初速补偿（于外测速度中扣除）
% 此函数包括三个子函数：外测数据的坐标转换、样条积分和梯形积分函数
load chwx.dat; % 载入遥测视速度
load chvx.dat; % 载入发射系中的外测速度
load chx.dat; % 载入发射系中的外测位置
% 遥测参数导入，数据时间间隔为0.05s
tn_tele=chwx(:,1); % 遥测时间数组
wx_tele=chwx(:,2); % X方向的遥测视速度
wy_tele=chwx(:,3); % Y方向的遥测视速度
wz_tele=chwx(:,4); % Z方向的遥测视速度
% 外测参数导入，数据时间间隔为0.05s
% 注意到速度与位置数据的采样时间点是一致的，统一为tn_out0
tn_out0=chvx(:,1); % 外测时间数组
vx_out0=chvx(:,2); % 发射系中X方向的外测速度
vy_out0=chvx(:,3); % 发射系中Y方向的外测速度
vz_out0=chvx(:,4); % 发射系中Z方向的外测速度
x_out0=chx(:,2); % 发射系中X方向的外测位置
y_out0=chx(:,3); % 发射系中Y方向的外测位置
z_out0=chx(:,4); % 发射系中Z方向的外测位置
format long;
disp('开始计算遥外差，来一杯咖啡慢慢品尝......');
%At=LaunchPara(4); % 发射方位角
At=277.0627*pi/180; % 发射方位角
% ship_VN=LaunchPara(5); % 潜艇北向速度
% ship_VR=LaunchPara(6); % 潜艇天向速度
% ship_VE=LaunchPara(7); % 潜艇东向速度
ship_VN=-0.4167; % 潜艇北向速度
ship_VR=0.0; % 潜艇天向速度
ship_VE=1.53304; % 潜艇东向速度
% 北天东坐标系到发射坐标系的转换矩阵
G_NRE=[cos(-At) 0 -sin(-At)
0 1 0
sin(-At) 0 cos(-At) ];
% 计算潜艇在发射坐标系中的速度
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);
%初始位置零点补偿
ship_P=[-154.94;-0.01;-372.166];
% 遥测参数时间段为0s~286s，外测数据时间段为10.45s~287s，补偿数据时间段为0s~300s，统一选取时间段为0s~286s
% 数据的时间点间隔均为0.05s
tn_out1=0:0.05:10.40;
kk=(286-10.45)/0.05+1;
% 注意初始时刻导弹坐标分量均为0
% 用潜艇速度近似代替导弹初速
% 使用四种插值方法补齐所缺数据
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 % 最近点插值
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('请选择正确的插值方法');
end
% 重新组合后的发射系外测数据
% 此时的遥测与外测数据在所有时间点上对齐，tn_out与tn_tele起点、间隔一致
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)];
% 计算遥测视位置
x_tele=(trapezia_intergral(tn_tele,wx_tele))';
y_tele=(trapezia_intergral(tn_tele,wy_tele))';
z_tele=(trapezia_intergral(tn_tele,wz_tele))';
% 计算视位置的遥外差补偿值
data_out=Coordinate_Transform(tn_out,vx_out,vy_out,vz_out,x_out,y_out,z_out); % 得到发射惯性系中的外测数据
% 发射惯性系中的外测视速度
wx_out_A=data_out(:,2);
wy_out_A=data_out(:,3);
wz_out_A=data_out(:,4);
% 发射惯性系中的外测视位置
x_out_A=data_out(:,5);
y_out_A=data_out(:,6);
z_out_A=data_out(:,7);
% % 计算整秒时刻遥外差
interval=tn_out(2)-tn_out(1); % 遥外测数据采样时间间隔
fre=1.0/interval; % 遥外测数据采样时间频率
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)';
% % 选取时间点为50s~63s,73s~128s,138s~182s,192s~281s的整秒时刻遥外差
% 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秒数据
% 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秒数据
% 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数据
% 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('遥外差计算完毕......');
%-----------------------------------------------------------------------------------------------------------------------
%-----------------------------------------------------------------------------------------------------------------------
function Waice_data=Coordinate_Transform(tn_out,vx_out,vy_out,vz_out,x_out,y_out,z_out)
% 将外测数据由发射坐标系转换到发射惯性系
% 将速度转化为视速度
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -