%Original Code by
%
%************************************************************
% Tim Hawkins, 1999 
% 
% Kalman Filter For 1st Order System Sensor Input
% Loops are used in this program for easier portability to C
% or assembly code
%
% http://www.techsystemsembedded.com/Kalman.html
%
%************************************************************
%
% Edited by Mitchell Kotler and John Penning for Demonstration Purposes

clear

%load NoisySen.dat  % Actual Noisy Data Taken from the Feedback
                              % from a Temperature Sensor

%change this to correct data file
load dec_z.log
x = dec_z(:,1);
y = dec_z(:,2);
z = dec_z(:,3);

%x = x( find ( z~= 0 ) );
%y = y( find ( z~= 0 ) );
%z = z( find ( z~= 0 ) );

%ind = find( x<50 & y<50 );
%x = x(ind);
%y = y(ind);
                              
NoisySen = z;

len = length(NoisySen)

Q = 1;
R = 200; %Adjust R for Degree of Damping

for n = 1: 1: len;

   zz(n) = NoisySen(n);

end;

len
Q
R

for n = 1: 1: len;

   index(n) = n;

end;

Pmin1 = 0;

Pmin1


K = Pmin1/(Pmin1+R)

X_hat(1) = NoisySen(1) + K*(zz(1) - NoisySen(1))
	
P = (1-K)*Pmin1

Pmin = P + Q

X_hat_min(1) = X_hat(1)


for n = 2: 1: len;

   K = Pmin/(Pmin+R);

   if z(n) ~= 0
      X_hat(n) = X_hat_min(n-1) + K*(zz(n) - X_hat_min(n-1));
   else
       X_hat(n) = X_hat_min(n-1) + K*(X_hat(n-1) - X_hat_min(n-1));
   end
	
   P = (1-K)*Pmin;

   Pmin = P + Q;

   X_hat_min(n) = X_hat(n);

end;

figure
plot(index,zz,'g-',index,zz,'gx',index,X_hat,'b--',index,X_hat,'b*')
legend('Z','Z','X^','X^')
title(['1st Order Kalman Filter Results; R = ',num2str(R)])
axis([0 length(zz) 0 max(zz)]);
