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

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阶次
nd=k-1;nf=nb+nd; ne=na-1; %nd、nf、ng为多项式D、F、G的阶次

K=400; %控制步数
ut=zeros(k+nf,1); %输入历史的初值
yt=zeros(k+ne,1); %输出历史的初值
yet=zeros(nc,1); %输出估计历史的初值
yrt=zeros(nc,1); %期望输出历史初值
wt=zeros(nc,1); %白噪声历史的初值
wet=zeros(nc+k-1,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+k+nc+nc+nd,1);
P=10^8*eye(na+nb+k+nc+nc+nd);

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

    phie=[yt(k:k+ne);ut(k:k+nf);-yet(1:nc);wet];
    
    ye=phie'*theta