带遗忘因子的有限数据窗递推广义增广最小二乘系统辨识

%递推增广最小二乘参数估计（RELS）
clear all; close all;
a=[1 -1.5 0.7]'; b=[1 0.5]'; c=[1 -1 0.2]'; d=[1 0.5]'; %对象参数
na=length(a)-1; nb=length(b); nc=length(c)-1;  nd=length(d)-1;%na、nb、nc为A、B、C阶次
d1=3;
L=40000; %仿真长度
thetas=[a(2:na+1);b];
thetan=[c(2:na+1);d(2:nd+1)];
thetaT=[thetas;thetan];
u=2*randn(L,1); %输入采用白噪声序列
v=sqrt(0.1)*randn(L,1);
uk=zeros(nb,1);
yk=zeros(na,1);
wk=zeros(nc,1);
vk=zeros(nd,1);
phis=[-yk;uk];
phin=[-wk;vk];
theta(:,1)=zeros(na+nb+nc+nd,1);
thetaP(:,1)=zeros(na+nb+nc+nd,1);
P=eye(na+nb+nc+nd)*10^6;
P_p=eye(na+nb+nc+nd)*10^6;
w_e=zeros(nc,1);
v_e=zeros(nd,1);
w_eP=zeros(nc,1);
v_eP=zeros(nd,1);
phine(:,1)=[-w_e;v_e];
phi_e(:,1)=[phis;phine];
w_ee(1)=0;
v_ee(1)=0;
w_eeP(1)=0.98;
v_eeP(1)=0.98;
lambda=1;
lambda_p=1;
P_l=1000;
count=0;
for k=1:L
    if k==L/4
        a=[1 -1 0.4]';b=[1.5 0.2]'; %对象参数突变
        thetas=[a(2:na+1);b];
        thetaT1=[thetas;thetan];
        uk=zeros(nb,1);
