function Q = ctrb(A,B,p)
% Q = ctrb(A,B,p)
% Form the controllability matrix Q = [B AB A^2B A^3B ...  A^(p-1)B ]
% for the discrete time system, x(k+1) = A*x(k) + B*u(k)

 [n,r] = size(B);				% number of states and inputs
 if nargin < 3, p = n; else p = max(p,n); end	% number of blocks in Q
 Q = zeros(n, r*p);				% dimension of Q
 Q(:,1:r) = B;					% first block is input matrix
 for k=1:p-1
        AkB = Q(:,(k-1)*r+1:(k-1)*r+r);
        Q(:,k*r+1:k*r+r) =  A * AkB;
 end
% --------------------------------------------------------------------- CTRB 
