% example to test filter
% refer eqns from ASTRO Hw1

 clear;
 close all;
 clear s
 % Specify Initial x
 

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

 % Do not define any system input (control) functions:
 s.B = 1;
 s.u = 1;
 
 % specify a time step
  s.dt=.1 ;
  
 % Generate random observation points to filter
 tru=[]; % truth value
 j=1;
 B=s.A ;
 for t=0:s.dt:10
    C=expm(B*t)*xo; 
    tru(1,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
 
 hold on
 grid on
 % plot measurement data:
 for i=1:1:length(s)-1
 s(i).z 
 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

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

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

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