untitled11134.m

状态空间的预测控制.仿真里写的是一个二维的例子.

A=[0 1.2;1 -1.5];
Ad=[0.2 0.2;0.1 0.1];
B=[1.1;1.3];
d=2;
umax=2;

x_1=[0;0];
x_2=[1;1];
x_3=[1;1];
Q=[1 0;0 1];
Q1=inv(Q);
Q11=[Q 0 0 0 0;0 Q 0 0 0;0 0 Q 0 0;0 0 0 Q 0;0 0 0 0 Q];
R=1;
R1=inv(R);
R11=[R 0 0 0 0;0 R 0 0 0;0 0 R 0 0;0 0 0 R 0;0 0 0 0 R];
A1=[0 0 0 0 0;A 0 0 0 0;0 A 0 0 0;Ad 0 A 0 0;0 Ad 0 A 0];   %表示N域前整理得的。
B1=[0 0 0 0 0;B 0 0 0 0; 0 B 0 0 0;0 0 B 0 0;0 0 0 B 0];
Ad1=[0 0;Ad 0;0 Ad;0 0;0 0];
xd1=[x_2;x_1];
x01=[x_3;0;0;0;0];
w1=inv(eye-A1)*B1;%
m1=inv(eye-A1)*Ad1*xd1;
v01=inv(eye-A1)*x01;
w11=w1'*Q11*w1+R11;   %表示从上面重整理下得到的。
v011=2*(v01'*Q11*w1+m1'*Q11*w1);
b5=[0 0 0 0 B];
b4=[0 0 0 B 0];
b3=[0 0 B 0 0];
a5=[0 0 Ad 0 A];
a4=[0 Ad 0 A 0];
a3=[Ad 0 A 0 0];
a51=a5*w1+b5;
m51=a5*v01+a5*m1;
a41=a4*w1+b4;
m41=a4*v01+a4*m1;
a31=a3*w1+b3;
m31=a3*v01+a3*m1;

%x5=[0.2 ;0.1];                                                                                                                                           
%x4=[0.47 ;0.32];
%x3=[0.51 ;0.35];
%setlmis
for k=0:1:20
setlmis([])
y1=lmivar(2,[1 2]);
y2=lmivar(2,[1 2]);
q1=lmivar(1,[2 1]);
q2=lmivar(1,[2 1]);
r=lmivar(1,[1 0]);
r1=lmivar(1,[1 0]);
u=lmivar(2,[5 1]);
%x5=lmivar(2,[2 1]);
%x4=lmivar(2,[2 1]);
%x3=lmivar(2,[2 1]);
%1st
lmiterm([-1 1 1 q1],1,1)
lmiterm([-1 2 1 q1],1,1)
lmiterm([-1 2 2 q2],1,1)
lmiterm([-1 3 1 q1],1,1)
lmiterm([-1 3 2 0],0)
lmiterm([-1 3 3 r],1,Q1)
lmiterm([-1 4 1 0],0)
lmiterm([-1 4 2 0],0)
lmiterm([-1 4 3 0],0)
lmiterm([-1 4 4 q2],1,1)
lmiterm([-1 5 1 y1],1,1); 
lmiterm([-1 5 2 y2],1,1); 
lmiterm([-1 5 3 0],0)
lmiterm([-1 5 4 0],0)
lmiterm([-1 5 5 0],R1)
lmiterm([-1 6 1 q1],A,1)
lmiterm([-1 6 1 y1],B,1)
lmiterm([-1 6 2 q2],Ad,1)
lmiterm([-1 6 2 y2],B,1)
lmiterm([-1 6 3 0],0)
lmiterm([-1 6 4 0],0)
lmiterm([-1 6 5 0],0)
lmiterm([-1 6 6 q1],1,1)
%2st
lmiterm([-2 1 1 0],umax^2)
lmiterm([-2 1 2 y1],1,1)
lmiterm([-2 1 3 y2],1,1)
lmiterm([-2 2 2 q1],1,1)
lmiterm([-2 2 3 0],0)
lmiterm([-2 3 3 q2],1,1)
%3st
lmiterm([-3 1 1 0],1)
lmiterm([-3 2 1 u],a51,1)
lmiterm([-3 2 1 0],m51)
lmiterm([-3 2 2 q1],1,1)
lmiterm([-3 3 1 u],a41,1)
lmiterm([-3 3 1 0],m41)
lmiterm([-3 3 2 0],0)
lmiterm([-3 3 3 0],q2)
lmiterm([-3 4 1 u],a31,1)
lmiterm([-3 4 1 0],m31)
lmiterm([-3 4 2 0],0)
lmiterm([-3 4 3 0],0)
lmiterm([-3 4 4 q2],1,1)
%4st
lmiterm([-4 1 1 r1],1,1)
lmiterm([-4 1 1 u],v011,1)
lmiterm([-4 2 1 0],m1)
lmiterm([-4 2 2 0],inv(Q11))
lmiterm([-4 3 1 u],w11^1/2,1)
lmiterm([-4 3 2 0],0)
lmiterm([-4 3 3 0],1)
lmisys=getlmis;
 n=decnbr(lmisys)
    c=zeros(n,1);
   for j=1:n,
     [X1,X2,X3,X4,X5,X6,X7]=defcx(lmisys,j,y1,y2,q1,q2,r,r1,u)
      c(j)=trace(X5)
   end
  [copt,xfeas]=mincx(lmisys,c)
r11=dec2mat(lmisys,xfeas,r)
y11=dec2mat(lmisys,xfeas,y1)
y21=dec2mat(lmisys,xfeas,y2)
q11=dec2mat(lmisys,xfeas,q1)
q21=dec2mat(lmisys,xfeas,q2)
u11=dec2mat(lmisys,xfeas,u)
u1=u(:,1)
%x51=dec2mat(lmisys,xfeas,x5)
%x41=dec2mat(lmisys,xfeas,x4)
%x31=dec2mat(lmisys,xfeas,x3)

x1=A*x_3+Ad*x_1+B*u1
x_1=x_2
x_2=x_3
x_3=x1
v(:,k)=u1
v1(:,k)=x_3
end 
for k=0:1:20
plot(k,v,'r')
plot(k,v1,'b')
end

%k1=y11*inv(q11)
%k2=y21*inv(q21)