%以B樣條網(wǎng)絡(luò)非線性濾波器為基礎(chǔ)的非線性自適應(yīng)逆控制結(jié)構(gòu)
clear all
clc

Ndr=150;% 訓(xùn)練樣本

%=========================非線性濾波器的參數(shù)===============================%
leng=5;%LMS濾波器的長(zhǎng)度
lengo=5;
len=2;%B樣條網(wǎng)絡(luò)的輸入

p=2;%B樣條的階次
n=2;%插入點(diǎn)的個(gè)數(shù)

wl=zeros(leng,1);%lms的權(quán)系數(shù)
wb=rand(p+n-1,1);%B樣條的權(quán)系數(shù)

Ul=zeros(leng,1);%LMS濾波器的輸入
Ulc=zeros(leng,1);
Ulw=zeros(leng,1);
% Ub=zeros(1,len);%B樣條函數(shù)的輸入
Nb=zeros(1,p+n-1);%B樣條函數(shù)的隱層節(jié)點(diǎn)數(shù)
Nbc=zeros(1,p+n-1);%B樣條函數(shù)的隱層節(jié)點(diǎn)數(shù)
Nbw=zeros(1,p+n-1);%B樣條函數(shù)的隱層節(jié)點(diǎn)數(shù)

 u=0.1;%lms濾波器的優(yōu)化步長(zhǎng)
 r=0.65;%B樣條網(wǎng)絡(luò)的修正步長(zhǎng)
 err=0;
%=========================非線性濾波器的參數(shù)===============================%

%========================非線性濾波器建模和消噪=============================%

%  uw=10+(rand(1,Ndr)-rand(1,Ndr))*1;
 uw=rand(1,Ndr);
 uu=10*ones(1,Ndr);
 
 uwmax=1;
 uwmin=0;
 
 ummax=10;
 ummin=9;
 
 ucmax=11;
 ucmin=9;
 
 ymax=121;
 ymin=81;
 
%  eqmax=17.5;
%  eqmin=-2.5;

 eqmax=3000;
 eqmin=-2500;


 yq=0;
% pp=rand(1,Ndr)*10;
 pp=ones(1,Ndr)*5+rand(1,Ndr)*2;

for k=1:leng
    yq(k)=0;
end

 for k=leng+1:Ndr
   %========================非線性濾波器建模=============================% 
     um(k)=uu(k)-yq(k-1);  %被控對(duì)象的輸入
     uc(k)=um(k)+uw(k);
     
     uf(k)=(um(k)-ummin)/(ummax-ummin);%建模濾波器的輸入
     ufc(k)=(uc(k)-ucmin)/(ucmax-ucmin);%復(fù)制濾波器的輸入
%      up(k)=(uw(k)-uwmin)/(uwmax-uwmin);
    up(k)=uw(k);
     
     yun(k)=uu(k)*uu(k);
     ynoise(k)=uc(k)*uc(k)+pp(k);
     yy(k)=uu(k)*uu(k)+pp(k);
   
   
    d(k)=(ynoise(k)-ymin)/(ymax-ymin);%期望輸出值
