```%Extended_KF   Extended Kalman Filter
%
% Version 1.0, May 16,2011
% Written by You Chong, Peking University
%
% Syntax:
%     [Xo,Po] = Extended_KF(f,g,Q,R,Z,X,P,Xstate)
%
% State Equation:
%     X(n+1) = f(X(n)) + w(n)
% Observation Equation:
%     Z(n) = g(X(n)) + v(n)
%     w ~ N(0,Q) is gaussian noise with covariance Q
%     v ~ N(0,R) is gaussian noise with covariance R
% Input:
%     f: function for state transition, type of symbolic expression
%     g: function for measurement, type of symbolic expression
%     Q: process noise covariance
%     R: measurement noise covariance
%
%     Z: current measurement
%
%     X: "a priori" state estimate
%     P: "a priori" estimated state covariance
%
%     Xstate: symbols of state vector
% Output:
%     Xo: "a posteriori" state estimate
%     Po: "a posteriori" estimated state covariance

% Algorithm for Extended Kalman Filter:
% Linearize input functions f and g to get fy(state transition matrix)
% and H(observation matrix) for an ordinary Kalman Filter
% 1. Xp = f(X)                      : One step projection, also provides
%                                     linearization point
%
% 2.
%          d f    |
% fy = -----------|                 : Linearize state equation, fy is the
%       d Xstate  |X=Xp               Jacobian of the process model
%
%
% 3.
%          d g    |
% H  = -----------|                 : Linearize observation equation, H is
%       d Xstate  |X=Xp               the Jacobian of the measurement model
%
%
% 4. Pp = fy * P * fy' + Q          : Covariance of Xp
%
% 5. K = Pp * H' * inv(H * Pp * H' + R): Kalman Gain
%
% 6. Xo = Xp + K * (Z - g(Xp))      : Output state
%
% 7. Po = [I - K * H] * Pp          : Covariance of Xo

% Example:
% Kalman filter for GPS positioning
% The following is a brief illustration of the principles of GPS. For more
% information see reference [2].
% The Global Positioning System(GPS) is a satellite-based navigation system
% information. The most commonly used approaches for GPS positioning are
% the Iterative Least Square(ILS) and the Kalman filtering(KF) methods.
% Both of them is based on the pseudorange equation:
%                rho = || Xs - X || + b + v
% in which Xs and X represent the position of the satellite and
% receiver, respectively, and || Xs - X || represents the distance between
% them. b represents the clock bias of receiver, and it need to be solved
% along with the position of receiver. rho is a measurement given by
% receiver for each satellites, and v is the pseudorange measurement noise
% modeled as white noise.
% There are 4 unknowns: the coordinate of receiver position and the clock
% bias. The ILS is largely used to calculate these unknowns and is
% implemented in this example as a comparison. In the KF solution we use
% the Extended Kalman filter(EKF) to deal with the nonlinearity of the
% pseudorange equation, and a CV model(constant velocity) as the process
% model.

%{
clear all
syms x Vx y Vy z Vz                 %position and velocity of the receiver in 3 dimensions,
syms b d                            %clock bias(b),clock drift(d)
Xstate = [x Vx y Vy z Vz b d].';    %state vector

T = 1; %positioning interval
N = 25;%total steps
% Set f
f = [x+T*Vx;
Vx;
y+T*Vy;
Vy;
z+T*Vz;
Vz;
b+T*d
d];
% Set Q
Sf = 36;Sg = 0.01;sigma=5;         %state transition variance
Qb = [Sf*T+Sg*T*T*T/3 Sg*T*T/2;
Sg*T*T/2 Sg*T];
Qxyz = sigma^2 * [T^3/3 T^2/2;
T^2/2 T];
Q=blkdiag(Qxyz,Qxyz,Qxyz,Qb);

% Set initial value of X and P
X = zeros(8,1);
X([1 3 5]) = [-2.168816181271560e+006
4.386648549091666e+006
4.077161596428751e+006];                 %Initial position
X([2 4 6]) = [0 0 0];                                            %Initial velocity
X(7,1) = 3.575261153706439e+006;                                 %Initial clock bias
X(8,1) = 4.549246345845814e+001;                                 %Initial clock drift
P = eye(8)*10;

fprintf('GPS positioning using EKF started\n')
tic

for ii = 1:N
% Set g
syms xs ys zs                                                % symbols for position of satellites
g_func = sqrt((x-xs)^2+(y-ys)^2+(z-zs)^2) + b;               % pseudorange equation
g = [];
for jj=1:4                                                   % pseudorange equations for each satellites
g = [g ;
subs(g_func,[xs ys zs],SV_Pos{ii}(jj,:))];
end

% Set R
Rhoerror = 36;                                               % variance of measurement error(pseudorange error)
R=eye(4) * Rhoerror;

% Set Z
Z = SV_Rho{ii}.';                                            % measurement value

[X,P] = Extended_KF(f,g,Q,R,Z,X,P,Xstate);
Pos_KF(:,ii) = X([1 3 5]).';                                 % positioning using Kalman Filter
Pos_LS(:,ii) = Rcv_Pos_Compute(SV_Pos{ii},SV_Rho{ii});       % positioning using Least Square as a contrast

fprintf('KF time point %d in %d  ',ii,N)
time = toc;
remaintime = time * N / ii - time;
fprintf('Time elapsed: %f seconds, Time remaining: %f seconds\n',time,remaintime)
end

for ii = 1:3
subplot(3,1,ii)
plot(1:N,Pos_KF(ii,:)-mean(Pos_KF(ii,:)),'-r')
hold on
plot(1:N,Pos_LS(ii,:)-mean(Pos_KF(ii,:)))
legend('EKF','ILS')
xlabel('Sampling index')
ylabel('Position error(meters)')
end
%}

% References:
% 1. R G Brown, P Y C Hwang, "Introduction to random signals and applied
%   Kalman filtering : with MATLAB exercises and solutions",1996
% 2. Pratap Misra, Per Enge, "Global Positioning System Signals,
%   Measurements, and Performance(Second Edition)",2006

function [Xo,Po] = Extended_KF(f,g,Q,R,Z,X,P,Xstate)
N_state = length(Xstate);
N_obs = length(Z);

Xp = subs(f,Xstate,X);%1

fy = subs(jacobian(f,Xstate),Xstate,Xp);%2

H = subs(jacobian(g,Xstate),Xstate,Xp);%3

Pp = fy * P * fy.' + Q;%4

K = Pp * H' * inv(H * Pp * H.' + R);%5

Xo = Xp + K * (Z - subs(g,Xstate,Xp));%6

I = eye(N_state,N_state);
Po = [I - K * H] * Pp;%7

```