% feedbl.m
% 反饋線性化

% 初始化
clear
X1 = [rand(1,300)*pi,  rand(1,100)*pi];
X2 = [rands(1,300)*pi,  zeros(1,100) ];
P = [X1; X2];
dt = 0.05;
T= X2 + dt*(10*sin(X1)-2*(X2));
[R,Q] = size(P);
 S1 = 10; 
[S2,Q] = size(T);

% 訓練網絡
%========

net = newff(minmax(P),[S1 S2],{'tansig' 'purelin'},'trainlm');
[w1,b1]=rands(S1,R);
[w2,b2]=rands(S2,S1);
net.iw{1,1}=w1*0.7;
net.b{1}=b1;
net.lw{2,1}=w2*0.5;
net.b{2}=b2*0.5;
net.performFcn = 'sse';            
net.trainParam.goal = 0.02;        
net.trainParam.show = 100;        
net.trainParam.epochs = 5000;     
net.trainParam.mc = 0.95;         
net.trainParam.lr=0.0001;         
net.trainParam.lr_inc =1.05;       
net.trainParam.lr_dec =0.7;
[net,tr] = train(net,P,T);            
W1 = net.iw{1,1};
B1 = net.b{1};
W2 = net.lw{2,1};
B2=net.b{2};

pause  %按任意鍵測試網絡性能

%  x2 = 0, Nf對x1的函數逼近

dt = 0.05;
x1 = 0:.01*pi:pi;
x2 = 0*ones(1,101);
xt = [x1; x2];

% 計算網絡輸出以及期望輸出
nf = purelin(W2*tansig(W1*xt,B1),B2);
df = x2 + dt*(9.81*sin(x1)-x2);

plot(x1,nf,x1,df,'--')
title('Nf __ & desired Nf -- as f[x(1)] with x(2)=0')
xlabel('x(1)')
ylabel('Nf')
grid

pause   %按任意鍵繼續

%  x1 = pi/2, Nf對x2的速度的響應

dt = 0.05;
x1 = pi/2*ones(1,101);
x2 = -2*pi:.04*pi:2*pi;
xt=[x1; x2];

% 計算網絡輸出以及期望輸出
nf=purelin(W2*tansig(W1*xt,B1),B2);
df = x2 + dt*(9.81*sin(x1)-x2);

% 作圖
plot(x2, nf, x2, df, '--')
title('Nf __ & Desired Nf -- as f[x(2)] with x(1)=pi/2')
xlabel('x(2)')
ylabel('Nf')
grid

pause

% 測試系統響應

dtr = 1/180*pi;  % 度與弧度的轉換量

% 狀態與參考輸入
xm=[10*dtr;0];
r=90*dtr;

% 線性參考模型常數
k=[9 6];km=k;

% 線性控制器常數
kl=[9 4];

ul=0;u =0;up=0;um=0;

x=xm;xl=xm;xp=xm;

% 仿真參數
tmax=4;dt=0.05;j=1;

% 計算初始輸入

  % 計算理想對消控制信號
  up=k(1)*r-k*xp-(9.81*sin(xp(1)))-2*xp(2);

  % 計算神經網絡控制信號(逼近對消)
  nf=purelin(W2*tansig(W1*x,B1),B2);
  u=(x(2)+dt*(k(1)*r-k*x)-nf)/dt;

  % 計算非線性系統線性控制信號
  ul=-10-kl*xl+kl(1)*r;
 
  % 計算線性參考控制模型的線性控制信號
  um=-km*xm+km(1)*r;

% 系統仿真回路
for t=dt:dt:tmax
  % 具有理想對消控制器系統的調整
  xpdot=zeros(2,1);
  xpdot(1)=xp(2,j);
  xpdot(2)=9.81*sin(xp(1,j))-2*xp(2,j) + up(j);
  xp = [xp (xpdot*dt + xp(:,j))];

  % 線性參考模型的調整
  xmdot=zeros(2,1);
  xmdot(1)=xm(2,j);
  xmdot(2)=um(j);
  xm = [xm (xmdot*dt + xm(:,j))];

  % 具有神經網絡控制器系統的調整
  xdot=zeros(2,1);
  xdot(1)=x(2,j);
  xdot(2)=9.81*sin(x(1,j))-2*x(2,j) + u(j);
  x = [x (xdot*dt + x(:,j))];

  % 具有線性控制器系統的調整
  xldot=zeros(2,1);
  xldot(1)=xl(2,j);
  xldot(2)=9.81*sin(xl(1,j))-2*xl(2,j) + ul(j);
  xl = [xl (xldot*dt + xl(:,j))];

  j=j+1;

  % 計算理想對消控制信號
  up=[up (k(1)*r-k*(xp(:,j))-(10*sin(xp(1,j))-2*xp(2,j)))];

  % 計算神經網絡控制信號(逼近對消)
  nf=purelin(W2*tansig(W1*x(:,j),B1),B2);
  u=[u (x(2,j)+dt*(k(1)*r-k*x(:,j))-nf)/dt];

  % 計算非線性系統的線性控制信號
  ul=[ul (-10-kl*xl(:,j)+kl(1)*r)];
 
  % 計算線性參考控制模型的線性控制信號
  um=[um (-km*xm(:,j)+km(1)*r)];

  end

t = 0:dt:tmax;
d = r*ones(size(t));

% 作圖(轉化成度)

rtd=180/pi;

% 神經網絡控制器與線性參考模型,以及精確非線性對消控制器的響應
plot(t,rtd*x(1,:),t,rtd*xm(1,:),'--',t,rtd*d)
hold on
plot(t,rtd*x(1,:),t,rtd*xp(1,:),'-',t,rtd*d)
title('Neural __, Reference Model & Perfect Cancel  --  Responses')
xlabel('Time (sec)')
ylabel('Position (deg)')
grid
hold off
pause

% 神經網絡控制器與線性控制器的響應

plot(t,rtd*x(1,:),t,rtd*xl(1,:),'--',t,rtd*d)
title('Neural __  & Linear Control --  Responses')
xlabel('Time (sec)')
ylabel('Position (deg)')
grid

pause

% 3種控制器的控制電流
plot(t,u,t,ul,'--',t,up,'o')
title('Currents: Neural __  Linear --  Perfect Cancel oo')
xlabel('Time (sec)')
ylabel('Control Current (amps)')
grid