% Francis Regulator 
% by Stefano Panzieri  (panzieri@uniroma3.it)
% 24-05-2007
%
%   A: plant state matrix
%   B: plant input matrix
%   C: plant output matrix (to be zeroed)
%   S: exosystem with disturbances and reference
%   P: state disturbances matrix
%   Q: output disturbances and reference
%   autov_s: desired eienvalues for state
%   autov_o: desired eigenvalues for observer
%
%   dw/dt = S w
%   dx/dt = A x + B u + P w
%   e = C x + Q w
%


function [F,G,H,I]=Francis_REG(A,B,C,S,P,Q,autov_s,autov_o)

F=[];G=[];H=[];I=[];

% Dimensions 
n=size(A,1);
ns=size(S,1);
ni=size(B,2);
nu=size(C,1);

if ni~=nu
    disp('Input number is different from output number!')
    return
end


% gains for system and observator
[K,prec,message]=place(A,-B,autov_s);


disp('System eigenvalues:')
eig(A+B*K)

OB=obsv([A P; zeros(ns,n) S],[C Q]);
if rank(OB)<n+ns
    disp('Unobservable system!')
    return
end

[G,prec,message]=place([A P; zeros(ns,n) S]',[C Q]',autov_o);
G=G';
disp(message)


disp('Observer eigenvalues:')
eig([A P; zeros(ns,n) S]-G*[C Q])

if sum(sum(abs(S)))==0
    % Simplified solution of regulator equations
    X=inv([A B; C zeros(nu,ni)])*[-P;-Q];
    Pigreco=X(1:n,:);
    Gamma=X(n+1:end,:);    
else
    % Let's do it using Maple toolbox
    
    [Gamma,Pigreco]=Francis_EQ(A,B,C,S,P,Q);
end



% Compensator matrices
H=[K Gamma-K*Pigreco];
F=[A P;  zeros(ns,n) S]+[B;zeros(ns,ni)]*H-G*[C Q];
I=zeros(ni,ni);

% Verify
Sigma=[Pigreco;eye(ns)];
if sum(sum(abs(Sigma*S-F*Sigma)))>1e-6
    disp('Francis equations uncorrect!')
    Sigma*S-F*Sigma
    return
end

