%Sugeno type fuzzy control for single inverted pendulum
close all;
g=9.8;
m=0.1;
M=1;
l=0.5;
a=1/(m+M);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Equation 1:
a11=g/(4/3*l-a*m*l)
a12=-g*m*l/(M+m)*(4/3*l-a*m*l)
A1=[0 1 0 0;
a11 0 0 0;
0 0 0 1;
a12 0 0 0]
b12=-a/(4/3*l-a*m*l)
b14=4/3*l/(M+m)*(4/3*l-a*m*l)
B1=[0;b12;0;b14]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Equation 2:
x2=200*pi/180;
a21=(g-a*m*l*x2^2)/(4/3*l-a*m*l)
a22=m*l*(4/3*l*x2*x2-g)/(M+m)*(4/3*l-a*m*l)
A2=[0 1 0 0;
a21 0 0 0;
0 0 0 1;
a22 0 0 0]
b22=b12;
b24=b14;
B2=[0;b22;0;b24]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Equation 3:
%x1=-15*pi/180;
x1=15*pi/180;
a31=g/(4/3*l-a*m*l*(cos(x1))^2)
a32=-m*l*g*(cos(x1))/(M+m)*(4/3*l-a*m*l*(cos(x1))^2)
A3=[0 1 0 0;
a31 0 0 0;
0 0 0 1;
a32 0 0 0]
b32=-a*cos(x1)/(4/3*l-a*m*l*(cos(x1))^2)
b34=4/3*l/(M+m)*(4/3*l-a*m*l*(cos(x1))^2)
B3=[0;b32;0;b34]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Equation 4:
x1=15*pi/180;
x2=200*pi/180;
a41=g/(4/3*l-a*m*l*(cos(x1))^2)
a42=-a*m*l*x2*sin(2*x1)*0.5/(4/3*l-a*m*l*(cos(x1))^2)
a43=-m*l*g*(cos(x1))/(M+m)*(4/3*l-a*m*l*(cos(x1))^2)
a44=m*l*(x2*sin(2*x1)*0.5+a*m*l*x2*sin(2*x1)*0.5*cos(x1)/(4/3*l-a*m*l*(cos(x1))^2))/(M+m)
A4=[0 1 0 0;
a41 a42 0 0;
0 0 0 1;
a43 a44 0 0]
b42=b32;
b44=b34;
B4=[0;b42;0;b44]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Equation 5:
x1=-15*pi/180;
x2=200*pi/180;
a51=g/(4/3*l-a*m*l*(cos(x1))^2)
a52=-a*m*l*x2*sin(2*x1)*0.5/(4/3*l-a*m*l*(cos(x1))^2)
a53=-m*l*g*(cos(x1))/(M+m)*(4/3*l-a*m*l*(cos(x1))^2)
a54=m*l*(x2*sin(2*x1)*0.5+a*m*l*x2*sin(2*x1)*0.5*cos(x1)/(4/3*l-a*m*l*(cos(x1))^2))/(M+m)
A5=[0 1 0 0;
a51 a52 0 0;
0 0 0 1;
a53 a54 0 0]
b52=b32;
b54=b34;
B5=[0;b52;0;b54]
P=[-5-5i;-5+5i;-1.5-1.5i;-1.5+1.5i]; %Stable pole point
F1=place(A1,B1,P)
F2=place(A2,B2,P)
F3=place(A3,B3,P)
F4=place(A4,B4,P)
F5=place(A5,B5,P)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tc=newfis('tc','sugeno');
tc=addvar(tc,'input','jiaodu',[-15,15]*pi/180);
tc=addmf(tc,'input',1,'NG','gaussmf',[5,-15]*pi/180);
tc=addmf(tc,'input',1,'ZR','gaussmf',[5,0]*pi/180);
tc=addmf(tc,'input',1,'PO','gaussmf',[5,15]*pi/180);
tc=addvar(tc,'input','jiasudu',[-200,200]*pi/180);
tc=addmf(tc,'input',2,'NG','gaussmf',[50,-200]*pi/180);
tc=addmf(tc,'input',2,'ZR','gaussmf',[50,0]*pi/180);
tc=addmf(tc,'input',2,'PO','gaussmf',[50,200]*pi/180);
tc=addvar(tc,'input','weiyi',[-10,10]);
tc=addmf(tc,'input',3,'NG','gaussmf',[4,-10]);
tc=addmf(tc,'input',3,'ZR','gaussmf',[4,0]);
tc=addmf(tc,'input',3,'PO','gaussmf',[4,10]);
tc=addvar(tc,'input','sudu',[-10,10]);
tc=addmf(tc,'input',4,'NG','gaussmf',[2,-10]);
tc=addmf(tc,'input',4,'ZR','gaussmf',[2,0]);
tc=addmf(tc,'input',4,'PO','gaussmf',[2,10]);
tc=addvar(tc,'output','u',[-100,100]);
tc=addmf(tc,'output',1,'No.1','linear',[F1(1),F1(2),F1(3),F1(4) 0]);
tc=addmf(tc,'output',1,'No.2','linear',[F2(1),F2(2),F2(3),F2(4) 0]);
tc=addmf(tc,'output',1,'No.3','linear',[F3(1),F3(2),F3(3),F3(4) 0]);
tc=addmf(tc,'output',1,'No.4','linear',[F4(1),F4(2),F4(3),F4(4) 0]);
tc=addmf(tc,'output',1,'No.5','linear',[F5(1),F5(2),F5(3),F5(4) 0]);
rulelist=[1 1 1 1 4 1 1;
1 1 1 2 4 1 1;
1 1 1 3 4 1 1;
1 1 2 1 4 1 1;
1 1 2 2 4 1 1;
1 2 3 3 3 1 1;
1 3 2 3 5 1 1;
2 1 2 2 2 1 1;
2 2 1 1 1 1 1;
3 1 2 1 5 1 1;
3 2 3 3 3 1 1;
3 3 1 2 4 1 1];
tc=addrule(tc,rulelist);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model=newfis('model','sugeno');
model=addvar(model,'input','jiaodu',[-15,15]*pi/180);
model=addmf(model,'input',1,'NG','gaussmf',[5,-15]*pi/180);
model=addmf(model,'input',1,'ZR','gaussmf',[5,0]*pi/180);
model=addmf(model,'input',1,'PO','gaussmf',[5,15]*pi/180);
model=addvar(model,'input','jiaosudu',[-200,200]*pi/180);
model=addmf(model,'input',2,'NG','gaussmf',[50,-200]*pi/180);
model=addmf(model,'input',2,'ZR','gaussmf',[50,0]*pi/180);
model=addmf(model,'input',2,'PO','gaussmf',[50,200]*pi/180);
model=addvar(model,'input','weiyi',[-10,10]);
model=addmf(model,'input',3,'NG','gaussmf',[5,-10]);
model=addmf(model,'input',3,'ZR','gaussmf',[5,0]);
model=addmf(model,'input',3,'PO','gaussmf',[5,10]);
model=addvar(model,'input','sudu',[-100,100]);
model=addmf(model,'input',4,'NG','gaussmf',[25,-100]);
model=addmf(model,'input',4,'ZR','gaussmf',[25,0]);
model=addmf(model,'input',4,'PO','gaussmf',[25,100]);
model=addvar(model,'input','u',[-5,5]);
model=addmf(model,'input',5,'Any','gaussmf',[1.5,-5]);
model=addvar(model,'output','d_jiaodu',[-200,200]*pi/180);
model=addmf(model,'output',1,'No.1','linear',[0 1 0 0 0 0]);
model=addmf(model,'output',1,'No.2','linear',[0 1 0 0 0 0]);
model=addmf(model,'output',1,'No.3','linear',[0 1 0 0 0 0]);
model=addmf(model,'output',1,'No.4','linear',[0 1 0 0 0 0]);
model=addmf(model,'output',1,'No.5','linear',[0 1 0 0 0 0]);
model=addvar(model,'output','d_jiaosudu',[-200,200]*pi/180);
model=addmf(model,'output',2,'No.1','linear',[A1(2,1),0,0,0,B1(2),0]);
model=addmf(model,'output',2,'No.2','linear',[A2(2,1),0,0,0,B2(2),0]);
model=addmf(model,'output',2,'No.3','linear',[A3(2,1),0,0,0,B3(2),0]);
model=addmf(model,'output',2,'No.4','linear',[A4(2,1),A4(2,2),0,0,B4(2),0]);
model=addmf(model,'output',2,'No.5','linear',[A5(2,1),A5(2,2),0,0,B5(2),0]);
model=addvar(model,'output','d_weiyi ',[-10,10]);
model=addmf(model,'output',3,'No.1','linear',[0 0 0 1 0 0]);
model=addmf(model,'output',3,'No.2','linear',[0 0 0 1 0 0]);
model=addmf(model,'output',3,'No.3','linear',[0 0 0 1 0 0]);
model=addmf(model,'output',3,'No.4','linear',[0 0 0 1 0 0]);
model=addmf(model,'output',3,'No.5','linear',[0 0 0 1 0 0]);
model=addvar(model,'output','d_sudu',[-100,100]);
model=addmf(model,'output',4,'No.1','linear',[0,A1(4,2),0,0,B1(4),0]);
model=addmf(model,'output',4,'No.2','linear',[A2(4,1),A2(4,1),0,0,B2(4),0]);
model=addmf(model,'output',4,'No.3','linear',[0,A3(4,2),0,0,B3(4),0]);
model=addmf(model,'output',4,'No.4','linear',[A4(4,1),A4(4,2),0,0,B4(4),0]);
model=addmf(model,'output',4,'No.5','linear',[A5(4,1),A5(4,2),0,0,B5(4),0]);
rulelist1=[1 1 1 1 0 4 4 4 4 1 1;
1 2 1 2 0 3 3 3 3 1 1;
1 3 1 3 0 5 5 5 5 1 1;
2 1 2 1 0 2 2 2 2 1 1;
2 2 2 2 0 1 1 1 1 1 1;
2 3 2 3 0 2 2 2 2 1 1;
3 1 3 1 0 5 5 5 5 1 1;
3 2 3 2 0 3 3 3 3 1 1;
3 3 3 3 0 4 4 4 4 1 1];
model=addrule(model,rulelist1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ts=0.020;
x=[0.20;0;-0.5;0]; %Initial state
for k=1:1:500
time(k)=k*ts;
u(k) =(-1)*evalfis([x(1),x(2),x(3),x(4)],tc); %Using feedback control
k0=evalfis([x(1),x(2),x(3),x(4),u(k)],model)'; %Using fuzzy T-S model
x=x+ts*k0;
y1(k)=x(1);
y2(k)=x(2);
y3(k)=x(3);
y4(k)=x(4);
end
figure(1);
plot(time,y1),grid on;
xlabel('time(s)'),ylabel('Angle');
figure(2);
plot(time,y2),grid on;
xlabel('time(s)'),ylabel('Angle rate');
figure(3);
plot(time,y3),grid on;
xlabel('time(s)'),ylabel('position');
figure(4);
plot(time,y4),grid on;
xlabel('time(s)'),ylabel('rate');
figure(5);
plot(time,u),grid on;
xlabel('time(s)'),ylabel('control force');
figure(6);
plotmf(tc,'input',1);
ylabel('角度隶属度函数');
figure(7);
plotmf(tc,'input',2);
ylabel('角速度隶属度函数');
figure(8);
plotmf(tc,'input',3);
ylabel('位移隶属度函数');
figure(9);
plotmf(tc,'input',4);
ylabel('速度隶属度函数');
showrule(tc);
showrule(model);
