% Simulate improved GPS tracking
% Using Kalman filtering, prediction and smoothing
% Finds best filter parameter qv

qvvec = logspace(-4,4,100);
nr=length(qvvec);
res=zeros(1,nr);
v=4;    % speed m/s
h=1;    % sampling period in seconds
N=1000;   % 2N+1 datapoints

% The true trajectory (two straight lines with a turn)
xtrue1=[zeros(1,N+1) 0:v*h:(N-1)*v*h];     
xtrue2=[0:v*h:N*v*h N*v*h*ones(1,N)];
xtrue=[xtrue1;xtrue2];

% The gps measurements
gps_accuracy = 5; % measurement accuracy  
randn('state',1)
ygps = xtrue + gps_accuracy*randn(size(xtrue));

for ind=1:nr
    qv=qvvec(ind);
    % The Kalman filter (matlab notation)
    I = eye(2);
    A = [I h*I; 0*I I];
    G = [0*I;I];
    C = [I 0*I];
    Q = qv*I;    % tuning parameter
    R = gps_accuracy^2*I;
    [M,P,Z,E] = dlqe(A,G,C,Q,R);
    kalman1 = ss(A-A*M*C,A*M,C,0*I,h);
    kalman0 = ss(A-A*M*C,A*M,C-C*M*C,C*M,h);
    time = (0:2*N)*h;
    xpred1 = lsim(kalman1, ygps,time)';
    xpred0 = lsim(kalman0, ygps,time)';

    err1 = xpred1-xtrue;
    res1(ind)=std(err1(:));
    err0 = xpred0-xtrue;
    res0(ind)=std(err0(:));
end
semilogx(qvvec,res1,'linewidth',1.2); hold on
semilogx(qvvec,res0,'r','linewidth',1.2); hold on
xlabel('qv','fontsize',16,'fontweight','b')
ylabel('Error standard deviation [m]','fontsize',16,'fontweight','b')
set(gca,'fontsize',14)
grid on
title('Prediction accuracy','fontsize',16,'fontweight','b')
legend('No Direct Term','Direct Term','Location','East')

 % print -depsc gps1.eps
