clc;clear;
M = 1.77; %copter mass
m = 0.622; %payload mass
r = 2; %length of rope
R = r;
g = 9.81; %gravitation constant
l = 0.55; %frame size
I = 0.05; %copter rotational inertia


A = [ 0 0 0 0 0 -g*m*r/(M*M*(r+1/m)+M*(m*r+2)+m);
      1 0 0 0 0 0;
      0 0 0 0 -2*m*r/(M+m) -g*m*r/(M*M*(r+1/m)+M*(m*r+2)+m);
      0 0 1 0 0 0;
      0 0 0 0 0 -g/(1+M*r+M/m);
      0 0 0 0 1 0];
B = [ (m*r+1)/(M*r*m+m+M) 0;
      0 0;
      0 1/(M+m);
      0 0;
      -1/(1+M*r+M/m) 0;
      0 0];
C = eye(6);
D = zeros(6,2);

sys = ss(A,B,C,D);
pol = pole(sys)
delay_sys = pi/-imag(pol(end))
