function [pos vel acc ]= Kalman2(z1,z2,z3,dt,initPos, initVel,H)
% *************************************************************
% 2nd Order Discrete Kalman Filter 
%*************************************************************

% System noise
%Assume a little bit of noise for all states. Helps the kalman filter
G=[1 1 1]';
% Measurement vector at time Tk
% Only position and acceleration can be observed
% H=[1 0 1]; Provided as Argument from main program
% Variance of the System Noise = 1
Q=0.1;
%Identity Matrix
I=[1  0  0;
   0  1  0;
   0  0  1 ];
%Variance for the noise of the measurement
R=3;
%-------------------------------------------------------------------
% MAIN LOOP
%-------------------------------------------------------------------
%Initial Covariance Matrix
P=[ 10  0   0;
    0  10   0
    0   0  10 ];
% Initial position and velocity estimates. X=0 and XD=0 fps
x=[initPos initVel 0]';
%-------------------------------------------------
for i=1:length(z1)
%--------------------------------------------------------------------------
% Simulate a data loss; this code does not belong here, delete later
%--------------------------------------------------------------------------

%--------------------------------------------------------------------------    
% System Matrix relating X(k+1) and X(k)
%x=x0 + vt + at^2/2 and v= v0 + at
% dk+1 = dk + skT + akT*T/2 and sk+1=sk + ak*T and ak+1 = ak

A=[1  dt(i)  (dt(i).^2)*0.5;
   0     1             dt(i)
   0     0               1   ];  
z= H * [z1(i), z2(i), z3(i) ]' ;
P=A*P*A' + G*Q*G';
K=P*H'*inv(H * P * H' + R);
P=(I - K*H) * P;
x= A*x;
x= x + K*(z - H*x);
pos(i) = x(1);
vel(i) = x(2);
acc(i) = x(3);
end