EASY6.M
上传用户:sfyaiting
上传日期:2009-10-25
资源大小:320k
文件大小:10k
源码类别:

GPS编程

开发平台:

Matlab

  1. %EASY6 We compute a baseline from C/A code and phase observations.
  2. %     The code does not handle
  3. %        1. cycle slips and
  4. %        2. outliers.
  5. %     In contrast to EASY5, we now use an extended Kalman filter for the
  6. %     estimation. The present code is no real RTK code as all computational
  7. %     steps do not happen on an epoch-by-epoch basis
  8. %Kai Borre 27-07-2002
  9. %Copyright (c) by Kai Borre
  10. %$Revision: 1.0 $  $Date: 2002/07/27  $
  11. % Initial computations of constants
  12. v_light = 299792458;      % vacuum speed of light m/s
  13. f1 = 154*10.23E6;      % L1 frequency Hz
  14. f2 = 120*10.23E6;  % L2 frequency Hz
  15. lambda1 = v_light/f1;      % wavelength on L1:  .19029367  m
  16. lambda2 = v_light/f2;      % wavelength on L2:  .244210213 m
  17. % Read RINEX ephemerides file and convert to internal Matlab format
  18. rinexe('SITE247J.01N','eph.dat');
  19. Eph = get_eph('eph.dat');
  20. % We identify the master observation file and open it
  21. ofile1 = 'SITE247J.01O';
  22. fid1 = fopen(ofile1,'rt');
  23. [Obs_types1, ant_delta1, ifound_types1, eof11] = anheader(ofile1);
  24. NoObs_types1 = size(Obs_types1,2)/2;
  25. % We start by estimating the master position
  26. [time1, dt1, sats1, eof1] = fepoch_0(fid1);
  27. NoSv1 = size(sats1, 1);
  28. m = NoSv1;
  29. obs1raw = grabdata(fid1, NoSv1, NoObs_types1);
  30. i = fobs_typ(Obs_types1,'C1'); % We use C/A pseudoranges
  31. [X_i, el] = recpo_ls(obs1raw(:,i), sats1, time1, Eph);
  32. [phi_i,lambda_i,h_i] = togeod(6378137,298.257223563,X_i(1),X_i(2),X_i(3));
  33. % We close all files to ensure that the next reading starts
  34. % at the top of the observation files
  35. fclose all;
  36. % Finding columns in Eph for each SV
  37. for t = 1:m
  38.     col_Eph(t) = find_eph(Eph,sats1(t),time1);
  39. end
  40. % Computation of elevation angle to all SVs.
  41. all_sats1 = sats1;
  42. % Delete Sv with elevation smaller than 10 degrees
  43. sats1(el<10) = [];
  44. del_sat = setdiff(all_sats1,sats1);
  45. no_del_sat = [];
  46. for t = 1:length(del_sat)
  47.     no_dels = find(del_sat(t) == all_sats1);
  48.     no_del_sat = [no_del_sat; no_dels];
  49. end
  50. No_del_sat = length(no_del_sat);
  51. % Selecting reference SV. We take the SV with largest elevation
  52. [y,ind] = max(el);
  53. refsv = sats1(ind);
  54. ofile1 = 'SITE247J.01O';
  55. fid1 = fopen(ofile1,'rt');
  56. ofile2 = 'SITE24~1.01O';
  57. fid2 = fopen(ofile2,'rt');
  58. % We start reading both observation files
  59. [Obs_types1, ant_delta1, ifound_types1, eof11] = anheader(ofile1);
  60. NoObs_types1 = size(Obs_types1,2)/2;
  61. obsstr = ['P1';'P2';'L1';'L2'];
  62. Oc = [];
  63. for t = 1:4
  64.     oc = strmatch(obsstr(t,:),strvcat('C1','P1','P2','L1','L2'),'exact');
  65.     Oc = [Oc oc];
  66. end
  67. [Obs_types2, ant_delta2, ifound_types2, eof12] = anheader(ofile2);
  68. NoObs_types2 = size(Obs_types2,2)/2;
  69. % Computation of covariance matrix Sigma for double differenced observations
  70. m1 = m-No_del_sat; % original number of SVs - deleted SVs due to low elevations
  71. D = [ones(m1,1) -eye(m1) -ones(m1,1) eye(m1)];
  72. Sigma = D*D';
  73. X = zeros(3+2*m1,1);       % coord.diff., N1, N2
  74. N = zeros(3+2*m1,3+2*m1);     % initialization of normals
  75. rs = zeros(3+2*m1,1);       % initialization of right side
  76. X_a = [];
  77. X_j = X_i(1:3,1);
  78. refrow = find(refsv == sats1);
  79. % We process three epochs for estimating ambiguities; the present data evidently
  80. % need three or more epochs for getting reliable estimates of the float ambiguities
  81. for q = 1:6
  82.     X_j = X_i(1:3,1)+X(1:3,1);
  83.     [time1, dt1, sats1, eof1] = fepoch_0(fid1);
  84.     [time2, dt2, sats2, eof2] = fepoch_0(fid2);
  85.     if time1 ~= time2
  86. disp('Epochs do not correspond in time')
  87. break
  88.     end;
  89.     time = time1;
  90.     NoSv1 = size(sats1,1);
  91.     NoSv2 = size(sats2,1);
  92.     obsm = grabdata(fid1, NoSv1, NoObs_types1);
  93.     obsr = grabdata(fid2, NoSv2, NoObs_types2);
  94.     obs1 = obsm(:,Oc); % P1 P2 Phi1 Phi2
  95.     % Reordering of rows in obsr to correspond to obsm
  96.     for s = 1:NoSv1
  97. Ind = find(sats1(s) == sats2(:));
  98. obs2(s,:) = obsr(Ind,Oc);
  99.     end
  100.     % Computing rho for refsv
  101.     [tcorr,rhok_j,Xk_ECF] = get_rho(time, obs2(refrow,1), Eph(:,col_Eph(refrow)), X_j);
  102.     [tcorr,rhok_i,Xk_ECF] = get_rho(time, obs1(refrow,1), Eph(:,col_Eph(refrow)), X_i);
  103.     tt = 0;
  104.     A1 = [];
  105.     t0 = 1:NoSv1;
  106.     t1 = setdiff(t0,no_del_sat); % we delete the low satellites
  107.     for t = t1
  108. tt = tt+1;
  109. [tcorr,rhol_j,Xl_ECF] = get_rho(time,obs2(t,1), Eph(:,col_Eph(t)), X_j);
  110. [tcorr,rhol_i,Xl_ECF] = get_rho(time,obs1(t,1), Eph(:,col_Eph(t)), X_i);
  111. A0 = [(Xk_ECF(1)-X_j(1))/rhok_j - (Xl_ECF(1)-X_j(1))/rhol_j  ...
  112. (Xk_ECF(2)-X_j(2))/rhok_j - (Xl_ECF(2)-X_j(2))/rhol_j ...
  113. (Xk_ECF(3)-X_j(3))/rhok_j - (Xl_ECF(3)-X_j(3))/rhol_j];
  114. A1 = [A1; A0];
  115. Phi1 = (obs1(refrow,3)-obs1(t,3)-obs2(refrow,3)+obs2(t,3))*lambda1;
  116. Phi2 = (obs1(refrow,4)-obs1(t,4)-obs2(refrow,4)+obs2(t,4))*lambda2;
  117. b(tt,:) = Phi1-lambda1*X(3+tt,1);
  118. b(m1+tt,:) = Phi2-lambda2*X(3+m1+tt,1);
  119. bk(tt,:) =  rhok_i-rhok_j-rhol_i+rhol_j;
  120. bk(m1+tt,:) =  rhok_i-rhok_j-rhol_i+rhol_j;
  121.     end;
  122.     A_modi = eye(m1);  % modified coefficient matrix
  123.     col = find(refsv == sats1);  % find column for reference PRN
  124.     A_modi(:,col) = -ones(m1,1);
  125.     A_aug = [A1 lambda1*A_modi 0*eye(m1); A1 0*eye(m1) lambda2*A_modi];
  126.     N = N+A_aug'*kron(eye(2),Sigma)*A_aug;
  127.     rs = rs+A_aug'*kron(eye(2),Sigma)*(b-bk);
  128. end %q
  129. PP = pinv(N);
  130. % X contains the three preliminary baseline components and the float ambiguities
  131. X = PP*rs;
  132. % Estimation of ambiguities by means of the Lambda method
  133. [a,sqnorm,Sigma_afixed,Z] = lambda2(X(4:4+2*m1-1,1),PP(4:4+2*m1-1,4:4+2*m1-1));
  134. % Correcting baseline vector as consequence of changing float ambiguities to fixed ones
  135. X(1:3,1) = X(1:3,1)-PP(1:3,4:4+2*m1-1)*inv(PP(4:4+2*m1-1,4:4+2*m1-1))*...
  136.      (X(4:4+2*m1-1,1)-a(:,1)); %select first set of candidates
  137. X(4:4+2*m1-1,1) = a(:,1);
  138. fprintf('n N1 for PRN %3.0f: %3.0f',[sats1(t1)'; a(1:m1,1)'])
  139. fprintf('n')
  140. fprintf('n N2 for PRN %3.0f: %3.0f',[sats1(t1)'; a(m1+1:2*m1,1)'])
  141. % We close and reopen all files in order to start reading at a known position
  142. fclose all;
  143. ofile1 = 'SITE247J.01O';
  144. fid1 = fopen(ofile1,'rt');
  145. ofile2 = 'SITE24~1.01O';
  146. fid2 = fopen(ofile2,'rt');
  147. % Setting covariances for the Kalman filter; the state vector contains (x,y,z)
  148. P = eye(3);                       % covariances of state vector
  149. Q = 0.05^2*eye(3);              % covariances of system
  150. R = 0.005^2*kron(eye(2),inv(Sigma));  % covariances of observations
  151. % In ofile2 we substitute empty observations with NaN's to obtain 22 valid epochs
  152. qend = 22;
  153. % Preliminary estimate of baseline components
  154. x = X(1:3,1);
  155. x_acc = [];
  156. delta_x = zeros(3,1);
  157. for q = 1:qend
  158.     X_j = X_i(1:3,1)+x;
  159.     [phi_j,lambda_j,h_j] = togeod(6378137,298.257223563,X_j(1),X_j(2),X_j(3));
  160.     [time1, dt1, sats1, eof1] = fepoch_0(fid1);
  161.     [time2, dt2, sats2, eof2] = fepoch_0(fid2);
  162.     if time1 ~= time2
  163. disp('Epochs do not correspond in time')
  164. break
  165.     end;
  166.     time = time1;
  167.     NoSv1 = size(sats1,1);
  168.     NoSv2 = size(sats2,1);
  169.     obsm = grabdata(fid1, NoSv1, NoObs_types1);
  170.     obsr = grabdata(fid2, NoSv2, NoObs_types2);
  171.     obs1 = obsm(:,Oc); % P1 P2 Phi1 Phi2
  172.     % Reordering of rows in obsr to correspond to obsm
  173.     for s = 1:m
  174. Ind = find(sats1(s) == sats2(:));
  175. obs2(s,:) = obsr(Ind,Oc);
  176.     end
  177.     % Computing rho for refsv
  178.     [tcorr,rhok_j,Xk_ECF] = get_rho(time, obs2(1,1), Eph(:,col_Eph(1)), X_j);
  179.     [tcorr,rhok_i,Xk_ECF] = get_rho(time, obs1(1,1), Eph(:,col_Eph(1)), X_i);
  180.     tt = 0;
  181.     A = zeros(2*m1,3);
  182.     for t = t1
  183. tt = tt+1;
  184. [tcorr,rhol_j,Xl_ECF] = get_rho(time,obs2(t,1), Eph(:,col_Eph(t)), X_j);
  185. [tcorr,rhol_i,Xl_ECF] = get_rho(time,obs1(t,1), Eph(:,col_Eph(t)), X_i);
  186. A0 = [(Xk_ECF(1)-X_j(1))/rhok_j - (Xl_ECF(1)-X_j(1))/rhol_j  ...
  187. (Xk_ECF(2)-X_j(2))/rhok_j - (Xl_ECF(2)-X_j(2))/rhol_j ...
  188. (Xk_ECF(3)-X_j(3))/rhok_j - (Xl_ECF(3)-X_j(3))/rhol_j];
  189. A(tt,:) =  A0;
  190. A(m1+tt,:) = A0;
  191. % Tropospheric correction using standard meteorological parameters
  192. %[az,el_ki,d] = topocent(X_i(1:3),Xk_ECF-X_i(1:3));
  193. %[az,el_li,d] = topocent(X_i(1:3),Xl_ECF-X_i(1:3));
  194. %[az,el_kj,d] = topocent(X_j(1:3),Xk_ECF-X_j(1:3));
  195. %[az,el_lj,d] = topocent(X_j(1:3),Xl_ECF-X_j(1:3));
  196. %el_ki,    el_li,    el_kj,    el_lj
  197. %t_corr = tropo(sin(el_lj*pi/180),...
  198. %    h_j*1.e-3,1013,293,50,0,0,0)...
  199. %    -tropo(sin(el_li*pi/180),....
  200. %    h_i*1.e-3,1013,293,50,0,0,0)...
  201. %    -tropo(sin(el_kj*pi/180),...
  202. %    h_j*1.e-3,1013,293,50,0,0,0)...
  203. %    +tropo(sin(el_ki*pi/180),...
  204. %    h_i*1.e-3,1013,293,50,0,0,0);
  205. Phi1 = (obs1(refrow,3)-obs1(t,3)-obs2(refrow,3)+obs2(t,3))*lambda1; %-t_corr;
  206. Phi2 = (obs1(refrow,4)-obs1(t,4)-obs2(refrow,4)+obs2(t,4))*lambda2; %-t_corr;
  207. b(tt,:) = Phi1-lambda1*a(tt,1);
  208. b(m1+tt,:) = Phi2-lambda2*a(m1+tt,1);
  209. bk(tt,:) =  rhok_i-rhok_j-rhol_i+rhol_j;
  210. bk(m1+tt,:) =  rhok_i-rhok_j-rhol_i+rhol_j;
  211.     end; % t
  212.     %Extended Kalman filter, see pages 509--510 in Strang & Borre (1997): Linear
  213.     % Algebra, Geodesy, and GPS, Wellesley-Cambridge Press
  214.     P = P+Q;
  215.     K = P*A'*inv(A*P*A'+R);
  216.     x = x+K*(b-bk);
  217.     P = (eye(3)-K*A)*P;
  218.     fprintf('nx: %8.3f m,  y: %8.3f m,  z: %8.3f mn',x(1),x(2),x(3))
  219.     x_acc = [x_acc x];
  220. end %q
  221. % Transformation of geocentric baseline coordinates into topocentric coordinates
  222. for i = 1:qend
  223.     [e(i),n(i),u(i)] = xyz2enu(phi_j,lambda_j,x_acc(1,i),x_acc(2,i),x_acc(3,i));
  224. end
  225. fprintf('nnBaseline Componentsn')
  226. fprintf('nX: %8.3f m,  Y: %8.3f m,  Z: %8.3f mn', ...
  227.   x_acc(1,qend),x_acc(2,qend),x_acc(3,qend))
  228. fprintf('nE: %8.3f m,  N: %8.3f m,  U: %8.3f mn',mean(e),mean(n),mean(u))
  229. figure(1);
  230. plot_handles = plot(1:qend,(e-e(1))'*1000,'-',...
  231.                     1:qend,(n-n(1))'*1000,'--',...
  232.                     1:qend,(u-u(1))'*1000,'-.');
  233. set(gca,'fontsize',16)
  234. set(plot_handles,'linewidth',2)
  235. title('Estimates of Baseline Components using Kalman Filter')
  236. ylabel('State Vector, Changes Relative to Initial Epoch [mm]')
  237. xlabel('Epochs [1 s interval]')
  238. legend('Easting','Northing','Upping')
  239. print -deps easy6
  240. %%%%%%%%%%%%%%%%%%%%%% end easy6.m  %%%%%%%%%%%%%%%%%%%