% demo of SSM with inputs
% set system =1 or system=2
system=2;

figure(1);

clf;
u=zeros(2,1);
x=zeros(2,1);
y=zeros(3,1);

if system==1;
  A=[0.6 -0.6;   0.0 0.8];
  B=[0.6 0.1; 0 0.4]*0.3;
elseif system==2,
  th=0.1; A=[cos(th) -sin(th); sin(th) cos(th)]*0.95;
  B=eye(2)*0.2; % B=[0.6 0.1; 0 0.4];
elseif system==3;
  A=[0.1 -0.7;   0.9 0.8];
  B=[0.6 0.1; 0 0.4];
end; 


C=[1 0 -1;
  0 0.5 0.5]';
subplot(2,2,1);  
p1=plot(u(1),u(2),'ro');
axis([-1 1 -1 1]);
xlabel('u_1');
ylabel('u_2');
title('Inputs (u)','FontSize',24);
axis('equal');
axis('square');
  
subplot(2,2,2);  
p2=plot(x(1),x(2),'bo');
axis([-2 2 -2 2]);
xlabel('x_1');
ylabel('x_2');
title('State (x)','FontSize',24);
axis('equal');
axis('square');

subplot(2,1,2);  
p3=plot3(y(1),y(2),y(3),'ko');
axis([-2 2 -2 2 -2 2]);
xlabel('y_1');
ylabel('y_2');
zlabel('y_3');
title('Outputs (y)','FontSize',24);

fig=gcf;
disp('Hit Key to Continue');
pause;

set(p1,'EraseMode','xor');
set(p2,'EraseMode','xor');
set(p3,'EraseMode','xor');

N=5000;
Y=zeros(N,3);

for i=1:N,
  subplot(2,2,1);  
  scrn_pt = get(0,'PointerLocation');
  loc = get(fig,'Position');
  pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)];
  set(gcf,'pointer','crosshair');
  set(fig,'CurrentPoint',pt);
  uu=get(gca,'CurrentPoint');
  u(1)=uu(1,1); u(2)=uu(1,2);
  set(p1,'MarkerSize',10,'MarkerFaceColor','r','XData',u(1),'YData',u(2));

  subplot(2,2,2);  
  x=A*x+B*u+randn(2,1)*0.03;
  % plot(x(1),x(2),'.');
  set(p2,'MarkerSize',10,'MarkerFaceColor','b','XData',x(1),'YData',x(2));

  subplot(2,1,2);  
  y=C*x+randn(3,1)*0.1;
  set(p3,'MarkerSize',10,'MarkerFaceColor','k','XData',y(1),'YData',y(2),'ZData',y(3));
  % plot3(y(1),y(2),y(3),'.');
  % hold on;
  drawnow;
  Y(i,:)=y';
  
end;

figure(2);
plot3(Y(:,1),Y(:,2),Y(:,3),'.');