rglsm.m

关于系统辨识的一些仿真程序

function [nois,e,cf,ce]=rglsm(A,B,C,u)
a=length(u);
z=zeros(1,a);
dif=zeros(1,a);
rz=zeros(1,a);
cf=zeros(9,a);% to define the result matrix of A and B.
ce=zeros(4,a)
ce0=ce(:,1);% to define the result matrix of C.
zf=zeros(1,a);% C*z.
uf=zeros(1,a);% C*u.
nois=zeros(1,a);% to define the matrix of noise as a base to get C.
cf(:,1)=0.0010;
cf0=cf(:,1);
ce(:,1)=0;
pf0=10^6*eye(9,9);
pe0=eye(4,4);
E=10^(-7);
e=zeros(9,a); % to define the matrix on difference between z and rz.
for k=5:a
  z(k)=-A(2)*z(k-1)-A(3)*z(k-2)-A(4)*z(k-3)-A(5)*z(k-4)+B(1)*u(k)+B(2)*u(k-1)+B(3)*u(k-2)+B(4)*u(k-3)+B(5)*u(k-4);
  zf(k)=z(k);
end
for k=1:a
    uf(k)=u(k);
end
for k=5:a
  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)]';
  hf1=[-zf(k-1) -zf(k-2) -zf(k-3) -zf(k-4) uf(k) uf(k-1) uf(k-2) uf(k-3) uf(k-4)]';
  x=hf1'*pf0*hf1+1;
  x1=inv(x);
  k1=pf0*hf1*x1;
  d1=zf(k)-hf1'*cf0;
  cf1=cf0+k1*d1;
  e1=cf1-cf0;
  e2=e1./cf0;
  e(:,k)=e2
  cf0=cf1;
  cf(:,k)=cf1
  pf1=pf0-k1*k1'*(hf1'*pf0*hf1+1);
  pf0=pf1;
  nois(k)=z(k)-h1'*cf1;
  he1=[-nois(k-1) -nois(k-2) -nois(k-3) -nois(k-4)]';
  xe=he1'*pe0*he1+1;
  xe1=inv(xe);
  ke1=pe0*he1*xe1;
  de1=nois(k)-he1'*ce0;
  ce1=ce0+ke1*de1;
  ce0=ce1;
  ce(:,k)=ce1;
  pe1=pe0-ke1*ke1'*(he1'*pe0*he1+1);
  pe0=pe1;
%     if e2<=E
%         break;
%     end
end