function [xp,xs,xf]=rtkdemo(t0,time,rref,obs,iobs,nav,inav,mode)
% RTKDEMO : real-time kinematic (RTK) positioning demo.
%
% This is an implementation of demonstration for the real-time kinematic (RTK)
% positioning algorithm with GPS/GNSS. In the algorithm, parameter adjustment
% is done by the Extended Kalman-Filter using double-differenced phase
% observables. Integer ambiguity is resolved by LAMBDA/MLAMBDA method.
% Cycle-slip/outlier detection is not implemented. The reference satellite is
% fixed to the highest elevation one at the first epoch.
%
%          Copyright (C) 2006 by T.TAKASU, All rights reserved.
%
% argin  : t0    = date number by datenum.m (days)
%          time  = estimation time vector relative to day t0 0:00 (sec)
%          rref  = reference station position [x;y;z] (ecef) (m)
%          obs,iobs = observation data/index(rover:1,ref.:2) (see readrnx.m)
%          nav,inav = navigation messages/index (see readrnx.m)
%          mode  = positioning mode (0:static,1:kinematic)
%
% argout : xp    = point-pos. solution (xp(n,:)=t(n) rover pos.) (ecef) (m)
%          xs    = float solution      (xs(n,:)=t(n) rover pos.) (ecef) (m)
%          xf    = fixed solution      (xf(n,:)=t(n) rover pos.) (ecef) (m)
%
% version: $Revision: 3 $ $Date: 06/01/29 11:20 $
% history: 2006/01/26 1.1 new

C=299792458; lam=C/1.57542E9; x(1:3+30,1)=nan; P=zeros(length(x));

for k=1:length(time)
    i=find(round(iobs(:,1))==time(k)); obsk=obs(i,:); iobsk=iobs(i,:);
    
    % single point positioning
    [rr,t,sat,dop]=pointp(t0,obsk,iobsk,nav,inav);
    if k==1, sref=sat; sats=1:31; sats(sat)=[]; end
    
    % temporal update of states
    [x,P]=udstate(sref,sats,x,P,rr(:,1),obsk,iobsk,mode,lam);
    
    % observables/measurement model
    y=lam*obs_dd(sref,sats,obsk,iobsk,1);
    [h,H,R]=measmodel(t0,t(1),t(2),x(1:3),rref,x(4:end),sref,sats,nav,inav,lam);
    
    % measurment update of states
    [x,P]=filt(x,P,y,h,H,R);
    
    % ambiguity resolution
    xf(k,:)=fixamb(x,P)'; xp(k,:)=rr(:,1)'; xs(k,:)=x';
    
    disp(sprintf('t=%5.0f: %12.3f %12.3f %12.3f : n=%d DOP=%.1f',time(k),...
         xf(k,1:3),sum(~isnan(h)),dop));
end

