function testrtk
% test driver for RTKDEMO
%
%          Copyright (C) 2006 by T.TAKASU, All rights reserved.
%
% version: $Revision: 1 $ $Date: 06/01/29 6:53 $
% history: 2006/01/26 1.1 new

t0=datenum(2005,4,2);     % date number by datenum.m (days)
time=7200:30:14400;       % estimation time vector relative to t0 (sec)

% rinex observation data/navigation messages ({rover obs., ref. obs., navs})
files={'07590920.05o','30400920.05o','07590920.05n','30400920.05n'};

% ref. station/rover positions (ecef) (m)
rref=[-3.9782419579E+06; 3.3828402344E+06; 3.6499008531E+06];
rrov=[-3.9762191879E+06; 3.3823716025E+06; 3.6525111478E+06];

% positioning mode (0:static,1:kinematic)
mode=1;

% read RINEX observation data/navigation messages
[obs,iobs,nav,inav]=readrnx(t0,time(1),time(end),files);

% RTK demo
[xp,xs,xf]=rtkdemo(t0,time,rref,obs,iobs,nav,inav,mode);

% plot errors
plot_pos(t0,time,xp,rrov,rref,'SPP');
plot_pos(t0,time,xs,rrov,rref,'FLOAT');
plot_pos(t0,time,xf,rrov,rref,'FIX');

% plot errors ------------------------------------------------------------------
function plot_pos(t0,time,xs,rrov,rref,type)
[pos,E]=gpos(rrov);
err=E*(xs(:,1:3)'-repmat(rrov,1,size(xs,1)));
s=sprintf(' : BASELINE=%.1fkm RATIO=%.1f%%',norm(rrov-rref)/1E3,...
          sum(~isnan(xs(:,1)))/size(xs,1)*100);
if strcmp(type,'SPP'), s=''; yl=10; else yl=0.2; end
figure, hold on, grid on
plot(time,err','.-'); legend({'East (m)','North (m)','Up (m)'});
xlim([time(1),time(end)]); ylim([-yl,yl]);
xlabel('Time (sec)'); ylabel('Position Error (m)');
title(['RTK POSITION ERROR (',type,')',s]);
for n=1:3, rms(n)=sqrt(mean(err(n,~isnan(err(n,:))).^2)); end
text(time(1),yl*0.92,sprintf('  RMSE : E:%7.4fm N:%7.4fm U:%7.4fm',rms));

% ecef to wgs84 / rotation matrix ----------------------------------------------
function [pos,E]=gpos(r)
RE=6378137; F=1/298.257223563;
e2=F*(2-F); r2=norm(r(1:2)); z=r(3); zk=0; n=0;
while abs(z-zk)>1E-4
    sinp=z/sqrt(r2+z^2);
    n=RE/sqrt(1-e2*sinp^2);
    zk=z; z=r(3)+n*e2*sinp;
end
pos=[atan(z/r2),atan2(r(2),r(1)),sqrt(r2+z^2)-n];
E=Rx(-pi/2-pos(1))*Rz(pi/2+pos(2));
pos(1:2)=pos(1:2)*180/pi;

function R=Rx(t), R=[1,0,0;0,cos(t),sin(t);0,-sin(t),cos(t)];
function R=Rz(t), R=[cos(t),sin(t),0;-sin(t),cos(t),0;0,0,1];
