MATLAB [*] files from GNSS Aided Navigation and Tracking ============================================================================= PAGE 72: Lower triangular inverse square root of normalized covariance matrix C = zeros(7,7); C(1,1) = 1; K = 1; frac = 1; for m=2:7 K = K + m ; for j=1:m-1, C(m,j)=-1/sqrt(K); end frac = frac + 1 / K ; C(m,m) = sqrt (frac) ; end C L = inv(C), pause product = C*L cov = L*L' ============================================================================= PAGE 81: Validation of equivalence betweeen velocity estimate variance * formed from correlated sequential differences in position * obtained from an estimator containing the position state N = input('Enter size of [R]-matrix '); rnorm = zeros(N,N); % pre-allocate rinv = zeros(N,N); % pre-allocate rho = -0.5; for I=1:N-1, rnorm(I,I) = 1.; % conformance to (5.39) rnorm(I,I+1) = rho; % conformance to (5.39) rnorm(I+1,I) = rho; % conformance to (5.39) end rnorm(N,N) = 1., pause % 1 on diag, -1/2 sub/super-diag rinv = inv(rnorm); sumv = 0.; for I=1:N, for J=1:N, sumv = sumv + rinv(I,J); end end % sum of elements rsumv = 1. / sumv; % reciprocal=6/[N*(N+1)*(N+2)]: [ rsumv , 6 / ( N*(N+1)*(N+2) ) ] ============================================================================= PAGE 89: Conservative worst-case 1-sec change in ionospheric time shift clear E = 0.02:0.02:1.; dE = 0.0002; for j = 1:50 di(j) = 16 * E(j) / (E(j)^2+0.1)^1.5; di(j) = 100 * dE * di(j); DI(j)=16*E(j)/sin((E(j)^2+0.1)^.5)/... tan((E(j)^2+.1)^.5)/(E(j)^2+0.1)^.5; DI(j) = 100 * dE * DI(j); end clf, plot(E,di,E,DI) xlabel('Elev '); ylabel('Iono change'); ============================================================================= NOTE: Results from m-files on pages 89 and 90 are pessimistic; carrier phase changes over 1 second were well below 1 cm, even at horizon, in flight. ============================================================================= PAGE 90: Conservative worst-case 1-sec change in tropospheric delay clear hkm = 0.1; expf = exp(-0.13346*hkm); elev = 0.02:0.02:0.5; delev = 0.0002; angle = elev * 180./pi; for j = 1:25 dtropo(j)=100*2.4*expf*(1/(.026+sin(elev(j))) ... -1/(.026+sin(elev(j)-delev))); end clf, plot(angle,dtropo); title('1-SEC TROPO CHANGE (cm)'); xlabel('Elevation Angle (deg)'); ylabel('Tropo change'); ============================================================================= PAGE 94: Inverse square root of a tridiagonal partitioned matrix containing * positive correlation effects due to across-satellite differencing * negative correlation effects due to time differencing MS = input('Enter # of meas. sets'); M1 = input('Enter # of meas. each set'); R = zeros(MS*M1,MS*M1); gbl = zeros(MS*M1,MS*M1); % Bierman p.47 gblinv = zeros(MS*M1,MS*M1); wt = zeros(M1,MS*M1); irow = 0; for ms=1:MS, ri1(ms) = sqrt( (ms-1) / (ms+1) ) ; D(ms) = sqrt( 2*ms / (ms+1) ) ; for m1=1:M1, if m1>ms, D(m1)=sqrt(2*m1/(m1+1)); end irow = irow + 1 ; i = 1 + rem(irow-1,M1) ; wt(i,irow) = D(ms) * D(m1) ; if (irow>1), for j=1:irow-1, if (ms>1)&(j<=M1*(ms-1)), wt(i,j) = wt(i,j) * ri1(ms) ; else wt(i,j) = -wt(i,irow) / m1 ; end gblinv(irow,j) = wt(i,j) ; end end gblinv(irow,irow) = wt(i,irow) ; end end gblinv ============================================================================= PAGE 105: Geodetic latitude and altitude from a geocentric vector clear txy=sqrt(RECEF(1)^2+RECEF(2)^2); tz=RECEF(3); iter = 0; ratio = 1.; lat=atan(tz/((1.-eesq)*txy)); while abs(ratio) > 1.e-9; iter=iter+1; slat=sin(lat); clat=cos(lat); radic=sqrt(1.-eesq*slat^2); f = ae*eesq/radic - txy/clat + tz/slat; fprime = ae*eesq^2*slat*clat/radic^3 - ... txy*slat/clat^2 - tz*clat/slat^2; ratio=f/fprime; lat=lat-ratio; end rp=ae/radic; xy0=rp*cos(lat); z0=rp*(1.-eesq)*sin(lat); alt=sqrt((txy-xy0)^2+(tz-z0)^2); [ lat*180/pi alt ] ============================================================================= PAGE 130: QR matrix decomposition, "home-made" sA = S(:,1) ; vec = sA ; Q(:,1) = vec / norm(vec) ; sB = S(:,2) ; qA = Q(:,1) ; vec = sB - (sB'*qA)*qA ; Q(:,2) = vec / norm(vec) ; sC = S(:,3); qB = Q(:,2) ; vec = sC - (sC'*qA)*qA - (sC'*qB)*qB ; Q(:,3) = vec / norm(vec); qC = Q(:,3) ; temp = Q(2:4,1:3); Q(1,4) = -det(temp); temp = [ Q(1,1:3); Q(3:4,1:3) ]; Q(2,4) = det(temp); temp = [ Q(1:2,1:3); Q(4,1:3) ]; Q(3,4) = -det(temp); temp = Q(1:3,1:3); Q(4,4) = det(temp); R = zeros(4,3); R(1,1:3) = [ qA'*sA qA'*sB qA'*sC ] ; R(2,2:3) = [ qB'*sB qB'*sC ] ; R(3,3) = qC'*sC ; QR=[ Q R ]; [QM,RM]=qr(S); QRMAT=[ QM RM ], pause ============================================================================= PAGE 208: Cartesian state transition matrix for Keplerian orbits % TRANS.M Ro=R; Vo=V; ro=sqrt(R'*R); vo=sqrt(V'*V); do=R'*V; I123=[eye(3) zeros(3,3)]; I456=[zeros(3,3) eye(3)]; dela=2*a^2*[Ro'/ro^3,Vo'/u]; delroa = ... -(1/u)*[Ro'*vo^2/ro,Vo'*2*ro]; incrE=dE-dEprev; dEprev=dE; deldo=[Vo',Ro']; sine=sin(incrE); cose=cos(incrE); ems=incrE-sine; omc=1-cose; rm=a*omc+ro*cose+(g_squa/u)*do*sine; dele=(-1.5*(g_squa/a^2)*tau + omc*do/(2*g_squa))*dela ... - (g_squa/u)*omc*deldo; dele=(1/rm)*(dele - a*sine*delroa); phi123=(1-omc*a/ro)*I123 - Ro * ((a/ro)*sine*dele ... - omc*(a/ro)^2*delroa); phi123 = phi123 + (tau-(a^2/g_squa)*ems)*I456; phi123 = phi123 - Vo * (a^1.5*omc*dele ... + 1.5*sqa*ems*dela); delro = [Ro'/ro 0 0 0]; delrm = sqa*(do*cose*dele+sine*deldo) ... + do*sine/(2*sqa)*dela; delrm = delrm/squ + (a-ro)*sine*dele+omc*dela+cose*delro; phi456 = - g_squa*sine/(rm*ro)*I123; phi456 = phi456 ... - squ*Ro*(sqa*cose*dele+sine*dela/(2*sqa))/(rm*ro); phi456 = phi456 ... + squ*Ro*sqa*sine*(rm*delro+ro*delrm)/(rm^2*ro^2); phi456 = phi456 + (1-a*omc/rm)*I456; phi456 = phi456 - Vo*( (a*sine*dele+omc*dela)/rm ... - (a*omc/rm^2)*delrm ); phi = [ phi123 ; phi456 ]; % finis ============================================================================= PAGE 210: Lambert's law formation of a Keplerian orbit from * pre- and post-apogee positions and * time interval between them % LAMBERT.M u=3.986005e+14; % WGS84 value Ts=60*mins; % time between R1 and R2 dHAVE=Ts*1.0027379*2*pi/86400; % HourAngleVernalEquinox cose=cos(dHAVE); % used in Transf from... sine=sin(dHAVE); % Earth to Inertial : TIE=[ cose -sine 0 ; sine cose 0 ; 0 0 1 ]; R2=TIE*R2; DR=R2-R1; % 2nd loc; diff, (IJK)_I c=sqrt(DR'*DR); % magnitudes -- r1=sqrt(R1'*R1);r2=sqrt(R2'*R2); s=(r1+r2+c)/2; % [17], mid-p.4 theta=acos((R1'*R2)/(r1*r2)); % angle between R1,R2 g_q=sqrt(r1*r2)*cos(theta/2)/s % [17]-Eq(19), + O to pi g_ups=sqrt(8*u/s)*Ts/s % [17]-Eq(20) beta0=acos(1+(c-s)/(3*h1+Rp/2)); %-Eq(15) 1st approx beta=fzero('fbeta',beta0), alph=2*asin(sin(beta/2)/g_q) a=(r1+r2)/(2-cos(alph)-cos(beta)) errAB = abs(alph-acos(1-s/a))+abs(beta-acos(1+(c-s)/a)); if errAB > .0001 fprintf ('alph & beta %g', errAB), end n=sqrt(u/a^3); % Keplerian mean motion Esum=2*atan2((r2-r1)/a,(alph-beta-n*Ts)); E1=(Esum-(alph-beta))/2; % eccentric anomaly #1 E2=(Esum+(alph-beta))/2; % eccentric anomaly #2 if abs(E1) > pi, E1 = -2*pi*sign(E1) + E1; end % Prin. if abs(E2) > pi, E2 = -2*pi*sign(E2) + E2; end % value e=(a-r1)/(a*cos(E1)) % eccentricity e=(a-r2)/(a*cos(E2)) % eccentricity e=(alph-beta-n*Ts)/(2*sin((alph-beta)/2)*cos(Esum/2)) w=sqrt(1-e^2); pause % compare 3 values of e X=(vecros(R1,R2))';X=X/sqrt(X'*X); % normal to plane TIO=[R1/a R2/a X] * inv ... ([cos(E1)-e cos(E2)-e 0;w*sin(E1) w*sin(E2) 0;0 0 1]); V1=(TIO*[ -a*sin(E1) a*w*cos(E1) 0 ]'*sqrt(u/a)/r1)' sL=sin(lat1); cL=cos(lat1); sl=sin(lon1); cl=cos(lon1); TGLE=[-sL*cl,-sL*sl,cL; -sl cl 0 ; -cL*cl -cL*sl -sL ] VG = (TGLE*V1')' - 1.0027379*2*pi/86400*[ cL 0 -sL] ========================================================= function[C] = vecros(A,B) C(1) = A(2) * B(3) - A(3) * B(2) ; C(2) = A(3) * B(1) - A(1) * B(3) ; C(3) = A(1) * B(2) - A(2) * B(1) ; --------------------------------------------------------- function z = fbeta(b) global g_q g_ups frac = sin(b/2)/g_q; a = 2*asin(frac); z = g_ups*frac^3 - a + sin(a) + b - sin(b); ========================================================= A TAR archive file for the free-inertial coast program of Appendix II is provided from http://jameslfarrell.com/... wp-content/uploads/2010/10/coastjlf.tar. _________________________________________________________ * MATLAB is a registered tademark of TheMathWorks, Inc.