基于最小方差控制的间接自校正控制

close all;
clear all;
clc;

%*********************系统初始化*********************%
a=[1 -1.7 0.7]; b=[1 0.5]; c=[1 0.2]; k=4; %对象参数
na=length(a)-1; nb=length(b)-1; nc=length(c)-1; %na、nb、nc为多项式A、B、C阶次
nf=nb+k-1; %nf为多项式F的阶次

K=400; %控制步数
ut=zeros(k+nb,1); %输入历史的初值
yt=zeros(na,1); %输出历史的初值
yrt=zeros(nc,1); %期望输出历史的初值
wt=zeros(nc,1); %白噪声历史的初值,用来计算输出y(k)
wet=zeros(nc,1); %白噪声估计历史的初值，用来参数辨识
yr=10*[ones(K/2,1);-ones(K/2+k,1)]; %期望输出
w=sqrt(0.1)*randn(K,1); %白噪声序列

%辨识参数初值设置
thetae_1=0.001*ones(na+nb+1+nc,1);
P=10^8*eye(na+nb+1+nc);

%*********************递推辨识与控制*********************%
for t=1:K
y(t)=-a(2:na+1)*yt+b*ut(k:k+nb)+c*[w(t);wt]; %采集输出数据

phie=[-yt;ut(k:k+nb);wet];

%递推增广最小二乘法A(Z)*y(t)=B(Z)*u(t-k)+C(Z)*w(t)
L=P*phie/(1+phie'*P*phie);
thetae(:,t)=thetae_1+L*(y(t)-phie'*thetae_1);
P=(eye(na+nb+1+nc)-L*phie')*P;

thetae_1=thetae(:,t);
we=y(t)-phie'*thetae(:,t); %白噪声的估计值

%提取辨识参数
ae=[1 thetae_1(1:na)']; be=thetae_1(na+1:na+nb+1)'; ce=[1 thetae_1(na+nb+2:na+nb+1+nc)'];

%辨识参数稳定化，特征方程为B(Z)C(Z)=0,系统稳定的充要条件是B(Z)和C(Z)稳定，稳定边界为1，取0.9
if abs(be(2))>0.9
be(2)=sign(be(2))*0.9;
end
if abs(ce(2))>0.9
ce(2)=sign(ce(2))*0.9;
end

[d,f,e]=sindiophantine(ae,be,ce,k); %求解单步Diophantine方程（见另一篇文章）

%求控制量u(k)=[C(Z)*yr(k+d)-E(Z)*y(k)]/[B(Z)*D(Z)]
u(t)=(-f(2:nf+1)*ut(1:nf)+ce*[yr(t+k:-1:t+k-min(k,nc));yrt(1:nc-k)]-e*[y(t);yt(1:na-1)])/f(1); %求控制量

%更新数据
for i=k+nb:-1:2
ut(i)=ut(i-1);
end
ut(1)=u(t);

for i=na:-1:2
yt(i)=yt(i-1);
end
yt(1)=y(t);

for i=nc:-1:2
yrt(i)=yrt(i-1);
wt(i)=wt(i-1);
wet(i)=wet(i-1);
end
if nc>0
yrt(1)=yr(t);
wt(1)=w(t);
wet(1)=we;
end

end

%*********************显示图像*********************%
figure(1);
subplot(2,1,1);
plot([1:K],yr(1:K),'r:',[1:K],y);
xlabel('k'); ylabel('y_r(k)，y(k)');
legend('y_r(k)','y(k)'); axis([0 K -20 20]);
subplot(2,1,2);
plot([1:K],u);
xlabel('k'); ylabel('u(k)'); axis([0 K -40 40]);
figure(2)
subplot(2,1,1);
plot([1:K],thetae(1:na,:));
xlabel('k'); ylabel('参数估计a');
legend('a_1','a_2'); axis([0 K -3 2]);
subplot(2,1,2);
plot([1:K],thetae(na+1:na+nb+1+nc,:));
xlabel('k'); ylabel('参数估计b，c');
legend('b_0','b_1','c_1'); axis([0 K 0 1.5]);