Extended_KF(f,g,Q,R,Z,X,P,Xstate)

%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 satellitebased navigation system
% that provides a user with proper equipment access to positioning
% 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
load SV_Pos %position of satellites
load SV_Rho %pseudorange of satellites
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((xxs)^2+(yys)^2+(zzs)^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

