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

% This file goes with the model_fsf_int.mdl simulink model

close all;

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

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

C = [ 1 0 0 0
      0 1 0 0 ];
  
D = [0 0
     0 0];

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

Dfull = [ 0 0
          0 0
          0 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 0;0 0;0 0] ), horzcat ( C, [0 0; 0 0] ) )
Bi = vertcat ( B, [0 0; 0 0] )

% place the poles
mypoles = 0.25*[ -1+.2i, -1-.2i, -2, -3, -4, -5 ];
Kall = place ( Ai, Bi, mypoles )
K=Kall(:,1:4)
Ki=Kall(:,5:6)
eigs(Ai-Bi*Kall)

Arand = A + 0.1*rand(4,4)

