% example to test filter
% refer eqns from ASTRO Hw1

 clear;
 close all;
 clear s
 % Specify Initial x


 s.x = [.1];
 xo=s.x ;
 s.A = [2];
 
 s.H = eye(1);
 s.P = nan;
 
 error_cov = .1^2 ;
 
 %arbitrary state noise  
 s.Q = (.1)^2*eye(1);
 
 %arbitrary measurement noise
 s.R = (.1)^2*eye(6);

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