yk=zeros(na,1);
wk=zeros(nc,1);
vk=zeros(nd,1);
phis=[-yk;uk];
phin=[-wk;vk];
w_e=zeros(nc,1);
v_e=zeros(nd,1);
w_eP=zeros(nc,1);
v_eP=zeros(nd,1);
P=eye(na+nb+nc+nd)*10^6;
P_p=eye(na+nb+nc+nd)*10^6;
count=0;
    end
 y(k)=phis'*thetas+phin'*thetan+v(k);
 w(k)=phin'*thetan+v(k);
 if k>1
 %%
 count=count+1;
 phi_e(:,k)=[phis;-w_e;v_e];
 LL=P*phi_e(:,k)/(lambda+phi_e(:,k)'*P*phi_e(:,k));
 P=(eye(na+nb+nc+nd)-LL*phi_e(:,k)')*P/lambda;
 theta(:,k)=theta(:,k-1)+LL.*(y(k)-phi_e(:,k)'*theta(:,k-1));
e(k)=y(k)-phi_e(:,k)'*theta(:,k-1);
 %%
 w_ee(k)=y(k)-phi_e(1:(na+nb),k)'*theta(1:(na+nb),k);
 v_ee(k)=y(k)-phi_e(:,k)'*theta(:,k);
 if k<=nc
 for i=1:k
     w_e(i)=w_ee(k-i+1);
 end
 else
     for i=1:nc
     w_e(i)=w_ee(k-i+1);
     end
 end
 if k<=nd
     for i=1:k
      v_e(i)=v_ee(k-i+1);
     end
 else
     for i=1:nd
      v_e(i)=v_ee(k-i+1);
     end
 end
 end
 %%
 if count>P_l+2
    phi_eP(:,k)=[phis;-w_eP;v_eP];
    Q=(1/lambda_p)*(P_p-(P_p*phi_eP(:,k)*phi_eP(:,k)'*P_p)/(lambda_p+phi_eP(:,k)'*P_p*phi_eP(:,k)));
    P_p=Q+(Q*phi_eP(:,k-P_l)*phi_eP(:,k-P_l)'*Q)/(lambda_p^(-P_l)-phi_eP(:,k-P_l)'*Q*phi_eP(:,k-P_l));
    phi_pp=[phi_eP(:,k),-lambda_p^(P_l)*phi_eP(:,k-P_l)];
    innov=[y(k)-phi_eP(:,k)'*thetaP(:,k-1);y(k-P_l)-phi_eP(:,k-P_l)'*thetaP(:,k-1)];
    thetaP(:,k)=thetaP(:,k-1)+P_p*phi_pp*innov;
    
 w_eeP(k)=y(k)-phi_e(1:(na+nb),k)'*thetaP(1:(na+nb),k);
 v_eeP(k)=y(k)-phi_e(:,k)'*thetaP(:,k);
 if k<=nc
 for i=1:k
     w_eP(i)=w_eeP(k-i+1);
 end
 else
     for i=1:nc
     w_eP(i)=w_eeP(k-i+1);
     end
 end
 if k<=nd
     for i=1:k
      v_eP(i)=v_eeP(k-i+1);
     end
 else
     for i=1:nd
      v_eP(i)=v_eeP(k-i+1);
     end
 end
 else
    thetaP(:,k)=theta(:,k);
    v_eP=v_e;
    w_eP=w_e;
 end
 %% 
 if k<=na
 for i=1:k
     yk(i)=y(k-i+1);
 end
 else
     for i=1:na
     yk(i)=y(k-i+1);
     end
 end
 if k<=nb
     for i=1:k
      uk(i)=u(k-i+1);
     end
 else
     for i=1:nb
      uk(i)=u(k-i+1);
     end
 end
  if k<=nc
 for i=1:k
     wk(i)=w(k-i+1);
 end
 else
     for i=1:nc
     wk(i)=w(k-i+1);
     end
 end
 if k<=nd
     for i=1:k
      vk(i)=v(k-i+1);
     end
 else
     for i=1:nd
      vk(i)=v(k-i+1);
     end
 end
 phis=[-yk;uk];
 phin=[-wk;vk];
 
 error_1(:,k)=norm(theta(:,k)-thetaT)/norm(thetaT);
 error_2(:,k)=norm(thetaP(:,k)-thetaT)/norm(thetaT);
end


figure(1)
plot([1:L],theta(1:na,:));hold; plot([1:L],thetaT(1:na).*ones(2,L),'k:'); plot([1:L],thetaT1(1:na).*ones(2,L),'k:');
xlabel('t'); ylabel('参数估计a');
legend('a_1','a_2'); axis([0 L -2 2]);
figure(2)
plot([1:L],theta(na+1:na+nb,:));hold on; plot([1:L],thetaT(na+1:na+nb).*ones(nb,L),'k:');plot([1:L],thetaT1(na+1:na+nb).*ones(nb,L),'k:');
xlabel('t'); ylabel('参数估计b');
legend('b_0','b_1'); axis([0 L 0 2]);
figure(3)
plot([1:L],theta(na+nb+1:na+nb+nc,:));hold on;plot([1:L],thetaT(na+nb+1:na+nb+nc).*ones(2,L),'k:');plot([1:L],thetaT1(na+nb+1:na+nb+nc).*ones(2,L),'k:');
xlabel('t'); ylabel('参数估计c');
legend('c_1','c_2'); axis([0 L -2 2]);

figure(4)
plot([1:L],theta(na+nb+nc+1:na+nb+nc+nd,:));hold on;plot([1:L],thetaT(na+nb+nc+1:na+nb+nc+nd).*ones(2,L),'k:');plot([1:L],thetaT1(na+nb+nc+1:na+nb+nc+nd).*ones(2,L),'k:');
xlabel('t'); ylabel('参数估计d');
legend('c_1','c_2'); axis([0 L -2 2]);

figure(5)
plot([1:L],thetaP(1:na,:));hold; plot([1:L],thetaT(1:na).*ones(2,L),'k:'); plot([1:L],thetaT1(1:na).*ones(2,L),'k:');
xlabel('t'); ylabel('参数估计a');
legend('a_1','a_2'); axis([0 L -2 2]);
figure(6)
plot([1:L],thetaP(na+1:na+nb,:));hold on; plot([1:L],thetaT(na+1:na+nb).*ones(nb,L),'k:');plot([1:L],thetaT1(na+1:na+nb).*ones(nb,L),'k:');
xlabel('t'); ylabel('参数估计b');legend('b_0','b_1'); axis([0 L 0 2]);
figure(7)
plot([1:L],thetaP(na+nb+1:na+nb+nc,:));hold on;plot([1:L],thetaT(na+nb+2:na+nb+nc+1).*ones(2,L),'k:');plot([1:L],thetaT1(na+nb+2:na+nb+nc+1).*ones(2,L),'k:');
xlabel('t'); ylabel('参数估计c');
legend('c_1','c_2'); axis([0 L -2 2]);

figure(8)
plot([1:L],thetaP(na+nb+nc+1:na+nb+nc+nd,:));hold on;plot([1:L],thetaT(na+nb+nc+1:na+nb+nc+nd).*ones(2,L),'k:');plot([1:L],thetaT1(na+nb+nc+1:na+nb+nc+nd).*ones(2,L),'k:');
xlabel('t'); ylabel('参数估计d');
legend('c_1','c_2'); axis([0 L -2 2]);


figure
hold
plot(error_1);
plot(error_2);