% example to test filter
% refer eqns from ASTRO Hw1

 clear;
 close all;
 clear s
 % Specify Initial x
 
 s.x = [1 -1 2 2 3 -3]';
 xo=s.x ;
 s.A = [0 1 0 0 0 0;-1 -1 0 0 0 0;0 0 0 1 0 0;0 0 -1 -1 0 0;0 0 0 0 0 1; 0 0 0 0 -1 -1];
 s.H = eye(6);
 s.P = nan;
 
 error_cov = .1 ;
 s.Q = error_cov.*eye(6);
 %arbitrary noise set to .8
 
 s.R = error_cov.*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 voltage
 j=1;
 B=s.A ;
 for t=0:s.dt:10
    C=expm(B*t)*xo; 
    tru(1:6,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
 end
 figure
 hold on
 grid on
 % plot measurement data:
 for i=1:1:length(s)-1
 Z(:,i)=s(i).z ;
 X(:,i)=s(i).x ;
 end
 
 %hz=plot3(Z(1,:) , Z(3,:) , Z(5,:) ,'r.');
 
 %hk=plot3(X(1,:) , X(3,:) , X(5,:),'g-');
 %ht=plot3(tru(1,:) , tru(3,:) , tru(5,:),'s-');
 %legend([hz hk ht],'observations','Kalman output','true state',0)
 
 
 hold off

 figure;
 subplot 221
 hold on
 plot(Z(1,:),'r.'); plot(X(1,:),'b-'); plot(tru(1,:),'g-');
 xlabel('X values'); 
 
 subplot 222
 hold on
 plot(Z(3,:),'r.'); plot(X(3,:),'b-'); plot(tru(3,:),'g-');
 xlabel('Y values'); 

 subplot 223
 hold on
 plot(Z(5,:),'r.'); plot(X(5,:),'b-'); plot(tru(5,:),'g-');
 xlabel('Z values');
 

 legend('observations','Kalman output','true state',0)
 s