Feedback control

% Feedback control
% Input from example 2-3
sympref('FloatingPointOutput',true)
clear
clc
%% Input:

A = [0 1 0; 0 0 1; 0 -36 -15]       % A matrix
B = [0; 0; 1]                       % B matrix
Ts = 0;                             % Time settling
Tp = 0.3;                           % Time peak
Os = 0.05;                          % Overshoot (100%=1.0)
zero = -10;                         % Open loop zero

%% Process:

syms s k1 k2 k3;
zeta = -(log(Os)/log(exp(1)))/sqrt(pi^2+(log(Os)/log(exp(1)))^2);
if(Tp==0) Wn = 4/(zeta*Ts);
else Wn =  pi/(Tp*sqrt(1-zeta^2));
end

Anew = A-(B*[k1 k2 k3])

Ds = expand((s^2 + 2*zeta*Wn*s + Wn^2)*(s-zero))
Ts = det(s*eye(3)-Anew)

% Solve
Dsm = coeffs(Ds,s);
Tsm = coeffs(Ts,s);
k = vpa(struct2cell((solve(Dsm==Tsm))))