% example to test filter
% refer eqns from ASTRO Hw1

 clear;
 close all;
 clear s
 % Specify Initial x

 dt=.5 ;
 s.x = [0; 0];
 xo=s.x ;
 s.A = [1 .5; 0 1];
 
 s.H = eye(1);
 s.P = nan;
 
 error_cov = .05^2 ;
 
 %arbitrary state noise  
 s.Q = .5^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; 
 
 %arbitrary measurement noise
 s.R = (10)^2*eye(1);

 % Do not define any system input (control) functions:
 s.B = 0;
 s.u = 0;
 
 % specify a time step
  s.dt=.25 ;
  
 % Generate random observation points to filter
 tru=[]; % truth value
 j=1;
 B=s.A ;
 hold on ;
 for t=0:s.dt:5
    C=expm(B*t)*xo; 
    tru(j) =C; j=j+1 ;
    s(end).z = tru(:,end) + normrnd(0,.01) ; % create a measurement
    s(end+1)=kalman_filter(s(end)); % perform a Kalman filter iteration
    
 end
 
 for i=1:1:length(s)-1
     Z(i)=s(i).z ;
     X(i)=s(i).x ;
 end
 
 T=0:.25:5;
    plot(T,Z,'g')
    plot(T,X,'r')
    plot(T,tru)