Skip to content

Commit 062c761

Browse files
fumikirifumikiri
committed
init commit
1 parent 03a00c9 commit 062c761

6 files changed

+345
-0
lines changed

‎ExampleTextConstrainssim.m

+100
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
clear
2+
clc
3+
close all
4+
5+
% E = [2 -1
6+
% -1 1];
7+
% F = [-1
8+
% 0]
9+
%
10+
% M = [-1 0
11+
% 0 -1
12+
% 3 2]
13+
% gamma = [0
14+
% 0
15+
% 4]
16+
% x0 = -E\F
17+
%
18+
%
19+
% eta=QPhild(E,F,M,gamma)
20+
21+
22+
23+
24+
Ts=2e-1; % Sampling time 1ms
25+
Tsim=50; % Simulation time 20s
26+
t=0:Ts:Tsim-1*Ts;
27+
Nsim=Tsim/Ts;
28+
29+
30+
31+
% Plant
32+
plant = tf(10,[1 0.1 3]);
33+
[A B C D]=tf2ss(plant.num{1},plant.den{1})
34+
sys=ss(A,B,C,D);
35+
plantd = c2d(sys,Ts)
36+
xplant(1:2,1:Nsim)=0;
37+
yplant(1,1:Nsim)=0;
38+
39+
[out,state] = size(plantd.C);
40+
[~,in] = size(plantd.B);
41+
42+
43+
Np = 25;
44+
Nc = 10;
45+
Rs = 1;
46+
Rw = 4e-2*1;
47+
Q= 1
48+
Rsbar=ones(Np,1);
49+
[F,phi,phi_phi,phi_f,phi_r] = mpcgainOrigin(plantd.A,plantd.B,plantd.C,Nc,Np,Rs,Q)
50+
[nnn mmm]=size(phi_phi)
51+
Umax = 1;
52+
Umin = -1;
53+
Ymax = 1.3;
54+
Ymin = -0.1;
55+
56+
[bound_ofUmax,bound_ofUmin,bound_ofYmax,bound_ofYmin,H,M1,M3,C1] = foropt(phi,phi_phi,Rw,Umax,Umin,Ymax,Ymin,in,out,Np,Nc);
57+
58+
59+
ref(1:Nsim/2) = 0;
60+
ref(Nsim/4+1:Nsim/4*3) = 1;
61+
ref(Nsim/4*3+1:Nsim) = 0;
62+
63+
u(1:Nsim) = 0;
64+
65+
% Simulation
66+
for i=2:Nsim-1
67+
Xh = [(xplant(:,i)-xplant(:,i-1));yplant(1,i-1)];
68+
deltaU=((phi_phi+eye(nnn,mmm)*Rw)\phi'*(Rsbar*ref(i)-F*Xh));
69+
70+
N1=[-bound_ofUmin+C1*u(i-1);bound_ofUmax-C1*u(i-1)];
71+
N3=[-bound_ofYmin+F*Xh;bound_ofYmax-F*Xh];
72+
73+
74+
A_cons = [M1;M3];
75+
b = [N1;N3];
76+
H = 2*(phi_phi+eye(nnn,mmm)*Rw);
77+
f = -2*phi'*(Rsbar*ref(i)-F*Xh);
78+
eta=QPhild(H,f,A_cons,b);
79+
u(i) = u(i-1)+([1 zeros(1,Nc-1)])*eta;
80+
%
81+
%deltau=([1 zeros(1,Nc-1)])*((phi_phi+eye(nnn,mmm)*Rw)\phi'*(Rsbar*ref(i)-F*Xh));
82+
83+
%u(i)=u(i-1)+deltau;
84+
85+
%u(i) = ref(i);
86+
xplant(:,i+1)=plantd.A*xplant(:,i)+plantd.B*u(i);
87+
yplant(1,i)=(plantd.C*xplant(:,i)+plantd.D*u(i));
88+
end
89+
90+
91+
figure;
92+
subplot(2,1,1)
93+
plot(t,yplant(1,:))
94+
subplot(2,1,2)
95+
plot(t,u)
96+
97+
98+
99+
100+

‎ExampleTextConstrainssim2.m

+106
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
clear
2+
clc
3+
close all
4+
5+
% E = [2 -1
6+
% -1 1];
7+
% F = [-1
8+
% 0]
9+
%
10+
% M = [-1 0
11+
% 0 -1
12+
% 3 2]
13+
% gamma = [0
14+
% 0
15+
% 4]
16+
% x0 = -E\F
17+
%
18+
%
19+
% eta=QPhild(E,F,M,gamma)
20+
21+
22+
23+
24+
Ts=2e-1; % Sampling time 1ms
25+
Tsim=50; % Simulation time 20s
26+
t=0:Ts:Tsim-1*Ts;
27+
Nsim=Tsim/Ts;
28+
29+
30+
31+
% Plant
32+
plant = tf(10,[1 0.1 3]);
33+
[A B C D]=tf2ss(plant.num{1},plant.den{1})
34+
sys=ss(A,B,C,D);
35+
plantd = c2d(sys,Ts)
36+
xplant(1:2,1:Nsim)=0;
37+
yplant(1,1:Nsim)=0;
38+
39+
[out,state] = size(plantd.C);
40+
[~,in] = size(plantd.B);
41+
42+
43+
Np = 25;
44+
Nc = 10;
45+
Rs = 1;
46+
Rw = 4e-2*1;
47+
Q= 1
48+
Rsbar=ones(Np,1);
49+
[F,phi,phi_phi,phi_f,phi_r] = mpcgainOrigin(plantd.A,plantd.B,plantd.C,Nc,Np,Rs,Q)
50+
[nnn mmm]=size(phi_phi)
51+
Umax = 1;
52+
Umin = -1;
53+
Ymax = 1.3;
54+
Ymin = -0.1;
55+
deltaumax = 0.5;
56+
deltaumin = -1;
57+
58+
[bound_ofUmax,bound_ofUmin,bound_ofYmax,bound_ofYmin,H,M1,M3,C1] = foropt(phi,phi_phi,Rw,Umax,Umin,Ymax,Ymin,in,out,Np,Nc);
59+
60+
61+
ref(1:Nsim/2) = 0;
62+
ref(Nsim/4+1:Nsim/4*3) = 1;
63+
ref(Nsim/4*3+1:Nsim) = 0;
64+
65+
u(1:Nsim) = 0;
66+
deltau(1:Nsim) = 0;
67+
68+
69+
% Simulation
70+
for i=2:Nsim-1
71+
Xh = [(xplant(:,i)-xplant(:,i-1));yplant(1,i-1)];
72+
%deltaU=((phi_phi+eye(nnn,mmm)*Rw)\phi'*(Rsbar*ref(i)-F*Xh));
73+
74+
75+
[M2, N2] = optimizationWithConstrains2(deltaumax,deltaumin,Np,Nc);
76+
A_cons = [M2];
77+
b = [N2];
78+
H = 2*(phi_phi+eye(nnn,mmm)*Rw);
79+
f = -2*phi'*(Rsbar*ref(i)-F*Xh);
80+
eta=QPhild(H,f,A_cons,b);
81+
deltau(i)=([1 zeros(1,Nc-1)])*eta;
82+
u(i) = u(i-1)+deltau(i);
83+
%
84+
%deltau=([1 zeros(1,Nc-1)])*((phi_phi+eye(nnn,mmm)*Rw)\phi'*(Rsbar*ref(i)-F*Xh));
85+
86+
%u(i)=u(i-1)+deltau;
87+
88+
%u(i) = ref(i);
89+
xplant(:,i+1)=plantd.A*xplant(:,i)+plantd.B*u(i);
90+
yplant(1,i)=(plantd.C*xplant(:,i)+plantd.D*u(i));
91+
end
92+
93+
94+
figure;
95+
subplot(3,1,1)
96+
hold on
97+
plot(t,yplant(1,:))
98+
plot(t,ref(1,:))
99+
subplot(3,1,2)
100+
plot(t,u)
101+
subplot(3,1,3)
102+
plot(t,deltau)
103+
104+
105+
106+

‎QPhild.m

+41
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
function eta=QPhild(H,f,A_cons,b)
2+
% E=H;
3+
% F=f;
4+
% M=A_cons;
5+
% gamma=b;
6+
% eta =x
7+
[n1,m1]=size(A_cons);
8+
eta=-H\f;
9+
kk=0;
10+
for i=1:n1
11+
if (A_cons(i,:)*eta>b(i)) kk=kk+1;
12+
else
13+
kk=kk+0;
14+
end
15+
end
16+
if (kk==0)
17+
return;
18+
end
19+
20+
P=A_cons*(H\A_cons');
21+
d=(A_cons*(H\f)+b);
22+
[n,m]=size(d);
23+
x_ini=zeros(n,m);
24+
lambda=x_ini;
25+
al=10;
26+
for km=1:38
27+
%find the elements in the solution vector one by one
28+
% km could be larger if the Lagranger multiplier has a slow
29+
% convergence rate.
30+
lambda_p=lambda;
31+
for i=1:n
32+
w= P(i,:)*lambda-P(i,i)*lambda(i,1);
33+
w=w+d(i,1);
34+
la=-w/P(i,i);
35+
lambda(i,1)=max(0,la);
36+
end
37+
al=(lambda-lambda_p)'*(lambda-lambda_p);
38+
if (al<10e-8); break; end
39+
end
40+
41+
eta=-H\f -H\A_cons'*lambda;

‎foropt.m

+32
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
function [bound_ofUmax,bound_ofUmin,bound_ofYmax,bound_ofYmin,H,M1,M3,C1] = foropt(phi,phi_phi,R,Umax,Umin,Ymax,Ymin,in,out,Np,Nc)
2+
if size(Umax)~=0
3+
bound_ofUmin=[];
4+
bound_ofUmax=[];
5+
for j=1:Nc
6+
bound_ofUmin((j-1)*in+1:j*in,:)=Umin;%[-40.68;-2.102;0];%[-500;-500;-500]
7+
bound_ofUmax((j-1)*in+1:j*in,:)=Umax;%[79.32;4.898;10];%[500;500;500]
8+
end
9+
C1=zeros(Nc*in,in);
10+
C2=zeros(Nc*in,Nc*in);
11+
for j=1:Nc;
12+
C1(in*(j-1)+1:in*j,:)=eye(in,in);
13+
end
14+
for j=1:Nc
15+
C2(in*(j-1)+1:in*Nc,in*(j-1)+1:in*j)=C1(1:in*(Nc-j+1),:);
16+
end
17+
M1=[-C2;C2];
18+
else
19+
M1=[];
20+
end
21+
if size(Ymax)~=0
22+
for j=1:Np
23+
bound_ofYmin((j-1)*out+1:j*out,:)=Ymin;%[-40.68;-2.102;0];%[-500;-500;-500]
24+
bound_ofYmax((j-1)*out+1:j*out,:)=Ymax;%[79.32;4.898;10];%[500;500;500]
25+
end
26+
M3=[-phi;phi];
27+
else
28+
M3=[];
29+
end
30+
H=2*(phi_phi+R*eye(Nc*in,Nc*in));
31+
end
32+

‎mpcgainOrigin.m

+34
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
function[F,Phi,Phi_Phi,Phi_F,Phi_R] = mpcgainOrigin(Ap,Bp,Cp,Nc,Np,Rs,Q)
2+
3+
%%augmented matrix%%
4+
[m1,n1]=size(Cp);
5+
[n1,n_in]=size(Bp);
6+
A_e=eye(n1+m1,n1+m1);
7+
A_e(1:n1,1:n1)=Ap;
8+
A_e(n1+1:n1+m1,1:n1)=Cp*Ap;
9+
B_e=zeros(n1+m1,n_in);
10+
B_e(1:n1,:)=Bp;
11+
B_e(n1+1:n1+m1,:)=Cp*Bp;
12+
C_e=zeros(m1,n1+m1);
13+
C_e(:,n1+1:n1+m1)=eye(m1,m1);
14+
n=n1+m1;
15+
h(:,:)=C_e;
16+
F(:,:)=C_e*A_e;
17+
for kk=2:Np
18+
h(kk,:)=h(kk-1,:)*A_e;
19+
F(kk,:)= F(kk-1,:)*A_e;
20+
end
21+
v=h*B_e;
22+
Phi=zeros(Np,Nc); %declare the dimension of Phi
23+
Phi(:,1)=v; % first column of Phi
24+
for i=2:Nc
25+
Phi(:,i)=[zeros(i-1,1);v(1:Np-i+1,1)]; %Toeplitz matrix
26+
end
27+
BarRs=ones(Np,1);
28+
Phi_Phi= Phi'*Phi;
29+
Phi_F= Phi'*F;
30+
Phi_R=Phi'*BarRs;
31+
end
32+
33+
34+

‎optimizationWithConstrains2.m

+32
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
%function [] = optimizationWithConstrains(phi,phi_phi,R,Umax,Umin,Ymax,Ymin,in,out,Np,Nc)
2+
function [M2, N2] = optimizationWithConstrains(deltaumax,deltaumin,Np,Nc)
3+
4+
M2 = [-eye(Nc)
5+
eye(Nc)];
6+
N2 = [-ones(Nc,1)*deltaumin
7+
ones(Nc,1)*deltaumax];
8+
9+
10+
11+
12+
13+
14+
15+
16+
17+
18+
19+
20+
21+
22+
23+
24+
25+
26+
27+
28+
29+
30+
31+
32+

0 commit comments

Comments
 (0)