function testrtk1
% RTK TEST 1 : Ambiguity fixing = ROUND : 2005/12/07 by TTAKA

t0=caltomjd([2004,10,1]); t=0:30:3600;

% satellites,stations (sats{1}/rcvs{1}:reference satellite/station)
sats={'GPS05','GPS09','GPS15','GPS18','GPS22','GPS26','GPS29','GPS30'};
rcvs={'92110','960627'};

mode=1; % 0:static,1:kinematic
ch  =1; % 1:L1,2:L2

% read observation data (rs{n}=[t,pos,vel,cdt;...])
[obs,iz,p,e,a,r,rs]=readobs(t0,t,sats,rcvs,'','obsc');

% reference station position
rref=readpos(t0,t(1),rcvs,'k:\gps\pos','gsipos');

% initialize states
[x,P]=initstate(t(1),rs{2}(rs{2}(:,1)==t(1),2:4)',sats,obs,iz,ch);

for k=1:length(t)
    if mode % kinematic mode
        x(1:3)=rs{2}(rs{2}(:,1)==t(k),2:4)'; % initial position
        T=diag([0;0;0;ones(length(x)-3,1)]);
        P=T*P*T+diag([100;100;100;zeros(length(x)-3,1)]);
    end
    % double-difference of observation data
    z=obs_dd(t(k),sats,obs,iz,ch);
    
    % receiver clock-bias
    C=299792458;
    dt1=rs{1}(rs{1}(:,1)==t(k),8)/C;
    dt2=rs{2}(rs{2}(:,1)==t(k),8)/C;
    
    % measurement model
    [h,H,R]=measmodel(t0,t(k),rref(:,:,1)',x(1:3),dt1,dt2,x(4:end),sats,ch);
    
    % measurement update of kalman filter
    K=P*H'/(H*P*H'+R);
    x=x+K*(z-h);
    P=P-K*H*P;
    
    xs(k,:)=x';           % float solution
    xf(k,:)=fixamb(x,P)'; % fix solution
    
    disp([sprintf('%5.0f: ',t(k)),sprintf('%11.3f ',xf(k,1:3))])
end
% plot positioning error
[e,E]=geodtoecef(eceftogeod(rref(:,:,2)'));
for k=1:size(xs)
    errs(k,:)=(xs(k,1:3)-rref(:,:,2))*E';
    errf(k,:)=(xf(k,1:3)-rref(:,:,2))*E';
end
gut('newfigg','','',[600,400]);
yl={'E(m)','N(m)','U(m)'};
for n=1:3
    if n<3, topts='nolabel'; else topts=''; end
    ggt('subplotv','',n,3,'taxis',t0,'xlim',[t(1),t(end)]/3600,'ylim',[-0.1,0.1],...
        'topts',topts,'margin',[0.08,0.03,0.04,0.012]); 
    plot(t/3600,errs(:,n),'r-'); plot(t/3600,errf(:,n),'.-'); ylabel(yl{n});
    if n==1, title(['RTK Positioning Error : ',rcvs{2}]), end
end

% initialize states ------------------------------------------------------------
function [x,P]=initstate(t,r0,sats,obs,iz,ch)
x=r0; sig=[10;10;10]; % initial std. deviations (m)
[z,N11]=obsdat(t,1,1,obs,iz,ch);
[z,N21]=obsdat(t,1,2,obs,iz,ch);
for n=1:length(sats)-1
    [z,N12]=obsdat(t,n+1,1,obs,iz,ch);
    [z,N22]=obsdat(t,n+1,2,obs,iz,ch);
    x=[x;dd(N11,N12,N21,N22)];
    sig=[sig;10];
end
P=diag(sig.^2);

% double difference of observables ---------------------------------------------
function z=obs_dd(t,sats,obs,iz,ch)
z11=obsdat(t,1,1,obs,iz,ch);
z21=obsdat(t,1,2,obs,iz,ch);
for n=1:length(sats)-1
    z12=obsdat(t,n+1,1,obs,iz,ch);
    z22=obsdat(t,n+1,2,obs,iz,ch);
    z(n,:)=dd(z11,z12,z21,z22);
end

% fix ambiguity ----------------------------------------------------------------
function x=fixamb(x,P)
N=round(x(4:end)); % ROUND method
x=[x(1:3)-P(1:3,4:end)/P(4:end,4:end)*(x(4:end)-N);N];

% retrieve observation data ----------------------------------------------------
function [z,N]=obsdat(t,sat,rcv,obs,iz,ch)
C=299792458; f=[1.57542E9,1.2276E9];
i=find(round(iz(:,1))==t&iz(:,2)==sat&iz(:,3)==rcv);
z=C/f(ch)*obs(i,ch);             % phase observable (m)
N=obs(i,ch)-obs(i,ch+2)*f(ch)/C; % approx. ambiguity (cycle)

% measurement model ------------------------------------------------------------
function [h,H,R]=measmodel(t0,t,rr1,rr2,dt1,dt2,N,sats,ch)
C=299792458; f=[1.57542E9,1.2276E9];
sig=0.003; % measurment noise
[r11,dr11]=geodist(t0,t-dt1,rr1,sats{1});
[r21,dr21]=geodist(t0,t-dt2,rr2,sats{1});
for n=1:length(sats)-1
    [r12,dr12]=geodist(t0,t-dt1,rr1,sats{n+1});
    [r22,dr22]=geodist(t0,t-dt2,rr2,sats{n+1});
    h(n,:)=dd(r11,r12,r21,r22)+C/f(ch)*N(n);
    H(n,1:3)=dr21-dr22; H(n,n+3)=C/f(ch);
end
R=2*(ones(n)+diag(ones(n,1)))*sig^2;

% geometric distance -----------------------------------------------------------
function [r,dr]=geodist(t0,t,rr,sat)
C=299792458; OMGE=0.7292115E-4; tau=0;
while 1 % solve light-time equation
    rr_rs=rr-Rz(OMGE*tau)*satpos(t0,t-tau,sat);
    r=norm(rr_rs);
    if abs(r-tau*C)<1E-4, break, else tau=r/C; end
end
dr=rr_rs'/r; % partial derivative

% satellite postion ------------------------------------------------------------
function rs=satpos(t0,t,sat)
rs=readeph(t0,t,sat,'k:\gps\nav\%Y%m','brdc')'; % broadcast ephemeris
rs=rs(1:3);

% rotation matrix around z axis ------------------------------------------------
function R=Rz(t), R=[cos(t),sin(t),0;-sin(t),cos(t),0;0,0,1];

% double difference  -----------------------------------------------------------
function d=dd(r11,r12,r21,r22), d=(r21-r22)-(r11-r12);
