% Sumit Tripathi
% University at Buffalo
function System_analysis_optimal()
%Finding gain value for state feedback with Quadratic Optimal control.
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)
T=0:0.01:2;
U=zeros(size(T));
U(1)=1;
qv=4;
for i=1:3
qv=qv*4;
Q=[qv 0 0 0;
0 qv*2 0 0;
0 0 0.5 0;
0 0 0 0.5;]; % Weighting matrix for theta, alpha and theta_dot, alpha_dot
R=[1];% Weighting matrix for input vector
P=zeros(4,4);
P=Q+ Ad'*P*Ad - Ad'*P*Bd*inv(R+Bd'*P*Bd)*Bd'*P*Ad;
delta=10;
while delta>.001
P1=P;
P=Q+ Ad'*P*Ad - Ad'*P*Bd*inv(R+Bd'*P*Bd)*Bd'*P*Ad;
delta=norm(P-P1);
end
K=inv(R+Bd'*P*Bd)*Bd'*P*Ad
X=[];
Y=[];
X(:,1)=[0 0 0 0]';
% Calculate the Impulse response of Controlled system
for k=2:length(T)
ctrl_in(k-1)=(-K*X(:,k-1)); % Calculate Control effort
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(i)
subplot(2,1,1)
plot(T,Y(:,1),'--',T,Y(:,2),'-')
legend('Motor','Pendulum');
ylabel('Radians');
xlabel('Time(Sec)');
title(sprintf('%s%d','Impulse response of Motor and Pendulum angle: qv=',qv));
figure(i)
subplot(2,1,2)
ctrl_in=ctrl_in';
plot(T,ctrl_in);
title(sprintf('%s%d','Control Effort: qv=',qv));
ylabel('Motor operating Voltage input')
xlabel('Time(Sec)')
end