fulllink-solver/GeometricSightTracking/SightPoint.m

function [Lon,Lat]=SightPoint(AOC_t0,AOC_fai,AOC_thita,AOC_pusai,AOC_a0,AOC_e0,AOC_i0,AOC_OMG0,AOC_os0,AOC_u0,Rp_b)
% AOC_t0：当前时刻（相对于J2000.0的秒计数）
% AOC_fai,AOC_thita,AOC_pusai：当前卫星三轴姿态角（3-1-2转序）
% AOC_a0,AOC_e0,AOC_i0,AOC_OMG0,AOC_os0,AOC_u0：当前时刻卫星轨道六根数
% Rp_b：本体下的视线指向矢量（单位矢量）
% Lon,Lat：视线指向星下点
%% 惯性系下的视线指向矢量计算
Aoi=GetAoi(AOC_i0,AOC_OMG0,AOC_u0);
Abo=GetAbo(AOC_fai,AOC_thita,AOC_pusai);
Abi=Abo*Aoi;
Aib=Abi.';
Rp_i=Aib*Rp_b; %惯性系下的视线指向矢量

%% 惯性系下的卫星矢量计算
Rs_i=Kepler2XYZ(AOC_a0,AOC_e0,AOC_i0,AOC_OMG0,AOC_os0,AOC_u0);

%% 地球扁率系数计算
eE=0.00335281066474748;
Ke=(1-eE)^2;

%% 视线指向与星下点矢量夹角
Thita=acos(dot(Rp_i,-Rs_i)/norm(Rs_i)/norm(Rp_i));
CT=cos(Thita);

%% 建立关于视线指向长度的方程
Rp3=Rp_i(3);
Rs3=Rs_i(3);
RE=6378.14;
Norm_Rs=norm(Rs_i);
Ka=Ke+(1-Ke)*Rp3^2;
Kb=2*(1-Ke)*Rp3*Rs3-2*Ke*Norm_Rs*CT;
Kc=Ke*Norm_Rs^2+(1-Ke)*Rs3^2-Ke*RE^2;

%% 视线指向矢量长度计算
Norm_Rp=(-Kb-(abs(Kb^2-4*Ka*Kc))^0.5)/2/Ka;
Rp_i=Norm_Rp*Rp_i;

%% 计算视线指向对应的经纬度信息
Rn_i=Rp_i+Rs_i; %地心到视线指向矢量与地面交点的矢量
Rn_i=Rn_i/norm(Rn_i);
[Lon,Lat]=Orbit2LLA(AOC_t0,Rn_i(1),Rn_i(2),Rn_i(3));



%% 轨道六根数转速度位置
function R_sat_i=Kepler2XYZ(AOC_a0,AOC_e0,AOC_i0,AOC_OMG0,AOC_os0,AOC_u0)
% 轨道倾角的正余弦值：
OBT_Si0=sin(AOC_i0);
OBT_Ci0=cos(AOC_i0);
% 升交点赤经的正余弦值：
OBT_SG0=sin(AOC_OMG0);
OBT_CG0=cos(AOC_OMG0);
% t0时刻真近点角：
OBT_f0=AOC_u0-AOC_os0;
% t0时刻偏近点角：
OBT_E0=atan2((1-AOC_e0^2)*sin(OBT_f0),AOC_e0+cos(OBT_f0));
% t0时刻平近点角：
OBT_M0=OBT_E0-AOC_e0*sin(OBT_E0);
% 当前时刻平近点角：
OBT_M=OBT_M0;
% OBT_M=OBT_M0+(Time-AOC_t0)*w0;
% 当前时刻真近点角：
AOC_f=OBT_M+(2*AOC_e0-AOC_e0^3/4)*sin(OBT_M)+5/4*AOC_e0^2*sin(2*OBT_M)+13/12*AOC_e0^3*sin(3*OBT_M);
% 卫星到地心距离：
OBT_rw=AOC_a0*(1-AOC_e0^2)/( 1+AOC_e0*cos(AOC_f));% ע<><D7A2><EFBFBD><EFBFBD>λkm
% 当前时刻轨道幅角：
u=AOC_f+AOC_os0;
Su=sin(u);
Cu=cos(u);
% 3）计算当前时刻惯性系下的太阳矢量
% 卫星方向单位矢量：
xw1=OBT_CG0*Cu-OBT_SG0*OBT_Ci0*Su;
yw1= OBT_SG0*Cu+OBT_CG0*OBT_Ci0*Su;
zw1=OBT_Si0*Su;
% 惯性坐标系下的卫星实际位置矢量SUN_R1_i（单位：km）：
R_sat_i=[xw1,yw1,zw1].'*OBT_rw;

%% 求解惯性系到轨道系转换矩阵
function Aoi=GetAoi(AOC_i0,AOC_OMG0,AOC_u0)
OBT_Si0=sin(AOC_i0);
OBT_Ci0=cos(AOC_i0);

OBT_SG0=sin(AOC_OMG0);
OBT_CG0=cos(AOC_OMG0);

OBT_Su0=sin(AOC_u0);
OBT_Cu0=cos(AOC_u0);
Aoi=[ -OBT_Su0*OBT_CG0-OBT_Cu0*OBT_Ci0*OBT_SG0,  -OBT_Su0*OBT_SG0+OBT_Cu0*OBT_Ci0*OBT_CG0,   OBT_Cu0*OBT_Si0;
                               -OBT_Si0*OBT_SG0,                          OBT_Si0*OBT_CG0,          -OBT_Ci0;
     -OBT_Cu0*OBT_CG0+OBT_Su0*OBT_Ci0*OBT_SG0,  -OBT_Cu0*OBT_SG0-OBT_Su0*OBT_Ci0*OBT_CG0,  -OBT_Su0*OBT_Si0];


 %% 求解轨道系到本体系转换矩阵
function Abo=GetAbo(AOC_fai,AOC_thita,AOC_pusai)
OBT_Sf=sin(AOC_fai);
OBT_Cf=cos(AOC_fai);

OBT_St=sin(AOC_thita);
OBT_Ct=cos(AOC_thita);

OBT_Sp=sin(AOC_pusai);
OBT_Cp=cos(AOC_pusai);

Abo=[OBT_Ct*OBT_Cp-OBT_St*OBT_Sf*OBT_Sp, OBT_Ct*OBT_Sp+OBT_St*OBT_Sf*OBT_Cp, -OBT_St*OBT_Cf;
                     -OBT_Cf* OBT_Sp,                      OBT_Cf* OBT_Cp,          OBT_Sf;
    OBT_St*OBT_Cp+OBT_Ct*OBT_Sf*OBT_Sp, OBT_St*OBT_Sp-OBT_Ct*OBT_Sf*OBT_Cp, OBT_Ct* OBT_Cf];


function [Lon,Lat]=Orbit2LLA(AOC_t0,AOC_xw1,AOC_yw1,AOC_zw1)
w0=7.29211585530487e-5;%地球自转角速度
% 惯性坐标系下的卫星实际位置矢量SUN_R1_i（单位：km）：
Tmp_Lat=asin(AOC_zw1)*180/pi;
Tmp_Lon=atan2(AOC_yw1,AOC_xw1)*180/pi;
% 格林威治恒星时角

Lon0 = rem(4.89496122373137+AOC_t0*w0,2*pi)*180/pi;% 格林威治恒星时角
Lon=Tmp_Lon-Lon0;% 星下点经度
if Lon<-180
    Lon=Lon+360;
elseif Lon>180
    Lon=Lon-360;
end
Lat=Tmp_Lat;% 星下点纬度