%
% full state feedback design example
% for a SISO system
% 

% This file goes with the model_fsf_int_obs.mdl simulink model

close all;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The system
%

A = [ -1  1 -1
       2 -3  0
       1  0  0 ];
   
B = [ 0
      0
      1 ];

C = [ 1 0 0 ];

Cfull = [ 1 0 0 
          0 1 0
          0 0 1 ];
      

D = [0];

Dfull = [ 0
          0
          0 ];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A full state feedback controller with an integrator
%

% Check for controllability
M = ctrb ( A, B );
rank ( M )

% Build the new A and B matrices for full state feedback + an integrator
% design
Ai = vertcat ( horzcat ( A, [0;0;0] ), horzcat ( C, [0] ) )
Bi = vertcat ( B, [0] )

% place the poles
mypoles = 0.25*[ -1+2i, -1-2i, -2, -3 ];
Ki = place ( Ai, Bi, mypoles );
eigs(Ai-Bi*Ki)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Observer design
%

% Check for observability
Om = obsv ( A, C );
rank ( Om )

% design observer
obspoles = 10*[ -1, -2, -3 ];
L = place ( A', C', obspoles )';

Ao = A-L*C
Bo = horzcat ( B, L )
Co = Cfull
Dob = [ 0 0
        0 0
        0 0 ]

Uncertainty=0.01*rand(3,3)