%From cfl@syscon.uu.se Fri Aug 23 15:33:43 1996
%From: Carl-Fredrik Lindberg <cfl@syscon.uu.se>
%Organization: Systems & Control Group, Uppsala University, Sweden

% EXAMPLE STARTS HERE
% Inverse response prcess 
num=3*[0 -2 1];
den=conv([5 1],[10 1]);
[A,B,C,D]=tf2ss(num,den);

t=0:0.1:50;
r=ones(length(t),1);

%-----------------------------------------------------
% Integrating controller with observer
% xdot = Ax+Bu
% x_hatdot = Ax_hat+Bu
% u = -K*x_hat - k3*x3
% xot = y-r = Cx-r


Ak=[A [0;0];
   C  0];
Bk=[B;0];
Dk=D;

Q=[0 0 0;
   0 0 0;
   0 0 1];
R=1;
K=lqr(Ak,Bk,Q,R);			% State feedback

p=eig(Ak-Bk*K);			% closed loop poles
L=place(A',C',1.1*p(1:2))';	% State estimation

k3=K(3); K=K(1:2);

% Controller designed, now perform closed-loop simulation
% New state space vector = [x x3 x_hat]'
Aa=[A -B*k3 -B*K;
    C    0   [0 0];
  L*C -B*k3 A-B*K-L*C];
Ba=[0 0 -1 0 0]';
Ca=[C 0 0 0];

xhat1=0; xhat2=0;		% Initial states for observer
[y,x]=lsim(Aa,Ba,Ca,Dk,r,t,[0 0 0 xhat1 xhat2]);
u=-[k3 K]*x(:,3:5)';
plot(t,y,t,u,'r'), grid
title('y yellow and u red (Integrating controller with observer)')
% Comments from S. Skogestad: 
%    1. Yields 2 degerees-of-freedom controller
%    2. What is the state-space description of the final controller?
% END EXAMPLE

%------------------------------------------------------------------

% The following is a general function for doing the same as above

function [K,kn,L]=lq(A,B,C,rho);
%LQ	[K,kn,L]=lq(A,B,C,rho)
%	Derives an integrating LQ controller with observer and simulate
%	it during the first 50s.
%	A,B,C is a state space system and rho is the input penalty.
%       K is the state-feedback and L is the observer gain
%
%  r  +  e  |----| xn |----|   -     u    |---------|         y
%  --->O--->| 1/s|--->| kn |---->O------->| Process |-------|---->
%      |-   |____|    |____|     |-       |_________|       |
%      |                         |                          |
%      |                         |  |---| xhat |--------|   |
%      |                         |--| K |<-----| Observ.|<--|
%      |                            |___|      |________|   |
%      |____________________________________________________|
%
% xdot = Ax + Bu                       % The process
% x_hatdot = Ax_hat + Bu+ L(y-Cxhat)   % The observer
% u = -K*x_hat - kn*xn                 % control signal
% xndot = r-y

n=length(B);		% System degree

			% A new system is set up with state vector [x xn]'
			% Then the feedback for this system is determined
			% as if these states were measureable.
Ak=[A zeros(n,1);	% Ak and Bk is only used to determine the feedback K.=

    -C  0];
Bk=[B;0];
Q=zeros(n+1,n+1); Q(n+1,n+1)=1;   % Output penalty
R=rho;			% Input penalty
K=lqr(Ak,Bk,Q,R);	% LQ state feedback and "integrator weight (kn)"
kn=K(n+1); K=K(1:n);

p=eig(A-B*K);		% closed loop poles
L=place(A',C',1.1*p)';	% Observer gain, L is derived so the observer
			% becomes slightly faster than closed loop system

% END general function; Below is only for simulation purposes.

% ----------------------------------------------------------------
% New state space vector = [x xn x_hat]'
Aa=[A -B*kn -B*K;
    -C   0   zeros(1,n);
  L*C -B*kn A-B*K-L*C];
Ba=[zeros(1,n) 1 zeros(1,n)]';
Ca=[C zeros(1,n+1)];

xhat0=0*ones(n,1);		% Initial states for observer
t=0:0.1:50;
r=ones(length(t),1);
D=0;				% The direct term is set to 0
[y,x]=lsim(Aa,Ba,Ca,D,r,t,[zeros(n,1); 0; xhat0]);
u=-[kn K]*x(:,n+1:2*n+1)';
plot(t,y,t,u,'r'), grid
title('y yellow and u red (Integrating LQ-controller with observer)')
%------------------------------------------------------------------

