System_analysis_deadBeat()

% Sumit Tripathi
% University at Buffalo
function System_analysis_deadBeat()
%Dead beat model, Lyapunov stability test
clc
clear all
close all
format short
%%Parameters for SRV-02 Rotary Inverted Pendulum from Quanser
Beq=5.0E-3;
ng=0.9; nm=0.69; Jeq=2.0E-3;Jm=3.87E-7; Kg=70;
Km=7.67E-3; Kt=7.67E-3;Rm=2.6;
r=0.2; L=0.35/2;
m=0.128;
g=9.8;
a=Jeq + m*r^2;
b=m*L*r;
c=4/3*m*L^2;
d=m*g*L;
E=a*c-(b^2);
G=((nm*ng*Kt*Km*Kg^2) + (Beq*Rm))/Rm;

Ac=[ 0     0     1     0;
    0     0     0     1;
    0 b*d/E  -c*G/E   0;
    0 a*d/E  -b*G/E   0]
%keyboard
Bc=[0;0;c*nm*ng*Kt*Kg/Rm/E;b*nm*ng*Kt*Kg/Rm/E]
C = [1 0 0 0;
     0 1 0 0]
D = [0;0]
[Ad,Bd]=c2d(Ac,Bc,0.01)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Dead Beat Model
a4=0.7984; a3=-3.403; a2=5.4116; a1=-3.8070;
M=[Bd Ad*Bd (Ad^2)*Bd (Ad^3)*Bd];
W=[a3 a2 a1 1;
     a2 a1 1  0;
     a1 1  0  0;
     1  0  0  0;];
Tm=M*W; % Transformation Matrix
K_dbt=[-a4,-a3,-a2,-a1]*inv(Tm); %Gain for Dead Beat Model
A_hat=inv(Tm)*Ad*Tm;
B_hat=inv(Tm)*Bd;
A_dbt=A_hat-B_hat*K_dbt*Tm
A_dbt^2
A_dbt^3
A_dbt^4 % Must go to zero in four steps
X=[];Y=[];ctrl_in=[];
X(:,1)=[0 0 0 0]'
T=0:0.01:0.08;
U=zeros(size(T));
U(1)=1;
for k=2:length(T)
    ctrl_in(k-1)=(-K_dbt*X(:,k-1)); % Calculate control effort
    X_hat=A_dbt*X(:,k-1)+ B_hat*U(k-1);
    X(:,k)=Tm*X_hat; % Convert back to original coordinates
    Y(:,k-1)=C*X(:,k-1)+D*U(k-1);
end
ctrl_in(k)=(-K_dbt*X(:,k));
Y(:,k)=C*X(:,k)+D*U(k);
X=X';
Y=Y';
% keyboard
figure
plot(T,Y(:,1),'--',T,Y(:,2),'-')
legend('Motor','Pendulum')
ylabel('Radians');
xlabel('Time(Sec)');
title({'Impulse response of Motor and Pendulum angle:';'Dead Beat Model'});
figure
ctrl_in=ctrl_in';
plot(T,ctrl_in);
title('Control Effort')
ylabel('Motor operating Voltage input')
xlabel('Time(Sec)')

% Lyapunov stability test
% Par matrix is calculated from MAPLE
Par_db=[-1     0       0       0     1.*10^(-8)  4.*10^(-8)  6.*10^(-8)  4.*10^(-8)   1.2*10^(-7)  9.*10^(-8);
         0   -.9999 0.2e-3  0.3e-3    0            0           0            0            0            0;
         0     0     -1       0     0.1e-3      0.2e-3       0.3e-3         0            0            0;
         0     0      0      -1         0      0.1e-3       0.2e-3      0.3e-3           0            0;
         1     0      0       0       -1         0             0            0            0            0;
         0     0      0       0        1         0             0           -1            0            0;
         0     0      0       0        0         0             0            1            0           -1;
         0  -.9999  0.2e-3  0.3e-3     0         0             0            0            0            0;
         0     1      0       0        0         -1            0            0            0            0;
         0     0     -1       1     0.1e-3    0.1e-3        0.3e-3       -0.2e-3     -0.3e-3          0;];
         

Val_db=[-1; 0; 0 ; 0; -1 ;-1 ;-1 ; 0 ; 0 ; 0];
P_n_db=Par_db\Val_db
P_n_db=double(P_n_db)
P_db=[P_n_db(1), P_n_db(2), P_n_db(3), P_n_db(4);
       P_n_db(2), P_n_db(5), P_n_db(6), P_n_db(7);
       P_n_db(3), P_n_db(6), P_n_db(8), P_n_db(9);
       P_n_db(4), P_n_db(7), P_n_db(9), P_n_db(10)]
[E,V]=eig(P_db)% Must be positive definite