[A_1,B_1,varargout]=ModalReduction(A,B,varargin)

function [A_1,B_1,varargout]=ModalReduction(A,B,varargin)
%
% [A_1,B_1]=ModalReduction(A,B,pref);
% Derives an optimal order (r-th order) model dZ/dt = A_1 z + B_1 u for the higher order
% model dX/dt = A X + B u. The reduction is based on Chidambara model as
% proposed in
% M. R. Chidambara and E. J. Davison, " Further remarks on simplifying
% linear dynamical systems", IEEE Trans. Auto. Contr. (Correspondence),
% Vol. 12, pp. 213-214, Apr. 1967.
%
% Inputs :
%         A - n x n State matrix of higher order system.
%         B - n x m Input matrix of higher order system.
%      pref - 1 x n Preference vector showing the order in which states are
%             to be retained in the reduced order model.
%             NOTE : pref can also be given as a single integer 'r', in the
%             range 1 to n. In this case, the optimal order routine is
%             turned off and a reduced r-th order model is obtained. In
%             this case, the states x1,...xr would be retained.
%
% Alternatives:
%  [A_1,B_1]=ModalReduction(A,B);
%  When the parameter 'pref' is omitted, it is assumed to be [1:n], i.e.,
%  it is assumed that the states of the system are already arranged in the
%  preferred order.
%
%  [A_1,B_1,C_1,D_1]=ModalReduction(A,B,C,D,pref);
%  [A_1,B_1,C_1,D_1]=ModalReduction(A,B,C,D);
%  This set of alternative formats of the command apply to complete state
%  space representation of a system, i.e., for systems with output matrix C
%  and transmission matrix D.
%  The matrices A_1,B_1,C_1 and D_1 correspond to the reduced order model.
%
%  In addition to the above formats, the script also gives out the optimal
%  order for a given input.
%  [A_1,B_1,C_1,D_1,r_opt]=ModalReduction(A,B,C,D);
%  [A_1,B_1,r_opt]=ModalReduction(A,B);
%Thus,
%
%  Example :
% A =[-2.5025   -0.3865   -0.7132    1.2355    0.1635;
%    -0.3865   -2.0955   -0.8675    0.6665   -0.0264;
%    -0.7132   -0.8675   -1.8732   -0.4799    0.6618;
%     1.2355    0.6665   -0.4799   -4.0270    1.2148;
%     0.1635   -0.0264    0.6618    1.2148   -1.4731];
% B =[-1.5309;  0;  0.6475;   0;  1.0916];
% C =[-0.0948    0.4700         0   -1.2266         0];
% D = 0.2563;
%
% [A_1,B_1,C_1,D_1,r_opt]=ModalReduction(A,B,C,D)
% A_1 =
%    -0.7126    0.0503
%     0.0186   -0.6687
% B_1 =
%    -1.5309
%          0
% C_1 =
%    -1.7511    0.9875
% D_1 =
%     0.2563
% r_opt =
%      2
%   Developed by : S. Janardhanan, [e-mail]
%   Date : 19 Sep. 2008

% Error Checking

error(nargchk(2,5,nargin));
error(nargoutchk(2,5,nargout));
[n1,n2]=size(A);
[n,m]=size(B);
if ~isequal([n1 n2],[n2 n])
    error('Error in sizes of A and B matrices');
end

pref=1:n;
switch nargin

    case 3
        pref=varargin{1};

    case 4
        C=varargin{1};
        Ds=varargin{2};
        pref = 1:n;
    case 5
        C=varargin{1};
        Ds=varargin{2};
        pref=varargin{3};
end
pref = reshape(pref,1,numel(pref));

if ~ismember(length(pref),[1 n])
    error('Error in the length of preference vector');
end

if any(pref<=0) || any(pref>n) || any(pref ~= fix(pref)) || ~ismember(length(unique(pref)),[1 n])
    error('Entries of preference vector should be integers in the range of 1-n only')
end
if nargin > 3
    [p,n3]=size(C);
    [ds1,ds2]=size(Ds);
    if ~isreal(C) || ~isreal(Ds) || n3 ~= n || ds1 ~= p || ds2 ~= m
        error('C and D matrices are of incompatible dimensions');
    end
end

%Checking for repetition of eigenvalues / complex eigenvalues
E=sort(eig(A));
if length(pref>1)
    if ~isempty(find(abs((imag(E)>1e-4)&(real(E)<0)),1)) || (length(find(real(E)<0))) ~= length(find(unique(fix(real(E)*1e4)/1e4)<0))
        error('Eigenvalues of A are complex or are repeated')
    end
end

%ch =1
%ch=ch-1 makes ch=0 for Davison model and ch=1 for chidambara model. Thus,
%the dependence of X2 on u is ignored in Davison reduced model and is
%considered in Chidambara reduced model.

if length(pref)==1
    des_order = pref;
    pref = 1:n;
end

%Rearrangement according to preference of states
A=A(pref,pref);
B=B(pref,:);

%Diagonalization
[P,D]=eig(A);
[P,D]=cdf2rdf(P,D);

%Arrangement According to decreasing real(lambda)
[Index,Index] = sort(diag(D),'descend');
I = eye(n);
I = I(Index,:);
P = P*inv(I);

Lambda  = inv(P)*A*P;
Gamma = inv(P)*B;

% Ensuring that all unstable poles are considered in the model and not
% discarded.
min_order = length(find(diag(Lambda)>0));

if ~exist('des_order','var')
    optimal_order = get_optorder(diag(Lambda),min_order);
else
    optimal_order=max(des_order,min_order);
end

A11=A(1:optimal_order,1:optimal_order);
A12=A(1:optimal_order,(optimal_order+1):n);
B1=B(1:optimal_order,:);

P11=P(1:optimal_order,1:optimal_order);
P12=P(1:optimal_order,(optimal_order+1):n);
P21=P((optimal_order+1):n,1:optimal_order);
P22=P((optimal_order+1):n,(optimal_order+1):n);

Lambda2=Lambda((optimal_order+1):n,(optimal_order+1):n);
Gamma2=Gamma((optimal_order+1):n,:);

A_1 = A11+A12*P21*inv(P11);
B_1=B1+A12*(P21*inv(P11)*P12-P22)*inv(Lambda2)*Gamma2;

if ismember(nargin,[4 5])
    C1 = C(:,1:optimal_order);
    C2 = C(:,(optimal_order+1):n);
    C_1 = C1+C2*P21*inv(P11);
    D_1 = Ds+C2*(P21*inv(P11)*P12-P22)*inv(Lambda2)*Gamma2;
    varargout{1}=C_1;
    varargout{2}=D_1;
end

if ismember(nargout,[3 5])
    varargout{nargout-2}=optimal_order;
end
return


function opt_order = get_optorder(LambdaV,min_order)
n=length(LambdaV);
if min_order > 0
    U(1:min_order) = NaN;
    U((min_order+1):n)=((n-(min_order:(n-1))).^.5+1)./abs(LambdaV((min_order:(n-1))+1))';
else
    U=((n-(0:(n-1))).^.5+1)./abs(LambdaV((0:(n-1))+1))';
end

V=U(1:(end-1))./U(2:end);

opt_order = find(V==max(V));
opt_order = opt_order(1);
return