% Sumit Tripathi
% University at Buffalo
function System_analysis_PROJECT_f()
%Controllability analysis, controller design by pole placement, 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;
% Read The project report to understand the Ac , Bc matrices
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]
lemda=eig(Ac) % Test whether all the eigenvalues are negative
%pause
C_control=[Bc Ac*Bc (Ac^2)*Bc (Ac^3)*Bc];
[u,s,v] = svd(C_control)
rank_C=rank(C_control)% rank_C must me 4 for system to be fully controllable
n=length(Ac)
I=eye(n)
Ad=expm(Ac*0.01)
[Ad,Bd]=c2d(Ac,Bc,0.01)
[E,V]=eig(Ad)
C_control=[Bd Ad*Bd (Ad^2)*Bd (Ad^3)*Bd]
format long;
[u,s,v] = svd(C_control)% The number of non zero singular values corrsponds to rank of the matrix
format short
rank_C=rank(C_control)
m=0;
Wdc=(Ad^m)*Bd*Bd'*(Ad')^m; % Calcuate controllability gramian
format long
for m=1:100
Wdc=Wdc+(Ad^m)*Bd*Bd'*(Ad')^m;
end
Wdc
Lemda=eig(Wdc)%Since, all the eigenvalues of Wdc must be positive so that
%all the states of the state-space model can be controlled from input.
% Lyapunov Stability test
%% Par matrix is calculated from MAPLE, please see the report for more
%% detail
format short
syms P_s p11 p12 p13 p14 p21 p22 p23 p24 p31 p32 p33 p34 p41 p42 p43 p44;
P_s=[p11,p12,p13,p14;
p12,p22,p23,p24;
p13,p23,p33,p34;
p14,p24,p34,p44];
Liapunov_fun=(Ad'*P_s*Ad)-P_s;
Par=[0.729e-5 0.542376e-2 0.277830e-2 0.465264e-2 0.881936e-2 1.03352760 1.73078208 .26471025 .88658640 .74235456;
0.8100e-4 -0.1620e-4 0.1436760e-1 -0.311580e-2 8.1*10^(-7) -0.143676e-2 0.31158e-3 -.36287676 -.27633684 0.2996361e-1;
0.2430e-4 0.903717e-2 0.678564e-2 0.728703e-2 -0.90396e-3 -.19875097 -.17463708 .41067390 .59866917 -.14914296;
0 0.27e-4 0.729e-5 0.271188e-2 0.10044e-1 0.785688e-2 0.1743536e-1 0.138915e-2 .51909012 .86539104;
0.2430e-4 0.903717e-2 0.678564e-2 0.728703e-2 -0.90396e-3 -.19875097 -.17463708 .41067390 .59866917 -.14914296;
0 0.90e-4 0.2430e-4 0.903960e-2 -0.9e-5 0.797957e-2 -0.263496e-2 0.215514e-2 -.19875529 -.17386164;
0 0.27e-4 0.729e-5 0.271188e-2 0.10044e-1 0.785688e-2 0.1743536e-1 0.138915e-2 .51909012 .86539104;
0 0.90e-4 0.2430e-4 0.903960e-2 -0.9e-5 0.797957e-2 -0.263496e-2 0.215514e-2 -.19875529 -.17386164;
0.27e-2 0.44e-2 .5145 .8616 -0.1e-3 -0.54e-4 -0.20088e-1 -0.729e-5 -0.542376e-2 -0.881936e-2;
0.90e-2 -0.9e-3 -.2018 -.1731*p14 0 0 0 0 0 0;];
Val=[-1; -1; 0 ; 0; 0 ;0 ;0 ; 0 ; 1 ; 0];
P_n=Par\Val;
P_n=double(P_n)
P=[P_n(1), P_n(2), P_n(3), P_n(4);
P_n(2), P_n(5), P_n(6), P_n(7);
P_n(3), P_n(6), P_n(8), P_n(9);
P_n(4), P_n(7), P_n(9), P_n(10)]
[E,V]=eig(P);
x=E(:,1);
test=x'*P*x % To find out whether P is positive definite
T=0:0.01:.8;
U=zeros(size(T));
U(1)=1;
X(:,1)=[0 0 0 0]'
for k=2:length(T)
X(:,k)=Ad*X(:,k-1)+Bd*U(k-1);
Y(:,k-1)=C*X(:,k-1)+D*U(k-1);
end
Y(:,k)=C*X(:,k)+D*U(k);
X=X';
Y=Y';
figure
plot(T,Y(:,1),'--',T,Y(:,2),'-')
legend('Motor','Pendulum')
ylabel('Radians');
xlabel('Time(Sec)');
title('Impulse response of Original system')
%pause
%Calculating optimum pole positions based on Reciprocal of Condition Number of
%Controllability matrix
p_ratio1 = [1.2:0.1:10]; % Specify the range of ratios of location of poles p1,p2 and p4,p3
p_ratio2 = [1.2:0.1:9];
p1=0.9; p4=0.94;
I=eye(4);
len1=length(p_ratio1);
len2=length(p_ratio2);
for m1=1:len1
p2=p1/p_ratio1(m1);
for m2=1:len2
p3=p4/p_ratio2(m2);
epsilon=[inv(Ad-p1*I)*Bd, inv(Ad-p2*I)*Bd, inv(Ad-p3*I)*Bd, inv(Ad-p4*I)*Bd];
K=[1 1 1 1]*inv(epsilon); % Derived using Ackerman's Formula
Ad_n=Ad-Bd*K; % The new system 'A' matrix
Cblty=[Bd Ad_n*Bd (Ad_n^2)*Bd (Ad_n^3)*Bd];
[u,s,v] = svd(Cblty);
s_diag=diag(s);
s_min=min(s_diag);
s_max=max(s_diag);
R_Cond_n(m1,m2)=abs(s_min/s_max);% Calculate Reciprocal of Condition Number of Controllability Matrix
x(m1,m2)=p_ratio1(m1);
y(m1,m2)=p_ratio2(m2);
end
end
min_R=min(min(R_Cond_n))
max_R=max(max(R_Cond_n))
figure
surf(x,y,R_Cond_n)
xlabel('Ratio of pole P1/P2');
ylabel('Ratio of pole P4/P3');
zlabel('Reciprocal Condition No of Controllability Matrix');
L=.2:.1:.95;
I=eye(4);
len1=length(L);
R_Cn_opt=0;
I=eye(4);
p1=0.9; p2=0.09; p3=0.1056; p4=0.94;
epsilon=[inv(Ad-p1*I)*Bd, inv(Ad-p2*I)*Bd, inv(Ad-p3*I)*Bd, inv(Ad-p4*I)*Bd];
K=[1 1 1 1]*inv(epsilon)
X=[];
Y=[];
X(:,1)=[0 0 0 0]';
% Calculate Impulse response
for k=2:length(T)
ctrl_in(k-1)=(-K*X(:,k-1));
X(:,k)=Ad*X(:,k-1)+Bd*(-K*X(:,k-1))+Bd*U(k-1);
Y(:,k-1)=C*X(:,k-1)+D*U(k-1);
end
ctrl_in(k)=(-K*X(:,k));
X(:,k)=Ad*X(:,k-1)+Bd*(-K*X(:,k-1))+Bd*U(k-1);
Y(:,k)=C*X(:,k)+D*U(k);
X=X';
Y=Y';
figure
plot(T,Y(:,1),'--',T,Y(:,2),'-')
legend('Motor','Pendulum');
ylabel('Radians');
xlabel('Time(Sec)');
title('Impulse response of Motor and Pendulum angle');
figure
ctrl_in=ctrl_in';
plot(T(1:length(T)/2),ctrl_in(1:length(T)/2));
title('Control Effort')
ylabel('Motor operating Voltage input')
xlabel('Time(Sec)')
Ad_new=Ad-Bd*K
% Check if the new system has all eigenvalues within unit circle
[EVec,lemda]=eig(Ad_new)
