% Problem 5.24 - Controllability of underwater vehicle
disp('System model:')
A=[ 0.995  0.485  0     -0.004 0     0;
   -0.017  0.942  0     -0.018 0     0;
    0     -0.003  0.995  0.485 0     0;
    0     -0.014 -0.017  0.942 0     0;
    0      0      0      0     1     0.5;
    0      0      0      0     0.002 1]
 
B=[ 0.0258 -0.0002 0;
    0.1023 -0.0010 0;
   -0.0001  0.0258 0;
   -0.0008  0.1023 0;
    0       0      0.0055;
    0       0      0.0221]

disp('Controllability matrix:')
Co=ctrb(A,B)
r_Co=rank(Co)
if r_Co < size(A,1)
  disp('System is not controllable!')
else
  disp('System is controllable!')
end