Extended Kalman Filter for GPS PPP

function [Result,Residual,Relativity,Pminus,CLKphi,NumSV,W,Pkalman] ...
= EKF(DOY,rTime,Ph1,Ph2,Pr1,Pr2,SP3X,SP3Y,SP3Z,SP3clk,Vx,Vy,Vz,x,Elev,trop,type)

% Run Extend Kalman Filtering
%
% %%%%%%%%% HELP %%%%%%%%%%
%
% [Result,Residual,Relativity,Pminus,CLKphi,NumSV,W,Pkalman] ...
% = EKF(DOY,rTime,Ph1,Ph2,Pr1,Pr2,SP3X,SP3Y,SP3Z,SP3clk,Vx,Vy,Vz,x,Elev,trop,type)
%
% INPUT
%
% DOY = Day of year [1 x 1]
% rTime = GPS second of RINEX file [GPSsec], [numep x 1]
% Ph1 = Carrier phase L1,cycles [numep x numsat]
% Ph2 = Carrier phase L2,cycles [numep x numsat]
% Pr1 = Pseudorange C/A code, metres [numep x numsat]
% Pr2 = Pseudorange P code , metres [numep x numsat]
% SP3X, SP3Y, SP3Z = Satellite coordinate (WGS84), metres [numep x numsat]
% SP3clk = Precise clock correction, microseconds [numep x numsat]
% Vx = Satellite velocity [numep x numsat], m/s
% Vy = Satellite velocity [numep x numsat], m/s
% Vz = Satellite velocity [numep x numsat], m/s
% x = Aproximate station coordinates, metres [3 x 1]
% Elev = Elevation angle, radians [numep x numsat]
% trop = 1 Estimate tropospheric correction from EKF
% = 2 Approximate tropospheric correction from Saastamoinen model
% = 3 Approximate tropospheric correction from Saastamoinen model & Neill mapping function
% type = 1 Normal
% = 2 transition matrix (Phi) is identity
%
% OUTPUT
%
% Result = [x y z Ni dT Tropo PDOP TDOP GDOP]
% Residual
% Relativity
% Pminus
% CLKphi
% NumSV
% W
% Pkalman
%
% Modified by Phakphong Homniam
% December 25, 2002
% Orginal source code in Mathcad by Boonsap Witchayangkoon, 2000

%%%%%%%%%% CONSTANT DECLARATION %%%%%%%%%%

% Global constants of the relevant parameters in constant_global.m file
constant_global

global NumSat NumPara gamma maskangle

% Satellite mask angle
maskangle = 15*deg;

% Kalman initialization constants
SDx = 0.0003; % (m) used in Q (dynamic model)
SDclk = 30; % (m) usde in Q (dynamic model)
SDph = 0.02; % (m) STDS of carrier phase (R)
SDpr = 0.2; % (m) STD of pseudorange (R)
SDcoorinit = 1; % (m) STD of initial parameters (P)
SDambinit = 10^10; % (m) STD of initial ambiguities (P)
SDclkinit = 500; % (m) STD for initial clock (P)
SDtropinit = 0.1; % (m) STD for initial tropo (P)

% Specification when estimating troposphere
% Tropospheric Correlation Time
thor = 18000; % seconds

% Number of satellite
NumSat = size(Ph1,2);

gamma = (f1/f2)^2;
tep = length(rTime);
TimeInterval = rTime(3)-rTime(2);

%%%%%%%%%% BEGIN %%%%%%%%%%

Result = [];
Residual = [];
Relativity = [];
Pminus = [];
CLKphi = [];
NumSV = [];
W = [];
Pkalman = [];

% Estimate receiver clock error for the first epoch for EKF
for i = 1:NumSat
if (Ph1(1,i) ~= NA) & (Pr1(1,i) ~= NA)
if SP3clk(1,i) ~= NAJ
rho = sqrt((SP3X(1,i)-x(1))^2+(SP3Y(1,i)-x(2))^2 ...
+(SP3Z(1,i)-x(3))^2);
h = rho-c*SP3clk(1,i)*10^-6;
FirstClkEst = Pr1(1,i)-h;
break
end
end
end

switch trop
case 1
% Number of unknown parameters
% (x,y,z)+(ambiguities)+Receiver Clock+Tropo
NumPara = 3+NumSat+2;

% Set martix Xm
zenith = 2.3;
Xstart = [x(:);zeros(NumSat,1);FirstClkEst;zenith];
Xm = Xstart;
Xminus = Xm;

% Set matrix Q
qSD(1:3) = SDx;
qSD(NumSat+4) = SDclk;
qSD(NumSat+5) = 0.01/3600; % may be 0.1/60 or 0.01/3600 : i don't sure
Q = diag(qSD.^2);

