function [pos vel acc]= Kalman_c(z1, z3, dt, initPos, initVel, H)
% *************************************************************
% 2nd Order Discrete Kalman Filter
% 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   ];  
%*************************************************************
% System noise, that is compensate for things that were not modeled such as
% wind gusts, drag. Assume variance of the System Noise = 0.1 affecting all
% states. 
G=[1 1 1]';
Q=0.1;
%--------------------------------------------------------------------------
% Measurement vector at time Tk
% H=[1 0 0]; Assume on position is observed
% H=[1 0 1]; Assume both position and acceleration are observed
%Variance for the noise of the position measurement
R=3;
%-------------------------------------------------------------------
% MAIN LOOP
%-------------------------------------------------------------------
%Initial Covariance Matrix
P=[ 1  0   0;
    0  1   0
    0   0  1 ];
% Initial position and velocity estimates. X=0 and XD=0 fps
x=[initPos initVel 0]';
p11 = P(1,1); p12=P(1,2); p13=P(1,3);
p22 = P(2,2); p21=P(2,1); p23=P(2,3);
p33 = P(3,3); p31=P(3,1); p32=P(3,2);
x1=x(1); x2=x(2); x3=x(3);
%-------------------------------------------------
for i=1:length(z1)
zz=z1(i) + z3(i);
v=dt(i);
a=dt(i).^2* 0.5;
% P * A'
l11 = p11+p12 *v + p13*a;
l12 = p12 +p13 * v;
l13 = p13;
l21= p21 + p22*v + p23 *a;
l22 = p22 + p23*v;
l23 = p23;
l31 = p31 + p32*v + p33* a;
l32 = p32 + p33 * v;
l33 = p33;
% A* P * A'
j11 = l11 + v* l21 + l31 *a;
j12 = l12 + l22 *v + l32 * a;
j13 = l13 + l23 *v + l33 * a;
j21 = l21 + l31 * v  ;
j22  = l22 + l32 * v;
j23 = l23 + l33 * v ;
j31 = l31;
j32 = l32;
j33 = l33;
%  G * Q * G' = [Q Q Q; 
%                      Q Q Q; 
%                      Q Q Q];
% P = A * P * A' + G*Q*G'
p11 = j11 +  Q;
p12 = j12 +  Q;
p13 = j13 +  Q;
p21 = j21 +  Q;
p22 = j22 +  Q;
p23 = j23 +  Q;
p31 = j31 +  Q;
p32 = j32 +  Q;
p33 = j33 +  Q;
% p11+R = (H * P * H' + R)
% K = P * H' * inv( H * P * H' + R )
if( H(1)> 0 && H(3) == 0 )   % Position Only
    if( (p11 + R)== 0 )
      'ERROR = Can not invert Matrix '
      return
    end
k1 = p11/ (p11 + R);
k2 = p21 / (p11 + R );
k3 = p31 / (p11 + R );
% P = (I- K *H ) * P
P11 = p11 * (1 - k1);
P12 = p12 * (1 - k1);
P13 = p13 * (1 - k1);
P21 = -k2*p11+p21;
P22 = -k2*p12 + p22;
P23 = -k2*p13 + p23;
P31 = -k3*p11 + p31;
P32 = -k3*p12 + p32;
P33 = -k3*p13 + p33;
elseif( H(1) > 0 && H(3) > 0 )       %Position and Acceleration
    if( (p11 + p13 +p31 + p33 + R)== 0 )
      'ERROR = Can not invert Matrix '
      return
    end
k1 = (p11+p13) / ( p11 + p13 +p31 + p33+ R);
k2 = (p21+p23) / ( p11 + p13 +p31 + p33+ R );
k3 = (p31+p33) / ( p11 + p13 +p31 + p33+ R );
% P = (I- K *H ) * P
P11 = p11 * (1 - k1)  - k1*p31;
P12 = p12 * (1 - k1)  - k1*p32;
P13 = p13 * (1 - k1)  - k1*p33;
P21 = -k2*p11 + p21 - k2*p31;
P22 = -k2*p12 + p22 - k2*p32;
P23 = -k2*p13 + p23 - k2*p33;
P31 = -k3*p11 + (1-k3)*p31;
P32 = -k3*p12 + (1-k3)*p32;
P33 = -k3*p13 + (1-k3)*p33;  
elseif( H(1)==0 && H(3) > 0 )   %Acceleration only
    if( (p33 + R)== 0 )
      'ERROR = Can not invert Matrix '
      return
    end
k1 = p13 / (p33 + R);
k2 = p23 / (p33 + R );
k3 = p33 / (p33 + R );
% P = (I- K *H ) * P
P11 = p11 -k1*p31;
P12 = p12-k1*p32;
P13 = p13-k1*p33;
P21 = p21-k2*p31;
P22 = p22-k2*p32;
P23 = p23-k2*p33;
P31 = p31*(1-k3);
P32 = p32*(1-k3);
P33 = p33*(1-k3);   
end
%---------prepare for next cycle ------------
p11=P11; p12=P12; p13=P13; 
p21=P21; p22=P22; p23=P23;
p31=P31; p32=P32; p33=P33;
%------------------------------------------------
% X= A * X
x1=x1 + x2*v + x3*a;
x2=x2 + x3*v;
x3=x3;
% X = X + K * (Z - H * X)
if( H(1)> 0 && H(3)==0 )
    resid = zz-x1;
elseif( H(1) > 0 && H(3) > 0 )
    resid = zz-x1-x3;
elseif( H(1)==0 && H(3) > 0 )
    resid = zz-x3;
end
x1 = x1 + k1 * resid;
x2 = x2 + k2 * resid;
x3 = x3 + k3 * resid;
pos(i) = x1;
vel(i) =  x2;
acc(i) = x3;
end