%Single Neural Net PID Controller based on RBF Wavelet Neural Network Identification
%完善網絡結構將RBF網絡的徑向基換成小波函數，調整權值以及公式的變更
%可望在仿真結構中添加非奇異項以驗證小波網絡的辨識精度和能力
%輸入層加權值進行調整，
clear all;close all;

Jp=0.0030;bp=0.067;
ts=0.01;

Gp=tf([1],[Jp,bp,0]);
Gpz=c2d(Gp,ts,'z');
[num,den]=tfdata(Gpz,'v');

h=zeros(6,1);
%w=rands(6,1);
ww1=rand(3,6);
w=[-0.5646;
    0.3937;
   -0.5556;
    0.3981;
    0.4495;
    0.2565];
w_1=w;w_2=w;w_3=w;
ww1_1=ww1;ww1_2=ww1;ww1_3=ww1;

xite=0.60;
alfa=0.10;
belte=0.10;

x=[0,0,0]';

c=1.11*rand(6,1);
%b=0.1*ones(6,1);
%c=[-3.1829   -0.5211    7.1754   11.6631   -3.6992  -10.9150;
%   -3.8909    2.3999    5.1730    8.5871  -11.3737   -7.0179;
%  -4.2018    2.6742    5.1828    8.5238   -1.8936   -6.1845];
b=[ 5.3074;
    1.4771;
   26.4114;
   22.1716;
   52.9082;
    5.6906];
c_1=c;c_2=c_1;c_3=c_2;
b_1=b;b_2=b_1;b_3=b_2;

xc=[0,0,0]';
xitec=0.60;

kp=80;
kd=5;
ki=50;

wc=[kp,kd,ki];
wc_1=wc;wc_2=wc;wc_3=wc;

error_1=0;error_2=0;
y_1=0;y_2=0;
u_1=0;u_2=0;

ei=0;
c_size=size(c);

for k=1:1000
    time(k)=k*ts;
    rin(k)=sin(3*2*pi*k*ts);    
S=1;    
if S==1    
   yrout(k)=1.0*rin(k);
end
if S==2    
	yrout(k)=0.2*y_1+0.6*rin(k); %Reference Model
end
%Linear model
yout(k)=-den(2)*y_1-den(3)*y_2+num(2)*u_1+num(3)*u_2;
%yout(k)=2*sin(0.3*u_1^2+0.6*u_2+0.2)-0.5*exp(-0.2*u_1+0.5);

%for j=1:1:6,
  %  h(j)=exp(-norm(x-c_1(:,j))^2/(2*b_1(j)*b_1(j)));
  %end
  s=(ww1_1'*x-c)./b;
  h=0.66*(1-s.^2).*exp(-s.^2./2);
  hh=0.66*s.*(s.^2-3).*exp(-s.^2./2);
  ymout(k)=w_1'*h;
    
id=abs(yout(k)-ymout(k));%參數的調整，需要加上輸入層至隱層的權值調整計劃
if id>0.00001
   d_w=0*w;        % Defining matrix number of d_w equal to that of w
   for j=1:1:6
   d_w(j)=xite*(yout(k)-ymout(k))*h(j);
   end
   d_ww1=(xite*(yout(k)-ymout(k))*w.*hh*x')';
   w=w_1+d_w+alfa*(w_1-w_2)+belte*(w_2-w_3);
   ww1=ww1_1+d_ww1+alfa*(ww1_1-ww1_2)+belte*(ww1_2-ww1_3);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   d_b=0*b;
   %for j=1:1:6
       %d_b(j)=xite*(yout(k)-ymout(k))*w_1(j)*hh(j)*(1./b_1);
       % end
   d_b=xite*(yout(k)-ymout(k))*w_1.*hh.*(ww1_1'*x-c_1).*b_1.^(-2); 
   b=b_1+ d_b+alfa*(b_1-b_2)+belte*(b_2-b_3);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %for j=1:1:6
     %for i=1:1:3
         %d_c(i,j)=xite*(yout(k)-ymout(k))*w_1(j)*h(j)*(x(i)-c_1(i,j))*(b_1(j)^-2);
         %end
  d_c=xite*(yout(k)-ymout(k))*w_1.*hh.*(1./b_1);   
         % end
   c=c_1+d_c+alfa*(c_1-c_2)+belte*(c_2-c_3);
end 
%%%%%%%%%%% Calculating Jacobian %%%%%%%%%%%%
dyu=0;
for j=1:1:6
    dyu=dyu+w(j)*hh(j)*(ww1(1,j))/b(j);
end
dyout(k)=dyu;
%%%%%%Parameters Return%%%%%%%
error(k)=yrout(k)-yout(k);
xc(1)=error(k);
xc(2)=(error(k)-error_1)/ts;
ei=ei+error(k)*ts;
xc(3)=ei;
%控制器從這里開始，不要修改以下代碼  
u(k)=wc*xc;  %Control law
if u(k)>10,
   u(k)=10;
end   
if u(k)<-10,
   u(k)=-10;
end   
d_wc=0*wc;      % Defining matrix number of d_w equal to that of w
for j=1:1:3
    d_wc(j)=xitec*error(k)*xc(j)*dyout(k);
end
wc=wc_1+d_wc+alfa*(wc_1-wc_2)+belte*(wc_2-wc_3);
error_2=error_1;error_1=error(k);
    
u_2=u_1;u_1=u(k);
y_2=y_1;y_1=yout(k);
   
x(3)=y_2;
x(2)=y_1;
x(1)=u_1; 
w_3=w_2;w_2=w_1;w_1=w;
c_3=c_2;c_2=c_1;c_1=c;
b_3=b_2;b_2=b_1;b_1=b;
wc_3=wc_2;wc_2=wc_1;wc_1=wc;
end
figure(1);
plot(time,yout,'r',time,ymout,'b-');
xlabel('仿真時間');ylabel('實際輸出,辨識結果');
legend('系統輸出','辨識結果');
title('基于小波神經網絡的神經PID控制仿真')
figure(2);
plot(time,dyout,'r');
xlabel('仿真時間');ylabel('雅可比信息');
title('基于小波神經網絡的神經PID控制仿真')
figure(3);
plot(time,yrout,'b',time,yout,'r');
xlabel('仿真時間');ylabel('參考輸出,實際輸出');
legend('參考輸出','實際輸出');
title('基于小波神經網絡的神經PID控制仿真')
figure(4);
plot(time,ymout-yout,'r');
xlabel('仿真時間');ylabel('系統辨識誤差');
title('基于小波神經網絡的神經PID控制仿真')
figure(5);
plot(time,yout);
xlabel('仿真時間');ylabel('系統響應');
title('基于小波神經網絡的神經PID控制仿真')