ZhanLi
/
UWBIns
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
UWBIns/lib/gnss/ch_sat_pos.m

78 lines
1.9 KiB
Matlab
Raw Normal View History

first commit
2025-04-16 20:15:33 +08:00
function [XYZ, sv_dt]=ch_sat_pos(t, toc, f0, f1, f2, crs, deln, M0, cuc, e, cus, sqrtA, toe, cic, OMG0, cis, i0, crc, omg, OMGd, idot)
% <20><><EFBFBD><EFBFBD>:
% toe: <20><><EFBFBD><EFBFBD><EFBFBD>ο<EFBFBD>ʱ<EFBFBD>䣺 һ<><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>Ϊtoeǰ<65><C7B0>4Сʱ
% a0 a1 a2 toc: <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>У<EFBFBD><D0A3>ģ<EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> toc: <20><>һ<EFBFBD><D2BB><EFBFBD>ݿ<EFBFBD><DDBF>ο<EFBFBD>ʱ<EFBFBD><CAB1>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>У<EFBFBD><D0A3>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>ο<EFBFBD><CEBF><EFBFBD>
% <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(16<31><36>):
% sqrtA: <20><><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>
% es: <20><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>
% i0: toeʱ<65>̹<EFBFBD><CCB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% OMEGA0: <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>0ʱ<30>̵Ĺ<CCB5><C4B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ྭ
% omega: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؽǾ<D8BD>
% M0: toeʱ<65>̵<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% Delta_n<5F><6E> ƽ<><C6BD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ٶ<EFBFBD>У<EFBFBD><D0A3>ֵ
% iDOT<4F><54> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>ı仯<C4B1><E4BBAF>
% OMEGA_DOT<4F><54> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ྭ<EFBFBD><E0BEAD>ʱ<EFBFBD><CAB1><EFBFBD>ı仯<C4B1><E4BBAF>
% Cuc: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǿ<EFBFBD><C7BE><EFBFBD><EFBFBD>Һ<EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% Cus: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǿ<EFBFBD><C7BE><EFBFBD><EFBFBD>Һ<EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% Cic: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Һ<EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% Cis: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Һ<EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
%<25><><EFBFBD><EFBFBD>:
% X, Y, Z ECEF<45><46><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
% sv_dt<64><74> <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>ƫ<EFBFBD><C6AB>
%<25><><EFBFBD><EFBFBD>:
%<25>㶯<EFBFBD><E3B6AF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>uk<75><6B><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʸ<EFBFBD><CAB8>rk<72>͹<EFBFBD><CDB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ik
%<25><><EFBFBD><EFBFBD><EFBFBD>ڹ<EFBFBD><DAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>x,y
%<25><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>L
%<25><><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
%1.<2E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
n0=sqrt(3.986005E+14)/(sqrtA.^3);
n=n0+deln;
%2.<2E><><EFBFBD><EFBFBD><EFBFBD>źŷ<C5BA><C5B7><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>ǵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
sv_dt = f0+f1*(t-toc)+f2*(t-toc).^2;%tΪδ<CEAA><CEB4><EFBFBD>Ӳ<EFBFBD><D3B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĺ۲<C4B9>ʱ<EFBFBD><CAB1>
t = t - 0;%<25><><EFBFBD><EFBFBD>tΪ<74><CEAA><EFBFBD>Ӳ<EFBFBD><D3B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
tk = t-toe;%<25>黯ʱ<E9BBAF><CAB1>
if tk>302400
tk=tk-604800;
elseif tk<-302400
tk=tk+604800;
else
tk=tk+0;
end
Mk=M0+n*tk;
%3.<2E><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
%E=M+e*sin(E);
ed(1)=Mk;
for i=1:3
ed(i+1)=Mk+e*sin(ed(i));
end
Ek=ed(end);
%4.<2E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Vk=atan2(sqrt(1-e.^2)*sin(Ek),(cos(Ek)-e));
%5.<2E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD>δ<EFBFBD><CEB4><EFBFBD>Ľ<EFBFBD>ʱ<EFBFBD><CAB1>
u=omg+Vk;
%6.<2E><><EFBFBD><EFBFBD><EFBFBD>㶯<EFBFBD><E3B6AF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
deltau=cuc*cos(2*u)+cus*sin(2*u);
deltar=crc*cos(2*u)+crs*sin(2*u);
deltai=cic*cos(2*u)+cis*sin(2*u);
%7.<2E><><EFBFBD><EFBFBD><EFBFBD>㶯<EFBFBD><E3B6AF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>uk<75><6B><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʸ<EFBFBD><CAB8>rk<72>͹<EFBFBD><CDB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ik
uk=u+deltau;
rk=(sqrtA.^2)*(1-e*cos(Ek))+deltar;
ik=i0+deltai+idot*tk;
%8.<2E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڹ<EFBFBD><DAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>
x=rk*cos(uk);
y=rk*sin(uk);
%9.<2E><><EFBFBD>㷢<EFBFBD><E3B7A2>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>67
L=OMG0+(OMGd-7.2921151467e-5)*tk-7.2921151467e-5*toe;
%10.<2E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵع<DAB5><D8B9><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
XYZ(1)=x*cos(L)-y*cos(ik)*sin(L);
XYZ(2)=x*sin(L)+y*cos(ik)*cos(L);
XYZ(3)=y*sin(ik);
XYZ = XYZ';
end