% GPS2d_demo.m - 2-dimensional Kalman filter using noisy (x,y)
% position data to track an unpowered projectile.
%
% Written by John Burnett
% Iowa State Robotics Club

clear all;
close all;

%%%%%INITIALIZE
% time increment
deltaT = .1;
% initialize time variable
T = 0;
% initialize index
k = 1;
% Initial state (actual)
X(:,1) = [0; 150; 0; 0; 200; -9.8];

%%% FILTER INITIALIZATION
% Initial state (estimate)
% Filter should be able to recover from incorrect initial estimates,
% Xest~=X.
Xest(:,2) = [100; 0; 0; 100; 0; 0];
% Q process noise matrix
% The noise propagates through matrix according to the state transition
% model, A
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 ];
% Process noise
PN = 0;
Q = PN*QT;
% R measurement noise
filterMeasNoise = 50;
actualMeasNoise = 100;
R = [filterMeasNoise 0;
     0 filterMeasNoise];
% Initial state error covariance matrix, P
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];

%%% LOOP
% pop a figure, handle is 'f'
f = figure;
% stop time
Tfinal = 100;
% start loop.  End at T=Tfinal or when object hits the ground.
while T < Tfinal && X(4,k) >= 0
k = k+1;
%%% FILTER
% State transition matrix, A
% Describes how one state affects the other, X(i+1)=A*X(i)
% State variables are X = [x; xd; xdd; y; yd; ydd]
% So, this is a 2 dimensional (x&y), three-state (position, velocity &
% acceleration) polynomial filter.
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           ;
     ];
% Measurement matrix, C
% Describes how states, X, are related to the measurements, Y
% Y = C*X
% Here, we are simply measuring the position states directly.
C = [1 0 0 0 0 0;
     0 0 0 1 0 0];

%% REAL-WORLD SIMULATION
% Simply calculating the next X.  Essentially integrating the acceleration
% and velocities, X(:,1).
A_real = [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_real*X(:,k-1);
% Actual Measurement
Z(:,k) = [1 0 0 0 0 0;
          0 0 0 1 0 0]*X(:,k) + [actualMeasNoise*randn; actualMeasNoise*randn];
%% Filter-Prediction
% Use filter model and state estimate to predict next state, state
% covariance matrix, and measurement.
Xpred(:,k) = A*Xest(:,k);
Ppred = A*P*A' + Q;
Zpred(:,k) = C*Xest(:,k);

%% Filter-Observation
% Measurement error
% by how much was prediction wrong?
innov(:,k) = Z(:,k) - Zpred(:,k);
% Measurement covariance
% how does the predicted state error affect the measurement?
S = C*Ppred*C' + R;

%% Filter-Update
% Gain matrix
% The "magic" equation for Kalman gains.  These gains are the optimal gains
% to use to update the state estimate.
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);

% Increment time
T = T + deltaT;

% Variables used for plotting
winSize = 300;
trail = 50;
pauseTime = .01;
% 
hold off;
% plot "trail" number of actual data
plot(X(1,max(1,k-trail):k), X(4,max(1,k-trail):k),'k')
% position window around current position, k
axis([X(1,k)-winSize X(1,k)+winSize X(4,k)-winSize X(4,k)+winSize])
hold on;
% plot filter's state position estimate
plot(Xest(1,max(1,k-trail):k), Xest(4,max(1,k-trail):k),'r.')
% plot measurement data
plot(Z(1,max(1,k-trail):k),Z(2,max(1,k-trail):k),'b+')

% display altitude
text(X(1,k)+winSize-200,X(4,k)-winSize+15,['Altitude: ' num2str(X(4,k))])

% display state error
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))])

% force Matlab to draw in the window
drawnow;
% pause to slow animation
pause(pauseTime);

end

%%% FINAL PLOTS
final = length(X);
title('Hit a key')
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)