%     d(k)=(yun(k)-ymin)/(ymax-ymin);%期望輸出值
     

     %===================建模部分=====================%
     sum=zeros(1,p+n-1); %計(jì)算B樣條網(wǎng)絡(luò)的隱層節(jié)點(diǎn)

      for m=1:leng
      Ulw(m)=uw(k-m+1);
      end
      
      for m=0:len-1
         sum=sum+Bs(up(k-m),n,p,0,1);
      end
      Nbw=sum;   
     yw2(k)=Nbw*wb;%計(jì)算B樣條網(wǎng)絡(luò)的輸出
   
     yw1(k)=wl'*Ulw;%lms濾波器的輸出

     yw(k)=yw2(k)+yw1(k);%整個(gè)非線性濾波器的輸出
     
     %===================建模部分結(jié)束==========================%
     
      sum=zeros(1,p+n-1); %計(jì)算B樣條網(wǎng)絡(luò)的隱層節(jié)點(diǎn)

      for m=1:leng
      Ul(m)=uf(k-m+1);
      end
      
      for m=0:len-1
         sum=sum+Bs(uf(k-m),n,p,0,1);
      end
      Nb=sum;    
     y2(k)=Nb*wb;%計(jì)算B樣條網(wǎng)絡(luò)的輸出
   
     y1(k)=wl'*Ul;%lms濾波器的輸出

     y(k)=y2(k)+y1(k);%整個(gè)非線性濾波器的輸出
     
   %===========================利用誤差對(duì)權(quán)系數(shù)進(jìn)行自適應(yīng)===================%
     e(k)=d(k)-y(k)-yw(k);%擬合誤差
     

     wl=wl+2*u*e(k)*Ulw;%利用誤差調(diào)整lms濾波器的權(quán)系數(shù)
   
     wb=wb+r*e(k)*Nbw'/(Nbw*Nbw');%利用誤差調(diào)整B樣條網(wǎng)絡(luò)的權(quán)系數(shù)
     


   %========================非線性濾波器建模=============================% 
  
   %========================非線性濾波器消噪=============================%

 
   for t=1:leng
      Ulc(t)=uc(k-t+1);
   end

    
      sumc=zeros(1,p+n-1); %計(jì)算B樣條網(wǎng)絡(luò)的隱層節(jié)點(diǎn)
        
      for m=0:len-1
         sumc=sumc+Bs(uc(k-m),n,p,0,1);
      end
      Nbc=sumc;   
      y2c(k)=Nbc*wb;%計(jì)算B樣條網(wǎng)絡(luò)的輸出
   
      y1c(k)=wl'*Ulc;%lms濾波器的輸出

      yc(k)=y2c(k)+y1c(k);%整個(gè)非線性濾波器的輸出
      
     ydn(k)=yc(k)*(ymax-ymin)+ymin;
     equ(k)=ynoise(k)-ydn(k);
     a(k)=ynoise(k)-ydn(k);

     if equ(k)>=eqmax
         equ(k)=eqmax;
     end
     if equ(k)<=eqmin
         equ(k)=eqmin;
     end
     
     eq(k)=(equ(k)-eqmin)/(eqmax-eqmin);
     

%    反饋濾波那塊

     U=[eq(k) eq(k-1) eq(k-2) eq(k-3) eq(k-4)]';

     if k>=10
     yqu=IMBONL(wl,wb,U);
     else
         yqu=0;
     end
     
%     yq(k)=yqu*(eqmax-eqmin)+eqmin;

     yq(k)=yqu;
     
   
   %========================非線性濾波器消噪=============================%
   
 end
 
 %======================非線性濾波器建模和消噪結(jié)束==========================%
 
 
figure:plot(ynoise(Ndr-50:Ndr),'b');
hold on
plot(yun(Ndr-50:Ndr),'r');
hold on
plot(yy(Ndr-50:Ndr),'g'),title('藍(lán)色：系統(tǒng)的實(shí)際輸出；紅色：期望輸出；綠色:未加消除器的系統(tǒng)輸出');

% figure:plot(yun(Ndr-50:Ndr),'b');
% hold on
% plot(yf(Ndr-50:Ndr),'r'),title('藍(lán)色：系統(tǒng)的輸出；紅色：模型的輸出');

% figure:plot(uu(Ndr-50:Ndr),'b');
% hold on
% plot(um(Ndr-50:Ndr),'r'),title('藍(lán)色：系統(tǒng)對(duì)象的輸入；紅色：反饋后的輸入');

figure:plot(equ,'b');
hold on
plot(a,'r');
figure:plot(ydn,'r'),title('復(fù)制濾波器輸出');

% figure: plot(a,'b'),title('消噪誤差');