%
%   SFSOTEST1.M  Test Problem for State Feedback & State Observer Design
%
%   This is a Matlab equivalent of the simple 2x2 test problem done in the
%   Lecture Notes.
%
%   This file is broken into three sections, as follows:
%     Part I. Setup base data for the linear model and show that the base open loop
%             plant is unstable.
%    Part II. Add state feedback control to stabilize the system and simulate the
%             system behavior for a step change in the state 1 reference point.
%   Part III. Add state feedback control and a full observer to stabilize the 
%             system. Simulate system behavior for a step change in the state 1 
%             reference point. This should give the same simulation as Part II.
%
%   File prepared by J. R. White, UMass-Lowell (last update: Feb. 2020)
%

      clear all,  close all, nfig = 0;
      format compact
%
%   Part I. Setup base data for the linear model and show that the open loop
%           plant is unstable.
%
%   create state space matrices for plant (output state 1 value)
      A = [0  1;  20.6  0];   B = [0 1]';   C = [1  0];   D = [0];
      disp('   *** Results for SFSOTest1 ***'),   disp('  ')
      disp('State Space Matrices for the Plant')
      A,  B,  C,  D
%
%   compute eigenvalues of state matrix for open loop plant
      disp('Eigenvalues of the "Open Loop Plant"');     ev = eig(A)
%
%
%   Part II. Add state feedback control to stabilize the system and simulate
%            system behavior for a step change in the state 1 reference point.
%
%   check for full state controllability
      disp('Controllability Matrix for this system'),   M = ctrb(A,B)
      disp('Rank of Controllability Matrix'),   rank(M)     
%
%   calculate state feedback gains for specified closed loop poles
      clp = [-1.8+2.4j  -1.8-2.4j];
      Ks = place(A,B,clp);
      disp('Desired closed loop poles for state feedback controller');   clp
      disp('State feedback gains needed to give desired poles');         Ks
      disp('Calculated eigenvalues of system with state feedback'); eig(A-B*Ks)
%
%   calculate Nv for zero SS error  (see derivation in notes - next subsection)
      Nv = -1.0/(C*inv(A-B*Ks)*B);
      disp('Setpoint gain for zero SS error');    Nv
%
%   simulate linear plant + controller
      to = 0;  tf = 5; 
      t = linspace(to,tf,201);
      syscl1 = ss(A-B*Ks,B*Nv,C,D);
      [y1,t,x1] = step(syscl1,t);
%
%   plot results from state feedback case
      nfig = nfig+1;      figure(nfig)
      subplot(2,1,1),plot(t,x1(:,1),'r-',t,x1(:,2),'g--','LineWidth',2),grid,
      title('SFSOTest1:  States for State Feedback Test Case')
      xlabel('Time (sec)'),ylabel('State Variables')
      legend('x1(t)','x2(t)','Location','East')
%
%   Part III. Add state feedback control and a full observer to stabilize the 
%             system. Simulate system behavior for a step change in the state 1 
%             reference point. This should give the same simulation as Part II.
%
%   check for full state observability
      disp('Observability Matrix for this system'),   H = obsv(A,C)
      disp('Rank of Observability Matrix'),   rank(H)     
%
%   calculate estimator gains for specified observer poles
      op = 3*clp;   % estimator dynamics is 3 times faster than closed loop poles
      L = place(A',C',op);   L = L';
      disp('Desired observer poles for state feedback controller');   op
      disp('Estimator gains needed to give desired poles');           L
      disp('Calculated eigenvalues of estimator system');      eig(A-L*C)
%
%   setup matrices for plant + controller model
      A11 = A;     A12 = -B*Ks;         B1 = B*Nv;
      A21 = L*C;   A22 = A-L*C-B*Ks;    B2 = B*Nv;
      zz = 0;
      AB = [A11 A12; A21 A22];   BB = [B1; B2];
      CB = [C zz*C];
%
%   simulate linear plant + controller
      syscl2 = ss(AB,BB,CB,D);    [y2,t,x2] = step(syscl2,t);
%
%   separate plant and estimator states
      nn = max(size(A));
      xp2 = x2(:,1:nn);     xe2 = x2(:,nn+1:2*nn);
%
%   plot results from case with full observer
      subplot(2,1,2),plot(t,xp2(:,1),'r-',t,xp2(:,2),'g--','LineWidth',2),grid,
      title('SFSOTest1:  States for State Feedback with Full Observer ')
      xlabel('Time (sec)'),ylabel('State Variables')
      legend('x1(t)','x2(t)','Location','East')
%
%   also plot error for plant vs estimator
      nfig = nfig+1;      figure(nfig)
      plot(t,xp2(:,1)-xe2(:,1),'r-',t,xp2(:,2)-xe2(:,2),'g--','LineWidth',2),grid,
      title('SFSOTest1:  Error (State - Observer) Dynamics')
      xlabel('Time (sec)'),ylabel('Error in State Variables')
      legend('e1(t)','e2(t)')
%
%   end of simulation
