chapter5attitude.zip_satelliteattitude_卫星轨道控制_姿态控制_指向误差_误差三轴

function [b_eci,b_ecef,b_ned,ti,bh,dec,dip]=mag_field(r,yr,mth,day,hr,min,sec,mln,dt,type); % [b_eci,b_ecef,b_ned,ti,bh,dec,dip]=mag_field(r,yr,mth,day,hr,min,sec,mln,dt,type); % % This program computes the reference magnetic field using the WMM model. % % The inputs are: % r = position vector (see type below) % yr = year (e.g. 2010) % mth = month % day = day % hr = hour (0-23) % min = minute (0-59) % sec = second (0-59) % mln = field order (up to 12) % dt = sampling interval in seconds (will be used for m > 1) % type = 1 for r in ECI coordinates in km % = 2 for r in ECEF coordinates in km % = 3 for r in [lat long height] in [deg deg meter] % % The outputs are: % b_eci = magnetic field in ECI coordinates in nT (m x 3) % b_ecef = magnetic field in ECEF coordinates in nT (m x 3) % b_ned = magnetic field in NED coordinates in nT (m x 3) % ti = total field intensity in nT (m x 1) % bh = horizontal field intensity in nT (m x 1) % dec = declination in deg (m x 1) % dip = inclination in deg (m x 1) % John L. Crassidis 5/28/06 % Note: this program was converted from FORTRAN code provided by the % National Geophysical Data Center. See http://www.ngdc.noaa.gov/seg/WMM/. % Epoch epoch=2010.00; % Load Data for 2010 wmm=wmm2010_data; % Constants for Degrees/Radians Conversions dtr=pi/180; rtd=180/pi; % Get Length of Data [mout,mout3]=size(r); if mout3~=3, disp('r must be (m x 3)');end % Initialize Field Vectors bt_store=zeros(mout,1);br_store=zeros(mout,1);bp_store=zeros(mout,1); t=[0:dt:(mout-1)*dt]'; % Convert from ECI to Lat, Long and Height if Needed if type == 1 r_ecef=eci2ecef(r,yr,mth,day,hr,min,sec+t); [lat,long,height]=ecef2llh(r_ecef);height=height/1000; end % Convert from ECEF to Lat, Long and Height if Needed if type == 2 [lat,long,height]=ecef2llh(r);height=height/1000; end % Get Lat, Long and Height if type == 3 lat=r(:,1); long=r(:,2); height=r(:,3)/1000; end % Convert to Radians rlon=long*dtr; rlat=lat*dtr; % Get Sidereal Time and Ascending Node theta=sidereal(yr,mth,day,hr,min,sec+t)*dtr; alpha=rlon+theta; c_alpha=cos(alpha);s_alpha=sin(alpha); % Leap Year if (mod(yr,400)&&~mod(yr,100)) % leapyear = false; ndays = 365; elseif ~mod(yr,4) % leapyear = true; ndays = 366; else % leapyear = false; ndays = 365; end % Get Days Past First of Year o1=ones(mout,1); day_of_year = datenum(yr*o1,mth*o1,day*o1,hr*o1,min*o1,sec+t)-datenum(yr,1,1); % Find Days That Go Into Next Year i_days=find(day_of_year > ndays); % Get Decimal Year Time if isempty(i_days) == 1 time = yr + day_of_year/ndays; else % Check Leap Year of New Year time = yr + day_of_year/ndays; yr_new=floor(time(i_days(1))); if (mod(yr_new,400)&&~mod(yr_new,100)) % leapyear = false; ndays_new = 365; elseif ~mod(yr_new,4) % leapyear = true; ndays_new = 366; else % leapyear = false; ndays_new = 365; end time=zeros(mout,1); time(1:i_days(1)-1) = yr + day_of_year(1:i_days(1)-1)/ndays; time(i_days(1):mout) = yr + (day_of_year(i_days:mout)-ndays+ndays_new)/ndays_new; end % Get Spherical Harmonic Coefficients from Data c=zeros(13,13);cd=zeros(13,13); for i=1:length(wmm) n=wmm(i,1); m=wmm(i,2); if (m <= n) c(n+1,m+1)=wmm(i,3); cd(n+1,m+1)=wmm(i,5); end if (m ~= 0) c(m,n+1)=wmm(i,4); cd(m,n+1)=wmm(i,6); end end % Convert Schmidt Normalized Gauss Coefficients to Unnormalized snorm=zeros(mln+1,mln+1); k=zeros(mln+1,mln+1); fn=zeros(mln+1,1); fm=zeros(mln+1,1); snorm(1,1)=1; for n=2:mln+1 snorm(n,1)=snorm(n-1,1)*(2*(n-1)-1)/(n-1); j=2; for m=1:n k(n,m)=((n-2)^2-(m-1)^2)/((2*(n-1)-1)*(2*(n-1)-3)); if m > 1 flnmj=((n-m+1)*j)/(n+m-2); snorm(n,m)=snorm(n,m-1)*sqrt(flnmj); j=1; c(m-1,n)=snorm(n,m)*c(m-1,n); cd(m-1,n)=snorm(n,m)*cd(m-1,n); end c(n,m)=snorm(n,m)*c(n,m); cd(n,m)=snorm(n,m)*cd(n,m); end fn(n)=n; fm(n)=n-1; end k(2,2)=0; % Constant for WGS-84 a=6378.137; b=6356.7523142; re=6371.2; a2=a^2; b2=b^2; c2=a2-b2; a4=a2^2; b4=b2^2; c4=a4-b4; % Sine and Cosine of Lat and Long srlon=sin(rlon); srlat=sin(rlat); crlon=cos(rlon); crlat=cos(rlat); srlon2=srlon.^2; srlat2=srlat.^2; crlon2=crlon.^2; crlat2=crlat.^2; % Convert from Geodetic to Spherical Coordinates q=(a2-c2*srlat2).^(0.5); q1=height.*q; q2=((q1+a2)./(q1+b2)).^2; ct=srlat./((q2.*crlat2+srlat2).^(0.5)); st=(1-ct.^2).^(0.5); r2=height.^2+2*q1+(a4-c4*srlat2)./(q.^2); r=(r2).^(0.5); d=(a2*crlat2+b2*srlat2).^(0.5); ca=(height+d)./r; sa=c2*crlat.*srlat./(r.*d); % Main Loop for i = 1:mout % Update Time dt_change=time(i)-epoch; % Initialize Quantities sp=zeros(13,1); cp=zeros(13,1); p=zeros(13,13); dp=zeros(13,13); pp=zeros(13,1); p(1,1)=1; pp(1)=1; dp(1,1)=0; sp(1)=0; cp(1)=1; sp(2)=srlon(i); cp(2)=crlon(i); for m=3:mln+1; sp(m)=sp(2)*cp(m-1)+cp(2)*sp(m-1); cp(m)=cp(2)*cp(m-1)-sp(2)*sp(m-1); end aor=re/r(i); ar=aor^2; br=0; bt=0; bp=0; bpp=0; tc=zeros(mln+1,mln+1); % Loop for n and m for n=2:mln+1 ar=ar*aor; for m=1:n % Compute Unnormalized Associated Legendre Polynomials and Derivatives if (n == m) p(n,m)=st(i)*p(n-1,m-1); dp(n,m)=st(i)*dp(n-1,m-1)+ct(i)*p(n-1,m-1); elseif (n == 2) & (m == 1) p(n,m)=ct(i)*p(n-1,m); dp(n,m)=ct(i)*dp(n-1,m)-st(i)*p(n-1,m); elseif (n > 2) & (n ~= m) if (m > n-2) p(n-2,m)=0; dp(n-2,m)=0; end p(n,m)=ct(i)*p(n-1,m)-k(n,m)*p(n-2,m); dp(n,m)=ct(i)*dp(n-1,m)-st(i)*p(n-1,m)-k(n,m)*dp(n-2,m); end % Time Adjust the Gauss Coefficients tc(n,m)=c(n,m)+dt_change*cd(n,m); if (m ~= 1) tc(m-1,n)=c(m-1,n)+dt_change*cd(m-1,n); end % Accumulate Terms of the Spherical Harmonic Expansions par=ar*p(n,m); if (m == 1) temp1=tc(n,m)*cp(m); temp2=tc(n,m)*sp(m); else temp1=tc(n,m)*cp(m)+tc(m-1,n)*sp(m); temp2=tc(n,m)*sp(m)-tc(m-1,n)*cp(m); end bt=bt-ar*temp1*dp(n,m); bp=bp+fm(m)*temp2*par; br=br+fn(n)*temp1*par; % Special Case: North/South Geographic Poles if (st(i) == 0) & (m == 2) if (n == 2) pp(n)=pp(n-1); else pp(n)=ct(i)*pp(n-1)-k(n,m)*pp(n-2); end parp=ar*pp(n); bpp=bpp+fm(m)*temp2*parp; end end end if (st(i) == 0) bp=bpp; else bp=bp/st(i); end % Store Field Variables bt_store(i)=bt; br_store(i)=br; bp_store(i)=bp; end % Rotate from Spherical to Geodetic Coordinates b_ned=[-bt_store.*ca-br_store.*sa bp_store bt_store.*sa-br_store.*ca]; % Get ECI Coordinates brbt=(br_store.*st+bt_store.*ct); b_eci=[brbt.*c_alpha-bp_store.*s_alpha brbt.*s_alpha+bp_store.*c_alpha br_store.*ct-bt_store.*st]; % Horizontal, Total Field, Declination and Dip bh=(b_ned(:,1).^2+b_ned(:,2).^2).^(0.5); ti=(bh.^2+b_ned(:,3).^2).^(0.5); dec=atan2(b_ned(:,2),b_ned(:,1))*rtd; dip=atan2(b_ned(:,3),bh)*rtd; % Get ECEF Coordinates b_ecef=eci2ecef(b_eci,yr,mth,day,hr,min,sec+t); return function out=wmm2010_data out=[1 0 -29496.6 0.0 11.6 0.0 1 1 -1586.3 4944.4 16.5 -25.9 2 0 -2396.6 0.0 -12.1 0.0 2 1 3026.1 -2707.7 -4.4 -22.5 2 2 1668.6 -576.1 1.9 -11.8 3 0 1340.1 0.0 0.4 0.0 3 1 -2326.2 -160.2 -4.1 7.3 3 2 1231.9 251.9 -2.9 -3.9 3 3 634.0 -536.6 -7.7 -2.6 4 0 912.6 0.0 -1.8 0.0 4 1 808.9 286.4 2.3 1.1 4 2 166.7 -211.2 -8.7 2.7 4 3 -357.1 164.3 4.6 3.9 4 4 89.4 -309.1 -2.1 -0.8 5 0 -230.9 0.0 -1.0 0.0 5 1 357.2 44.6 0.6 0.4 5 2 200.3 188.9 -1.8