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.