function [ Ab, Bb, Cb, G, T, Ti ] = bal_real( A, B, C )
% [ Ab, Bb, Cb, G, T, Ti ] = bal_real( A, B, C )
% balanced realization for a continuous time system
%
%   input     A,B,C	 a continuous-time state-space realization
% 
%  output     Ab,Bb,Cb   the balanced continuous-time state-space realization
%             G          the balanced observability and controllability gramians
%             T          coordinate transformation matrix
%             Ti         inverse of coordinate transformation matrix

  P = liap ( A , C );   % solve left  Liapunov eq'n    A'*P + P*A  + C'*C  = 0
  Q = liap ( A , B );   % solve right Liapunov eq'n    A *Q + Q*A' + B *B' = 0
  R = chol(Q);		% right Cholesky factor of Q ... R'*R = Q 

  [U,G2] = eig(R*P*R');	% diagonalization of R*P*R'

% the balanced observability and controllability gramian
  G = sqrt(real(diag(G2)));

% the coordinate transformation matrix ...
  T  = R'*U * diag(1./sqrt(G));

% the inverse of the coordinate transformation matrix ...
  Ti = diag(sqrt(G)) * U' * inv(R');

% sorted grammian ...
  [G,idx] = sort(G,'descend');	

% determine the state re-sorting matrix, S   X_sorted  =  S * X
  n = length(A);
  S = zeros(n);
  for i=1:n
      S(i,idx(i)) = 1;
  end
  Ti = S*Ti;	T  = T/S;	% reorganize the transformation matrices

  Ab = Ti*A*T;			% balanced dynamics matrix
  Bb = Ti*B;			% balanced   input  matrix
  Cb =    C*T;			% balanced  output  matrix

% --------------------------------------------------- H.P. Gavin 2013-10-18

% Frazzoli, Emilio, and Munther Dahleh. 
% 6.241J Dynamic Systems and Control, Spring 2011. 
% (MIT OpenCourseWare: Massachusetts Institute of Technology), 
% http://ocw.mit.edu/courses/electrical-engineering-and-computer-science/6-241j-dynamic-systems-and-control-spring-2011 (Accessed 18 Oct, 2013).
% License: Creative Commons BY-NC-SA

% checks

% zero_1 = A'*P + P*A  + C'*C
% zero_2 = A *Q + Q*A' + B *B'
% zero_3 = R'*R - Q
% zero_4 = R*P*R' - U*G2*U'
% zero_5 = liap(Ab,Bb*Bb') - liap(Ab,Cb'*Cb)
