mysoscill

function [Ar,Br,Cr,Dr] = mysoscill(~,m,k,B)
%
% mysoscill(ver,m,k,B) - dynamic system of mechanical setup
%   for control system design. The system describes dynamics
%   of solid bodies with small oscillations.
%
% Input parameters:
%   ver: scalar    ... system structure descriptor
%   m: [1 x 7]     ... weight of each body of the system
%   k: [1 x 7]     ... spring constants of body links
%   B: [7 x 7]     ... matrix of attenuation coefficients
%
% Output parameters:
%   Ar,Br,Cr,Dr    ... Matrix of reduced dynamic system description.

% memory allocation - basic system
% Ar = eye(4,4);
% Br = [0; 0; 0; 1];
% Cr = [eye(2,2), zeros(2,2)];
% Dr = zeros(2,1);


k1 = k(1);
k2 = k(2);
k3 = k(3);
k4 = k(4);
k5 = k(5);
k6 = k(6);
k7 = k(7);

M = diag(m);

K = [k1+k2 -k2 0 0 0 0 0;...
     -k2 k2+k3 -k3 0 0 0 0;...
     0 -k3 k3+k4 -k4 0 0 0;...
     0 0 -k4 k4+k5+k6 -k5 -k6 0;...
     0 0 0 -k5 k5 0 0;...
     0 0 0 -k6 0 k6+k7 -k7;...
     0 0 0 0 0 -k7 k7];


[V,L] = eig(K, M);

Om = diag(L);

F = V'*B*V;
f = diag(F);

As = [0 Om(1) zeros(1,12);...
      -Om(1) -f(1) zeros(1,12);...
      zeros(1,2) 0 Om(2) zeros(1,10);...
      zeros(1,2) -Om(2) -f(2) zeros(1,10);...
      zeros(1,4) 0 Om(3) zeros(1,8);...
      zeros(1,4) -Om(3) -f(3) zeros(1,8);...
      zeros(1,6) 0 Om(4) zeros(1,6);...
      zeros(1,6) -Om(4) -f(4) zeros(1,6);...
      zeros(1,8) 0 Om(5) zeros(1,4);...
      zeros(1,8) -Om(5) -f(5) zeros(1,4);...
      zeros(1,10) 0 Om(6) zeros(1,2);...
      zeros(1,10) -Om(6) -f(6) zeros(1,2);...
      zeros(1,12) 0 Om(7);...
      zeros(1,12) -Om(7) -f(7)];

  Bstmp = [zeros(1,7);...
        V(:,1)';...
        zeros(1,7);...
        V(:,2)';...
        zeros(1,7);...
        V(:,3)';...
        zeros(1,7);...
        V(:,4)';...
        zeros(1,7);...
        V(:,5)';...
        zeros(1,7);...
        V(:,6)';...
        zeros(1,7);...
        V(:,7)'];
%    Bs = Bstmp*[0 0 0 0 1 0 0]';

   Cs = [V(1,1)/Om(1) 0;...
         0 V(1,1);...
         V(1,2)/Om(2) 0;...
         0 V(1,2);...
         V(1,3)/Om(3) 0;...
         0 V(1,3);...
         V(1,4)/Om(4) 0;...
         0 V(1,4);...
         V(1,5)/Om(5) 0;...
         0 V(1,5);...
         V(1,6)/Om(6) 0;...
         0 V(1,6);...
         V(1,7)/Om(7) 0;...
         0 V(1,7)]';

   Ds = [0;0];

   Ar = As(1:4,1:4);
   Br = Bstmp(1:4,:)*[0 0 0 0 1 0 0]';
   Cr = Cs(:,1:4);

   I = As(5:end,5:end)\eye(10);

   Dr = Ds-Cs(:,5:end)*I*Bstmp(5:end,:)*[0 0 0 0 1 0 0]';

end