ZhaiShuaiShuai/UWBIns
1
0
Fork 0
forked from ZhanLi/UWBIns
Code Pull Requests Activity
UWBIns/lib/gnss/ch_sat_pos.m

78 lines
1.9 KiB
Mathematica
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)
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
% toe: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ο<EFBFBD>ʱ<EFBFBD>䣺 һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD>Ϊtoeǰ<EFBFBD><EFBFBD>4Сʱ
% a0 a1 a2 toc: <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>У<EFBFBD><EFBFBD>ģ<EFBFBD>ͷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> toc: <EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD>ݿ<EFBFBD><EFBFBD>ο<EFBFBD>ʱ<EFBFBD><EFBFBD>, <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>У<EFBFBD><EFBFBD>ģ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>ο<EFBFBD><EFBFBD><EFBFBD>
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(16<EFBFBD><EFBFBD>):
% sqrtA: <EFBFBD><EFBFBD><EFBFBD>ǹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% es: <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% i0: toeʱ<EFBFBD>̹<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% OMEGA0: <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>0ʱ<EFBFBD>̵Ĺ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ྭ
% omega: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؽǾ<EFBFBD>
% M0: toeʱ<EFBFBD>̵<EFBFBD>ƽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% Delta_n<EFBFBD><EFBFBD> ƽ<EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>У<EFBFBD><EFBFBD>ֵ
% iDOT<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>ı仯<EFBFBD><EFBFBD>
% OMEGA_DOT<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ྭ<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>ı仯<EFBFBD><EFBFBD>
% Cuc: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Һ<EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% Cus: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Һ<EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% Cic: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Һ<EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% Cis: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Һ<EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
%<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
% X, Y, Z ECEF<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>
% sv_dt<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>ƫ<EFBFBD><EFBFBD>
%<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
%<EFBFBD>㶯<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>uk<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD>rk<EFBFBD>͹<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ik
%<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>е<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>x,y
%<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD>L
%<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
%1.<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>ƽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
n0=sqrt(3.986005E+14)/(sqrtA.^3);
n=n0+deln;
%2.<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŷ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>ƽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
sv_dt = f0+f1*(t-toc)+f2*(t-toc).^2;%tΪδ<EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĺ۲<EFBFBD>ʱ<EFBFBD><EFBFBD>
t = t - 0;%<EFBFBD><EFBFBD><EFBFBD><EFBFBD>tΪ<EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
tk = t-toe;%<EFBFBD>黯ʱ<EFBFBD><EFBFBD>
if tk>302400
tk=tk-604800;
elseif tk<-302400
tk=tk+604800;
else
tk=tk+0;
end
Mk=M0+n*tk;
%3.<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD><EFBFBD><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.<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Vk=atan2(sqrt(1-e.^2)*sin(Ek),(cos(Ek)-e));
%5.<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD>δ<EFBFBD><EFBFBD><EFBFBD>Ľ<EFBFBD>ʱ<EFBFBD><EFBFBD>
u=omg+Vk;
%6.<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㶯<EFBFBD><EFBFBD><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.<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㶯<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>uk<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD>rk<EFBFBD>͹<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ik
uk=u+deltau;
rk=(sqrtA.^2)*(1-e*cos(Ek))+deltar;
ik=i0+deltai+idot*tk;
%8.<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>е<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
x=rk*cos(uk);
y=rk*sin(uk);
%9.<EFBFBD><EFBFBD><EFBFBD>㷢<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD>67
L=OMG0+(OMGd-7.2921151467e-5)*tk-7.2921151467e-5*toe;
%10.<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵع<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD><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
Copy Permalink