% Set matrix Pminus
pmSD(1:3) = SDcoorinit;
pmSD(4:NumSat+3) = SDambinit;
pmSD(NumSat+4) = SDclkinit;
pmSD(NumSat+5) = exp(-TimeInterval/thor);
Pm = diag(pmSD.^2);
Pminus = Pm;

case {2,3}
% Number of unknown parameters
% (x,y,z)+(ambiguities)+Receiver Clock
NumPara = 3+NumSat+1;

% Set martix Xm
Xstart = [x(:);zeros(NumSat,1);FirstClkEst];
Xm = Xstart;
Xminus = Xm;

% Set matrix Q
qSD(1:3) = SDx;
qSD(NumSat+4) = SDclk;
Q = diag(qSD.^2);

% Set matrix Pminus
pmSD(1:3) = SDcoorinit;
pmSD(4:NumSat+3) = SDambinit;
pmSD(NumSat+4) = SDclkinit;
Pm = diag(pmSD.^2);
Pminus = Pm;
end

% Set matrix Phi
switch type
case 1
Phi = eye(NumPara);
% Set Phi for the receiver clock
% = 0 pure white noise
% = 1 random walk
Phi(NumSat+4,NumSat+4) = 0;
case 2
Phi = eye(NumPara);
end
[PR,Phase] = typeobs(Ph1,Ph2,Pr1,Pr2,tep);
ep = 1;

for i = 1:(tep-1)
phase = Phase(i,:);
pr = PR(i,:); prp1 = PR(i+1,:);
sp3x = SP3X(i,:); sp3xp1 = SP3X(i+1,:); vx = Vx(i,:);
sp3y = SP3Y(i,:); sp3yp1 = SP3Y(i+1,:); vy = Vy(i,:);
sp3z = SP3Z(i,:); sp3zp1 = SP3Z(i+1,:); vz = Vz(i,:);
sp3clk = SP3clk(i,:); sp3clkp1 = SP3clk(i+1,:); elev = Elev(i,:);

% set matrix H
[HhZ1,HhZ2,HhZ3,HhZ4] = MatrixHhZ(Xm,phase,pr,sp3x,sp3y,sp3z,sp3clk,elev,x,DOY,vx,vy,vz,trop);
H = HhZ1;

% Set matrix R
rSD(1:NumSat) = SDph;
rSD(NumSat+1:2*NumSat) = SDpr;
R = diag(rSD.^2); % Observation eorror
% Reweight observation at low elevation angles
% for j = 1:NumSat
% if (elev(j) ~= NA) & (elev(j) < 30*deg)
% if elev(j) < 25*deg
% lv = elev(j)+5*deg;
% else
% lv = elev(j);
% end
% R(j,j) = (SDph/sin(lv))^2;
% R(j+NumSat,j+NumSat) = (SDpr/sin(lv))^2;
% end
% end

K = Pm*H'*inv(H*Pm*H'+R); % Calculate Kalman Gain
w = K*HhZ2;
W = [W,w];
Xestimate = Xm+w; % Using measurement to update estimate
P = Pm-K*H*Pm; % Update the error covariance
Pkalman = [Pkalman;P];
% Phi clk here
Phi(NumSat+4,NumSat+4) ... % Xestimate or x???
= clkphiall(Xestimate,pr,sp3x,sp3y,sp3z,sp3clk,prp1,sp3xp1,sp3yp1,sp3zp1,sp3clkp1);
Xm = Phi*Xestimate; % Project the state ahead
Xminus = [Xminus,Xm];

switch type
case 1
Pm = PhiPPhiT(Phi,P,trop)+Q; % Project the covariance ahead
case 2
Pm = P+Q;
end

Pminus = [Pminus;Pm];
Result(1:NumPara,ep) = Xestimate;
A = H4DOPs(phase,pr,sp3x,sp3y,sp3z,elev,x);
ATA = A'*A;
detATA = det(ATA);
if detATA == 0
PDOP = NA;
TDOP = NA;
GDOP = NA;
else
Pvar = inv(ATA);
PDOP = sqrt(Pvar(1,1)+Pvar(2,2)+Pvar(3,3));
TDOP = sqrt(Pvar(4,4));
GDOP = sqrt(Pvar(1,1)+Pvar(2,2)+Pvar(3,3)+Pvar(4,4));
end
Result(NumPara+1,ep) = PDOP;
Result(NumPara+2,ep) = TDOP;
Result(NumPara+3,ep) = GDOP;
Residual = [Residual,HhZ2];
NumSV(ep) = HhZ3;
Relativity = [Relativity,HhZ4(:)];
CLKphi(ep) = Phi(NumSat+4,NumSat+4);
ep = ep+1;
end

%%%%%%%%%% RESULT %%%%%%%%%%

Result = Result';
Residual = Residual';
Relativity = Relativity';
Pminus;
CLKphi = CLKphi(:);
NumSV = NumSV(:);
W = W';
Pkalman;
%%%%%%%%%% END %%%%%%%%%%