广义预测控制和丢番图方程求解程序

%GPC.m 广义预测自适应控制算法（GPC）
clear;
%被控对象为y(k)-2y(k-1)+1.1y(k-2)=u(k-4)+2u(k-5)+xi(k)/delta
A=[1 -2 1.1];C=1;d=4;
B=[zeros(1,d-1) 1 2];
% A=[1 -1.414 0.6065];d=1;B=[0.2088 0.1766];
na=length(A)-1;nb=length(B)-1; %na、nb分别为A、B末项下标
N1=d; %优化时域初值N1
N2=8; %优化时域终值N-7
Nu=2; %控制时域Nu
L=400; %仿真步长L
xi=sqrt(0.001)*randn(L,1); %xi(k)：方差为0.01的白噪声序列
w=10*[ones(1,L/4),-ones(1,L/4),ones(1,L/4),-ones(1,L/4+d)];
%设定值w （振幅为10的方波信号）
dy=zeros(1,L); %用dy(t)表示Δy（t），存放每一时刻的Δy
du=zeros(L,1); %用du(k)表示Δu(t)，存放每一时刻的Δu
dyt=zeros(na,1); %dyt(i)表示Δy(t-i)，对每一时刻t，存放过去的Δy dut=zeros(nb+1,1); %dut(i)表示Δu(t-i)，对每一时刻t，存放过去的Δu y=zeros(1,L); %存放每一时刻计算的系统输出值
u=zeros(1,L); %控制量初值
Y=zeros(na+1,1); %计算Δu式中的Y'=[Δy（t）Δy(t-1)……Δy(t-i)]
theta0=0.9*[A(2:na+1) B]'; %模型参数辨识初值
% theta0=[A(2:na+1) B]';
% theta0=0.001*ones(na+nb+2-d,1);
theta_e=zeros(na+nb+1,L); %θ的估计值，用θ(:,i)表示i时刻θ的估计值P=10^6*eye(na+nb+1); %协方差矩阵P初值
mu=0.98; %遗忘因子μ
alpha=0.2; %0<α<1用于计算参考轨迹
Lambda=1; %加权常数λ
%循环开始
for k=1:L
%计算输出数据
switch k
case 1
dy(1)=xi(1); %Δy(1)
y(1)=dy(1); %t=1时刻系统输出值
otherwise
dy(k)=-A(2:na+1)*dyt+B*dut+xi(k);
y(k)=y(k-1)+dy(k);
end
%辨识开始
X=[-dyt;dut];
switch k
case 1
theta_e(:,1)=theta0; %t=1时刻θ的估计值等于θ0
otherwise
theta_e(:,k)=theta_e(:,k-1)+P*X*(dy(k)-X'*theta_e(:,k-
1))/(mu+X'*P*X);
P=(1/mu)*(P-P*X*X'*P/(mu+X'*P*X));
end
%提取辨识结果
A_e=[1 (theta_e(1:na,k))'];
B_e=theta_e(na+1:na+nb+1,k)';
% A_e=A;
% B_e=B;%测试用
%解丢番图方程
[E,F,G,H]=DPT(A_e,B_e,N1,N2,Nu,na,nb); %计算参考轨迹
yr=zeros(N2,1);
yr(1)=y(k);
for i=2:N2
yr(i)=alpha*yr(i-1)+(1-alpha)*w(k+d); end
yr=yr(N1:N2);
%计算控制量
G1=(G'*G+Lambda*eye(Nu,Nu))^(-1)*G'; p=G1(1,:);
Y(1)=y(k);
switch k
case 1
otherwise
i=min(k,na+1);
for j=2:i
Y(j)=y(k-j+1);
end
end
switch k
case 1
du(1)=p*yr-p*F*Y;
otherwise
du(k)=p*yr-p*F*Y-p*H*dut(1:nb); end
switch k
case 1
u(1)=du(1);
otherwise
u(k)=u(k-1)+du(k);
end
for i=na:-1:2
dyt(i)=dyt(i-1);
end
dyt(1)=dy(k);
for i=nb+1:-1:2
dut(i)=dut(i-1);
end
dut(1)=du(k);
end
%图形显示
figure(1)
subplot(2,1,1);
plot(1:L,y);
xlabel('\fontsize{15}k');
ylabel('\fontsize{15}y,w');
hold on;
plot(1:L,w(1:L),':r');
subplot(2,1,2);
plot(1:L,u);
xlabel('\fontsize{15}k');
ylabel('\fontsize{15}u');
figure(2);
plot(1:L,theta_e)
xlabel('\fontsize{15}k');
ylabel('\fontsize{15}\theta');
function [E,F,G,H]=DPT(A_e,B_e,N1,N2,Nu,na,nb)
%************************************************ %功能：丢番图方程的递推求解
%输入：模型向量A、B的估计值A_e、B_e和下标na、nb, % 优化时域的初值和终止N1、N2,控制时域Nu
%输出：丢番图方程2.3、2.4(P11)中的E、F、G、H
%************************************************
E=zeros(N2,N2);
F=zeros(N2,na+1);
G=zeros(N2,Nu);
H=zeros(N2,nb);
AA=conv(A_e,[1 -1]);
AA=AA(2:na+2); % AA=A*Δ-1
F(1,:)=-AA;
for j=1:1:N2
for i=1:1:na+1
switch i
case na+1
F(j+1,na+1)=-AA(na+1)*F(j,1);
otherwise
F(j+1,i)=F(j,i+1)-AA(i)*F(j,1);
end
end
end % F
E(1,:)=1;
for i=2:1:N2
for j=1+1:1:i
E(j,i)=F(j-1,1);
end
end % E
H(1,:)=B_e(2:nb+1);
for j=1:1:N2-1
for i=1:1:nb
switch i
case nb
H(j+1,nb)=E(j+1,N2)*B_e(nb+1);
otherwise
H(j+1,i)=E(j+1,N2)*B_e(i+1)+H(j,i+1);
end
end
end % H
G(1,1)=B_e(1);
for j=2:1:N2
G(j,1)=E(j,N2)*B_e(1)+H(j-1,1);
end
for i=2:1:Nu
for j=i:1:N2
G(j,i)=G(j-1,i-1);
end
end % G
E=E';
E=E(N1:N2,:);
F=F(N1:N2,:);
G=G(N1:N2,:);
H=H(N1:N2,:); % 考虑N1不为1的情况
end。