% single point positioning -----------------------------------------------------
function [rr,t,sat,dop]=pointp(t0,obs,iobs,nav,inav)
C=299792458; f1=1.57542E9; f2=1.2276E9;
for n=1:2
    i=find(iobs(:,3)==n); tr=iobs(i(1),1); sats=iobs(i,2);
    y=obs(i,3:4)*[f1^2;-f2^2]/(f1^2-f2^2); % ion-free pseudorange
    x=zeros(4,1); xk=ones(4,1);
    while norm(x-xk)>0.1
        [h,H,el]=prmodel(t0,tr,x(1:3),x(4)/C,sats,nav,inav);
        i=find(~isnan(y)&~isnan(h)); H=H(i,:);
        if length(i)<4, x(:)=nan; break, end
        xk=x; x=x+(H'*H)\H'*(y(i)-h(i));
    end
    rr(:,n)=x(1:3); t(n)=tr-x(4)/C; % t=tr-dtr
    if n==1, [e,i]=max(el); sat=sats(i); dop=sqrt(trace(inv(H'*H))); end
end

% pseudorange model (ionosphere-free) ------------------------------------------
function [h,H,el]=prmodel(t0,tr,rr,dtr,sats,nav,inav)
C=299792458;
for n=1:length(sats)
    [r,e,el(n),dts]=geodist(t0,tr-dtr,rr,nav(inav==sats(n),:));
    h(n,1)=r+C*(dtr-dts)+2.4/sin(el(n)); H(n,:)=[e',1];
end

% temporal update of states ----------------------------------------------------
function [x,P]=udstate(sref,sats,x,P,rr,obs,iobs,mode,lam)
F=ones(length(x),1); Q=zeros(length(x),1);
if mode|isnan(x(1)), x(1:3)=rr; F(1:3)=0; Q(1:3)=100; end
N=obs_dd(sref,sats,obs,iobs,1)-obs_dd(sref,sats,obs,iobs,3)/lam;
i=find(isnan(x(4:end))&~isnan(N)); x(3+i)=N(i); F(3+i)=0; Q(3+i)=10;
P=diag(F)*P*diag(F)'+diag(Q).^2;

% double difference of observables ---------------------------------------------
function y=obs_dd(sref,sats,obs,iobs,ch)
y11=obsdat(1,sref,obs,iobs,ch);
y21=obsdat(2,sref,obs,iobs,ch);
for n=1:length(sats)
    y12=obsdat(1,sats(n),obs,iobs,ch);
    y22=obsdat(2,sats(n),obs,iobs,ch);
    y(n,1)=(y11-y12)-(y21-y22);
end

function y=obsdat(rcv,sat,obs,iobs,ch)
i=find(iobs(:,2)==sat&iobs(:,3)==rcv);
if ~isempty(i), y=obs(i(1),ch); else y=nan; end

% double difference of phase model ---------------------------------------------
function [h,H,R]=measmodel(t0,t1,t2,rr1,rr2,N,sref,sats,nav,inav,lam)
[r11,e11]=geodist(t0,t1,rr1,nav(inav==sref,:));
[r21,e21]=geodist(t0,t2,rr2,nav(inav==sref,:));
for n=1:length(sats)
    [r12,e12]=geodist(t0,t1,rr1,nav(inav==sats(n),:));
    [r22,e22]=geodist(t0,t2,rr2,nav(inav==sats(n),:));
    h(n,1)=(r11-r12)-(r21-r22)+lam*N(n); H(n,[1:3,3+n])=[e11'-e12',lam];
end
R=2*(ones(length(h))+eye(length(h)))*0.003^2;

% measurement update of states -------------------------------------------------
function [x,P]=filt(x,P,y,h,H,R)
i=find(~isnan(y)&~isnan(h)); H=H(i,:);
K=P*H'/(H*P*H'+R(i,i));
x=x+K*(y(i)-h(i));
P=P-K*H*P;

% ambiguity resolution ---------------------------------------------------------
function x=fixamb(x,P);
i=1:3; j=4:length(x); j=j(~isnan(x(j))&diag(P(j,j))<10^2);
[N,s]=mlambda(x(j),P(j,j),2);
if isempty(N)|s(2)/s(1)<3, x(:)=nan; return, end  % ratio-test
x([i,j])=[x(i)-P(i,j)/P(j,j)*(x(j)-N(:,1));N(:,1)];

% geometric distance -----------------------------------------------------------
function [r,e,el,dt]=geodist(t0,t,rr,nav)
C=299792458; OMGE=7.292115167E-5; r=0; rk=1;
while abs(r-rk)>1E-4
    [rs,dt]=satpos(t0,t-r/C,nav);
    rrs=rr-Rz(OMGE*r/C)*rs; rk=r; r=norm(rrs);
end
e=rrs/r;
if norm(rr)>0, el=-asin(rr'*e/norm(rr)); else el=pi/2; end
if el*180/pi<15, r=nan; end % elevation cutoff

% navigation messeage to satellite position/clock bias -------------------------
function [rs,dt]=satpos(t0,t,nav)
if isempty(nav), rs=[nan;nan;nan]; dt=0; return, end
C=299792458; MU=3.986005E14; OMGE=7.292115167E-5;
tk=(t0-datenum(1980,1,6)-nav(:,28)*7)*86400+t-nav(:,18);
[tt,i]=min(abs(tk)); tk=tk(i); nav=nav(i,:);
A=nav(17)^2; e=nav(15); n=sqrt(MU/A^3)+nav(12); M=nav(13)+n*tk;
E=M; Ek=0; while abs(E-Ek)>1E-12, Ek=E; E=M+e*sin(Ek); end % solve kepler's eq.
p=atan2(sqrt(1-e^2)*sin(E),cos(E)-e)+nav(24);
r=[p;A*(1-e*cos(E));nav(22)+nav(26)*tk]+nav([16,14;11,23;21,19])*[sin(2*p);cos(2*p)];
OMG=nav(20)+(nav(25)-OMGE)*tk-OMGE*nav(18);
rs=r(2)*Rz(-OMG)*Rx(-r(3))*[cos(r(1));sin(r(1));0];
if nav(1)<70, y=2000; else y=1900; end
tc=(t0-datenum(nav(1)+y,nav(2),nav(3)))*86400+t-nav(4:6)*[3600;60;1];
dt=nav(7:9)*[1;tc;tc^2]-2*sqrt(MU*A)*e*sin(E)/C^2;

% coordinate rotation matrix ---------------------------------------------------
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];
