%輔助變量法RIV用純延遲方法
%白噪聲作為輸入
clc
clear
close all
J=30;
var_u=1;
var_v=0.1;
u=sqrt(var_u)*randn(1,J);
z=zeros(1,J);
v=sqrt(var_v)*randn(1,J);%均值為0,方差為var_v的白噪聲信號
ab=[-3.808 5.434 -3.445 0.8187 1 -3.935 5.806 -3.807 0.9362]';
for k=5:J  
    z(k)=3.808*z(k-1)-5.434*z(k-2)+3.445*z(k-3)-0.8187*z(k-4)+u(k)-3.935*u(k-1)+5.806*u(k-2)-3.807*u(k-3)+0.9362*u(k-4)+v(k);
end
c0=[0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001]';
p0=10^6*eye(9,9);
E=0.000000001;
c=[c0,zeros(9,J-1)];%被辨識參數矩陣的初始值及大小
e=zeros(9,J);%相對誤差的初始值及大小
for k=9:J 
    h1=[-z(k-1),-z(k-2),-z(k-3),-z(k-4),u(k),u(k-1),u(k-2),u(k-3),u(k-4)]'; 
    h2=[-u(k-5),-u(k-6),-u(k-7),-u(k-8),u(k),u(k-1),u(k-2),u(k-3),u(k-4)]'; 
    x=h1'*p0*h2+1; x1=inv(x); 
    k1=p0*h2*x1;
    d1=z(k)-h1'*c0; 
    c1=c0+k1*d1;
    e1=c1-c0;
    e2=e1./c0;
    e(:,k)=e2;        
    c0=c1;
    c(:,k)=c1; 
    p1=p0-k1*k1'*[h1'*p0*h2+1];
    p0=p1;
    if e2<=E break;
    end
end
c,e,
a1=c(1,:); a2=c(2,:);a3=c(3,:);a4=c(4,:); b1=c(5,:); b2=c(6,:);b3=c(7,:);b4=c(8,:);b5=c(9,:);
ea1=e(1,:); ea2=e(2,:);ea3=e(3,:);ea4=e(4,:); eb1=e(5,:); eb2=e(6,:);eb3=e(7,:);eb4=e(8,:);eb5=e(9,:); 
figure(1)
i=1:J;
plot(i,a1,'r',i,a2,'b',i,a3,'g',i,a4,'y',i,b1,'r:',i,b2,'b:',i,b3,'g:',i,b4,'y:',i,b5,':') 
title('Parameter Identification with RIV Method')%圖形標題
figure(2)
i=1:J; 
plot(i,ea1,'r',i,ea2,'g',i,ea3,'b',i,ea4,'y',i,eb1,'r:',i,eb2,'b:',i,eb3,'g:',i,eb4,'y:',i,eb5,':') 
title('RIV Identification Precision') %圖形標題

ze=zeros(1,J);
for k=5:J 
    z(k)=3.808*z(k-1)-5.434*z(k-2)+3.445*z(k-3)-0.8187*z(k-4)+u(k)-3.935*u(k-1)+5.806*u(k-2)-3.807*u(k-3)+0.9362*u(k-4)+v(k);
    ze(k)=-a1(k)*ze(k-1)-a2(k)*ze(k-2)-a3(k)*ze(k-3)-a4(k)*ze(k-4)+b1(k)*u(k)+b2(k)*u(k-1)+b3(k)*u(k-2)+b4(k)*u(k-3)+b5(k)*u(k-4)+v(k);
    ee(k)=abs((ze(k)-z(k))/z(k));
end
figure(3)
i=1:J; 
plot(i,ee,'r')
title('RIV算法模型誤差')