% GPSbias2.m - 1-dimensional Kalman filter using GPS to aid an
% accelerometer bias.
% Version 2 because of a difference in measurement setup.  This version
% measures the position rather than the acceleration.
function GPS2D_demo(noise_GPS)

%clear all;
%close all;
%%%%%INITIALIZE
% time step
deltaT = .1;
T = 0;
k = 1;
w = .1;

%noise_GPS = 100;
%noise_GPS = noise;


%Initial state estimate
X(:,1) = [0; 150; 0; 0; 200; -9.8];
Xold = X;

%% Filter initialization
Xest(:,2) = [0; 0; 0; 0; 0; 0];
% Q process noise matrix
QT = [(deltaT^5)/20 (deltaT^4)/8 (deltaT^3)/6 0 0 0 ;
      (deltaT^4)/8  (deltaT^3)/3 (deltaT^2)/2 0 0 0 ;
      (deltaT^3)/6  (deltaT^2)/2 deltaT       0 0 0 ;
      0             0            0            (deltaT^5)/20 (deltaT^4)/8 (deltaT^3)/6;
      0             0            0            (deltaT^4)/8  (deltaT^3)/3 (deltaT^2)/2;
      0             0            0            (deltaT^3)/6  (deltaT^2)/2 deltaT ];
PHIS = 0;
Q = PHIS*QT;
% R measurement noise
R = [50 0;
     0 50];
% P initial old cov. matrix
P_init = 10000;
P = [P_init 0 0 0 0 0;
     0 P_init 0 0 0 0;
     0 0 P_init 0 0 0;
     0 0 0 P_init 0 0;
     0 0 0 0 P_init 0;
     0 0 0 0 0 P_init];

Tfinal = 100;
%%%%%LOOP

f = figure;
while T < Tfinal && X(4,k) >= 0
k = k+1;
%%%FILTER
%State transition matrix
A = [1 deltaT 0.5*deltaT^2  0 0 0;
     0 1      deltaT        0 0 0;
     0 0      1             0 0 0;
     0 0      0     1 deltaT 0.5*deltaT^2;
     0 0      0     0 1      deltaT      ;
     0 0      0     0 0      1           ;
     ];

U(k)=1;
B = [zeros(6,1)];
%Measurement matrix
C = [1 0 0 0 0 0;
     0 0 0 1 0 0];

%%%Real-World Simulation
Areal = [1 deltaT 0 0 0 0;
         0 1 0 0 0 0;
         0 0 0 0 0 0;
         0 0 0 1 deltaT 0.5*deltaT^2;
         0 0 0 0 1 deltaT;
         0 0 0 0 0 1];

X(:,k) = A*X(:,k-1);
%% Measurement
Z(:,k) = [1 0 0 0 0 0;
          0 0 0 1 0 0]*X(:,k) + [noise_GPS*randn; noise_GPS*randn];
 
%Filter-Prediction
Xpred(:,k) = A*Xest(:,k) + B*U(k);
Zpred(:,k) = C*Xest(:,k);
Ppred = A*P*A' + Q;

%Filter-Observation
innov(:,k) = Z(:,k) - Zpred(:,k);
S = C*Ppred*C' + R;
%Filter-Update
%gain matrix
W = Ppred*C'*S^-1;
%state update
Xest(:,k+1) = Xpred(:,k) + W*innov(:,k);
%covariance update
P = Ppred - W*S*W';
Pt(:,k) = diag(P);

T = T + deltaT;

%end


winSize = 300;
trail = 50;
pauseTime = .01;
% 
hold off;
plot(X(1,max(1,k-trail):k), X(4,max(1,k-trail):k),'k')
axis([X(1,k)-winSize X(1,k)+winSize X(4,k)-winSize X(4,k)+winSize])
hold on;
plot(Xest(1,max(1,k-trail):k), Xest(4,max(1,k-trail):k),'r.')
plot(Z(1,max(1,k-trail):k),Z(2,max(1,k-trail):k),'b+')

text(X(1,k)+winSize-200,X(4,k)-winSize+15,['Y Position: ' num2str(X(4,k))])

text(X(1,k)+winSize-200,X(4,k)+winSize-15,'Error:')
text(X(1,k)+winSize-150,X(4,k)+winSize-15,['x   : ' num2str(X(1,k)-Xest(1,k))])
text(X(1,k)+winSize-150,X(4,k)+winSize-45,['xd  : ' num2str(X(2,k)-Xest(2,k))])
text(X(1,k)+winSize-150,X(4,k)+winSize-75,['xdd : ' num2str(X(3,k)-Xest(3,k))])
text(X(1,k)+winSize-150,X(4,k)+winSize-105,['y   : ' num2str(X(4,k)-Xest(4,k))])
text(X(1,k)+winSize-150,X(4,k)+winSize-135,['yd  : ' num2str(X(5,k)-Xest(5,k))])
text(X(1,k)+winSize-150,X(4,k)+winSize-165,['ydd : ' num2str(X(6,k)-Xest(6,k))])



drawnow;
pause(pauseTime);

end
%

final = length(X);
pause;
figure
plot(X(1,1:final), X(4,1:final),'k')
axis([-1000 7000 -500 2500])
title('Actual Position')
hold on;
pause;
plot(Z(1,1:final),Z(2,1:final),'b+')
title('Measurements')
pause;
plot(Xest(1,1:final), Xest(4,1:final),'r.')
title('Estimated Postion')
pause;

figure
subplot(3,1,1)
plot(X(1,1:final)-Z(1,1:final),'c.')
hold on;
plot(X(1,1:final)-Xest(1,1:final),'k')
plot(1:final,Pt(1,1:final),'b:',1:final,-Pt(1,1:final), 'b:')
title('Error in X position')
legend('Measured', 'Filter','Theory', '',-1)

subplot(3,1,2)
plot(X(2,1:final)-Xest(2,1:final),'k')
hold on;
plot(Pt(2,:),'b:')
plot(-Pt(2,:),'b:')
title('Error in X velocity')
legend('Simulation','Theory',-1)

subplot(3,1,3)
plot(X(3,1:final)-Xest(3,1:final),'k')
hold on;
plot(Pt(3,:),'b:')
plot(-Pt(3,:),'b:')
title('Error in X acceleration')
legend('Simulation','Theory',-1)

