📄 calculate_teleout_diff12.m
字号:
% Waice_data=Coordinate_Transform(tn_out,vx_out,vy_out,vz_out,x_out,y_out,z_out)
% tn_out--时间序列
% vx_out,vy_out,vz_out--发射系中的外测速度(数组)
% x_out,y_out,z_out--发射系中的外测位置(数组)
% Waice_data--发射惯性系中的外测数据（矩阵）
% At=LaunchPara(4)*pi/180; % 发射方位角
% B0=LaunchPara(2)*pi/180; % 发射点大地纬度
% L0=LaunchPara(1)*pi/180; % 发射点大地经度
% H0=LaunchPara(3); % 发射点高程（水下为负值）
% Bt=LaunchPara(2)*pi/180; % 发射点天文纬度
% Lt=LaunchPara(1)*pi/180; % 发射点天文经度
% ship_VN=LaunchPara(5); % 潜艇北向速度
% ship_VR=LaunchPara(6); % 潜艇天向速度
% ship_VE=LaunchPara(7); % 潜艇东向速度
At=277.0627*pi/180; % 发射方位角
B0=38.60737*pi/180; % 发射点大地纬度
L0=121.3345/180*pi; % 发射点大地经度
H0=-2.695068e1; % 发射点高程（水下为负值）
ship_VN=-0.4167; % 潜艇北向速度
ship_VR=0.0; % 潜艇天向速度
ship_VE=1.53304; % 潜艇东向速度
Ae=6378145; % 地球长半轴
Be=6356755; % 地球短半轴
alpha=1/298.275; % 地球扁率
e2=0.006694385; % 偏心率的平方
Omega_e=7.292115e-5; % 地球自转角速度
GM=3.986005e14; % 地球引力常数
J2=1.08263e-3; % 地球引力二阶带谐系数
n=length(tn_out); % 时刻数
% 北天东坐标系到发射坐标系的转换矩阵
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);
% 求地心到发射点的矢径在发射系中的分量
N0=Ae/sqrt(1-(2*alpha-alpha^2)*(sin(B0))^2); % 发射点的卯酉圈半径
N0=Ae*(1+alpha*sin(B0)*sin(B0));
% 发射点在地心坐标系中的分量
X0=(N0+H0)*cos(B0)*cos(L0);
Y0=(N0+H0)*cos(B0)*sin(L0);
Z0=[N0*(1-e2)+H0]*sin(B0);
% 地心坐标系到发射坐标系的转换矩阵
GE=[-sin(At)*sin(L0)-cos(At)*sin(B0)*cos(L0) sin(At)*cos(L0)-cos(At)*sin(B0)*sin(L0) cos(At)*cos(B0)
cos(B0)*cos(L0) cos(B0)*sin(L0) sin(B0)
-cos(At)*sin(L0)+sin(At)*sin(B0)*cos(L0) cos(At)*cos(L0)+sin(At)*sin(B0)*sin(L0) -sin(At)*cos(B0)];
r0_G=GE*[X0;Y0;Z0];
% 地球自转角速度在发射系中的分量
% 可推导该分量与发射惯性系中的分量一致！！！
Omega_e_G=Omega_e*[ cos(B0)*cos(At)
sin(B0)
-cos(B0)*sin(At) ];
Matrix_AG=zeros(3,3,n); % 初始化各个时刻的发射系－>发射惯性系转换矩阵
g_A=zeros(3,n); % 初始化各个时刻的地球引力加速度（发射惯性系中）
Omega_e_A=zeros(3,n); % 初始化各个时刻的地球自转角速度（发射惯性系中）
% 计算t时刻的坐标转换矩阵（发射系－>发射惯性系）、地球引力加速度值（发射惯性系）和地球自转角速度值（发射惯性系）
for k=1:n % 循环次数，对应每个时刻
t=tn_out(k);
A=[ cos(At)*cos(B0) sin(B0) -sin(At)*cos(B0)
-cos(At)*sin(B0) cos(B0) sin(At)*sin(B0)
sin(At) 0 cos(At) ];
B=[ 1 0 0
0 cos(Omega_e*t) sin(Omega_e*t)
0 -sin(Omega_e*t) cos(Omega_e*t) ];
Matrix_AG(:,:,k)=A'*B'*A; % 得到t时刻的坐标转换矩阵
r1_G=[x_out(k);y_out(k);z_out(k)]; % r1_G--发射系中发射点O至导弹质心的矢径（外测数据引入）
r_G=r0_G+r1_G; % 地心至导弹质心的矢径在发射系中的分量
fi=asin(dot(r_G,Omega_e_G)/(norm(r_G)*norm(Omega_e_G))); % 导弹质心的地心纬度
J=1.5*J2;
gr=-GM/((norm(r_G))^2)*(1+J*((Ae/norm(r_G))^2)*(1-5*(sin(fi))^2));
gwe=-2*GM/((norm(r_G))^2)*J*((Ae/norm(r_G))^2)*sin(fi);
g_G=gr/norm(r_G)*r_G+gwe/norm(Omega_e_G)*Omega_e_G; % 考虑J2项的地球引力加速度在发射系中的分量
g_A(:,k)=Matrix_AG(:,:,k)*g_G; % 地球引力加速度在发射惯性系中的分量
Omega_e_A(:,k)=Matrix_AG(:,:,k)*Omega_e_G; % 发射系相对于发射惯性系的旋转角速度（即地球自转角速度）在发射惯性系中的投影
end
r0_A=Matrix_AG(:,:,1)*r0_G; % 计算起飞时刻地心至发射点的矢径在发射惯性系中的投影
g_Ax=g_A(1,:)'; % g_A在发射惯性系x轴方向的分量
g_Ay=g_A(2,:)'; % g_A在发射惯性系y轴方向的分量
g_Az=g_A(3,:)'; % g_A在发射惯性系z轴方向的分量
% 积分计算各时刻因地球引力加速度产生的位移rg_A和速度vg_A （发射惯性系中）
vg_Ax=(trapezia_intergral(tn_out,g_Ax))'; % vg_A在发射惯性系x轴方向的分量
vg_Ay=(trapezia_intergral(tn_out,g_Ay))'; % vg_A在发射惯性系y轴方向的分量
vg_Az=(trapezia_intergral(tn_out,g_Az))'; % vg_A在发射惯性系z轴方向的分量
rg_Ax=(trapezia_intergral(tn_out,vg_Ax))'; % rg_A在发射惯性系x轴方向的分量
rg_Ay=(trapezia_intergral(tn_out,vg_Ay))'; % rg_A在发射惯性系y轴方向的分量
rg_Az=(trapezia_intergral(tn_out,vg_Az))'; % rg_A在发射惯性系z轴方向的分量
% 合成矢量形式
vg_A=[vg_Ax vg_Ay vg_Az];
rg_A=[rg_Ax rg_Ay rg_Az];
% 初始化所有时刻的外测视速度和外测视位置（发射惯性系中）
w_out_A=zeros(3,n);
r_out_A=zeros(3,n);
% 计算所有时刻的外测视速度和外测视位置（发射惯性系中）
for i=1:n % 循环次数，对应每个时刻
t=tn_out(i);
% 导弹在发射系中的外测位置（扣除艇速影响）
r1_G=[x_out(i)-ship_Vx*t;y_out(i)-ship_Vy*t;z_out(i)-ship_Vz*t]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
r1_G=[x_out(i);y_out(i);z_out(i)]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 导弹在发射系中的外测速度（扣除艇速影响）
v_G=[vx_out(i)-ship_Vx;vy_out(i)-ship_Vy;vz_out(i)-ship_Vz];
v_G=[vx_out(i);vy_out(i);vz_out(i)];
% 将地球自转角速度表示为矩阵形式从而将矢量叉乘转换为矩阵与矢量点乘
Omega_Matrix=[ 0 -Omega_e_A(3,i) Omega_e_A(2,i)
Omega_e_A(3,i) 0 -Omega_e_A(1,i)
-Omega_e_A(2,i) Omega_e_A(1,i) 0 ];
% 外测视速度（发射惯性系中）
w_out_A(:,i)=Matrix_AG(:,:,i)*v_G+Omega_Matrix*Matrix_AG(:,:,i)*(r0_G+r1_G)-Omega_Matrix*r0_A-vg_A(i,:)'-ship_VG;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 外测视位置（发射惯性系中）
r_out_A(:,i)=Matrix_AG(:,:,i)*(r0_G+r1_G)-r0_A-Omega_Matrix*r0_A*t-rg_A(i,:)'-ship_VG*t;
end
% 合成变换后的外测数组
% 为n*7的矩阵，第1列是时间，第2~4列是发射惯性系中的外测视速度，第5~7列是发射惯性系中的外测视位置
Waice_data=zeros(n,7);
for j=1:n
Waice_data(j,:)=[tn_out(j) w_out_A(1,j) w_out_A(2,j) w_out_A(3,j) r_out_A(1,j) r_out_A(2,j) r_out_A(3,j)];
end
%-----------------------------------------------------------------------------------------------------------------------
%-----------------------------------------------------------------------------------------------------------------------
function y=spline_intergral(tn,x)
% 利用样条函数进行数值积分
% y=spline_intergral(tn,x)
% tn--时间序列
% x--被积数组
% y--积分返回数组
pp=spline(tn,x); % 根据采样点数据(tn,x)，获得逐段多项式样条函数数据pp
integral_pp=fnint(pp); % 求样条函数pp的数值不定积分integral_pp
y=ppval(integral_pp,tn); % 根据样条函数integral_pp，计算tn向量对应的积分值向量
%-----------------------------------------------------------------------------------------------------------------------
%-----------------------------------------------------------------------------------------------------------------------
function yy=trapezia_intergral(tn,xx)
% 数组的梯形积分程序
% yy=trapezia_intergral(tn,xx)
% s=1/2*(x2-x1)*[f(x2)+f(x1)]
% tn--x1,x2……序列
% xx--f(x1),f(x2)……序列
% yy--积分值序列
n=length(tn); % 数组长度
% 梯形积分
s(1)=0.0;
yy(1)=0.0;
for i=2:n
s(i)=1.0/2.0*(xx(i)+xx(i-1))*(tn(i)-tn(i-1));
yy(i)=yy(i-1)+s(i);
end
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -