function [Ak,Bk,Ck,Dk,T] = KalmanDcmp(A,B,C,D, tol)
% [Ak,Bk,Ck,Dk] = KalmanDcmp(A,B,C,D, tol)
% Transform a state-space realization, (A,B,C,D) into 
% Kalman Canonical Form in states: [Xco Xc_o, X_co, X_c_o], where states:
%   Xco   are    controllable and    observable
%   Xc_o  are    controllable and un-observable
%   X_co  are un-controllable and    observable
%   X_c_o are un-controllable and un-observable
%
% tol : tolerance on singular value ratio, sigma_n/sigma_1, to 
%       determine the null space from SVD,   default = 1e-9 

if nargin < 5,  tol = 1e-9;  end

[n,r,m] = abcddim(A,B,C,D);

[Up,Sp,Vp] = svd(obsv(A,C,n)); % singular value decomp. of observability matrix
[Uq,Sq,Vq] = svd(ctrb(A,B,n)); % singular value decomp. of controllability matrix

figure(101); 
 clf;
 semilogy([1:n],diag(Sq)/Sq(1,1),'x', [1:n],diag(Sp)/Sp(1,1),'o');
  ylabel('\sigma_i / \sigma_1')
%plot([1:n],cumsum(diag(Sq))/sum(diag(Sq)),'x', [1:n],cumsum(diag(Sp))/sum(diag(Sp)),'o');
% ylabel('(cumulative sum of  \sigma_i) / (sum of  \sigma_i)')
  legend('controllability', 'observability'); 

no = sum( diag(Sp)/Sp(1,1) > tol )  % dimension of the observable   subspace
nc = sum( diag(Sq)/Sq(1,1) > tol )  % dimension of the controllable subspace

if (no == n && nc == n) , Ak = A; Bk = B; Ck = C; Dk = D; return; end

Bo  = Vp(:,1:no);                   % basis for the    observable   subspace
B_o = Vp(:,no+1:end);               % basis for the un-observable   subspace

Bc  = Uq(:,1:nc);                   % basis for the    controllable subspace
B_c = Uq(:,nc+1:end);  % not needed % basis for the un-controllable subspace

%  orthornomal bases for the Kalman decomposition  
%                                                    ctrlbl      obsvbl
%v = null([Bc , -B_o], tol);  Bc_o = Bc * v(1:nc,:);  
 Bc_o  = Bc  * null(Bo'   * Bc  , tol );          %   Y             N
 Bco   = Bc  * null(Bc_o' * Bc  , tol );          %   Y           Y    
 B_c_o = B_o * null(Bc'   * B_o , tol );          %     N           N    Bc_o ??
 B_co  = null ( [ Bco , Bc_o , B_c_o ]' , tol );  %     N         Y

 T = [ Bco , Bc_o , B_co , B_c_o ];  % coordinate transformation x = T xk

Ak = T \ A * T;
Bk = T \ B;
Ck = C * T;
Dk = D;

% =========================================================================
% H.P. Gavin, 2017-11-05
nco   = size(Bco,2) 
nc_o  = size(Bc_o,2) 
n_co  = size(B_co,2) 
n_c_o = size(B_c_o,2) 

