clear variables
%
l    = 0.3226;
r    = 0.3;
m    = 0.04;
Jp   = 0.0003;
zeta = 0.0005;
Jb   = 0.01;
f    = 0.01;
g    = 9.81;
%
E  = [1 0 0 0; 0 Jb+m*r^2 0 m*r*l; 0 0 1 0; 0 m*r*l 0 Jp+m*l^2];
A  = [0 1 0 0; 0 -f 0 0; 0 0 0 1; 0 0 -g*m*l -zeta];
B  = [0; 1; 0; 0];
C  = [1 0 0 0; 0 0 1 0]; % y = [\phi (shaft); \theta (pendulum)]
P = ss(E\A,E\B,C,0);
clear E A B C l r m Jp zeta Jb f g
%
lect4 = 1;
[A,B,C,D] = ssdata(P);
[n,m] = size(B);
if lect4
    Q = diag([10 6 0 0]);
    % Q = diag([1 0.1 1 0]);
    z0 = 3;
else
    z0 = [-2 -2 -3];
    % z0 = 5*[-1 -1+i/2 -1-i/2];
    Cz = acker(A,B,roots(poly(eig(A))+[zeros(1,n-length(z0)) poly(z0)]));
    Cz = Cz / Cz(1);
    Q = Cz'*Cz;
end
R = eye(m)*1e1;
K = lqr(A,B,Q,R); K = -K;
%
% plots
%
x0 = [deg2rad(90); 0; 0; 0];
t = linspace(0,10,10001);
y = rad2deg(initial(ss(A+B*K,B,C,0),x0,t))';
u = initial(ss(A+B*K,B,K,0),x0,t)';
%
figure(1)
plot(t,y(2,:),'LineWidth',2)
xlabel('Time, $t$','FontSize',16,'Interpreter','latex')
ylabel('Pendulum angle, $\theta(t)$','FontSize',16,'Interpreter','latex')
%
figure(2)
plot(t,y(1,:),'LineWidth',2)
xlabel('Time, $t$','FontSize',16,'Interpreter','latex')
ylabel('Shaft angle, $\phi(t)$','FontSize',16,'Interpreter','latex')
%
figure(3)
plot(t,u,'LineWidth',2)
xlabel('Time, $t$','FontSize',16,'Interpreter','latex')
ylabel('Shaft torque, $\tau_{\mathrm m}(t)$','FontSize',16,'Interpreter','latex')
%
figure(4)
chicl = eig(A+B*K);
RL = rlocus(ss([A zeros(n); Q -A'],[B; zeros(n,m)],[zeros(m,n) -B'],0))';
RL = RL(:,real(RL(end-10,:))<0);
plot(real(RL),imag(RL),'LineWidth',2)
hold on
plot(real(RL(1,:)),imag(RL(1,:)),'x','LineWidth',3,'MarkerSize',10,'Color',[0.5,0.5,0.5])
plot(real(RL(end,:)),imag(RL(end,:)),'o','LineWidth',2,'MarkerSize',10,'Color',[0.5,0.5,0.5])
plot(real(chicl),imag(chicl),'.','LineWidth',2,'MarkerSize',20,'Color',[0.7,0.2,0.2])
plot(-real(RL),imag(RL),'LineWidth',2)
plot(-real(RL(1,:)),imag(RL(1,:)),'x','LineWidth',3,'MarkerSize',10,'Color',[0.5,0.5,0.5])
plot(-real(RL(end,:)),imag(RL(end,:)),'o','LineWidth',2,'MarkerSize',10,'Color',[0.5,0.5,0.5])
hold off
axis([-3*abs(z0(end)) 3*abs(z0(end)) -2.2*abs(z0(end)) 2.2*abs(z0(end))])
% axis('equal')