% Simulate improved GPS tracking
% Using Kalman  smoothing

qvvec = 0.04;
qvvec = logspace(-3,4,100);

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

np = 5; %prediction horizon
% 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;
    
    I4 = eye(4);
    O4 = zeros(4,4);
    Ae = [A O4 O4 O4 O4 O4; I4 O4 O4 O4 O4 O4; O4 I4 O4 O4 O4 O4; O4 O4 I4 O4 O4 O4; O4 O4 O4 I4 O4 O4; O4 O4 O4 O4 I4 O4];
    Ge = [G ; zeros(4*np,2)];
    Ce = [C zeros(2,4*np)];
    Cut = [zeros(2,4*np) C];
    
    
    [M,P,Z,E] = dlqe(Ae,Ge,Ce,Q,R);
    % Change the following two lines to
    %   kalman1 = ss(YourA, YourB, YourC, YourD,h);
    %   kalman0 = ss(....,h); 
    % to give 4-step and 5-step smoothers 
    kalman1 = ...        
    kalman0 = ...
    time = (0:2*N)*h;
    xpred1 = lsim(kalman1, ygps,time)';
    xpred0 = lsim(kalman0, ygps,time)';

    err1 = xpred1(:,np+1:end)-xtrue(:,1:end-np);
    res1(ind)=std(err1(:));
    err0 = xpred0(:,np+1:end)-xtrue(:,1:end-np);
    res0(ind)=std(err0(:));
end
figure(1)
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')

figure(2)
plot(xpred0(1,np+1:end),xpred0(2,np+1:end),'r-','linewidth',1.2)
hold on
plot(xtrue(1,1:end-np),xtrue(2,1:end-np),'k-')
title(['Smoothing with horizon ' num2str(np)],'fontsize',16,'fontweight','b')
 % print -depsc gps1.eps
