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

close all;

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

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

C = [ 1 0 0 ];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A full state feedback controller
%

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

% place the poles
mypoles = [ -1+i, -1-i, -5 ];
K = place ( A, B, mypoles );
systemp = ss ( A-B*K, B, C, 0 );

% simulate
sys1 = ss ( A-B*K, B, C, 0 );
impulse ( sys1 );
title ( 'Impulse Response With Full State' );

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

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

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

% define the combined system
Anew = vertcat ( horzcat ( A, -B*K  ), horzcat ( L*C, A-B*K-L*C ) );
Bnew = vertcat ( B, B );
Cnew = horzcat ( C, -C );
sys2 = ss ( Anew, Bnew, Cnew, 0 );

% simulate -- note, since the state and the estimate start out the same,
% they never differ
figure;
impulse(sys2);

% If the estimate starts out wrong, it eventually coverges to the
% actual state

x0 = [ 0 0 0 1 1 1 ];
t = 0:0.01:5;
figure;
initial ( sys2, x0, t );







