% RBF03.m
clear all;
close all;
kp_1=0.03;
ki_1=0.01;
kd_1=0.03;

yout_1=0;
u_1=0;
e_1=0;e_2=0;
ts=0.001;

eta=0.25;
x=[0,0,0]';
c=30*ones(3,6);
b=40*ones(1,6);
w=0.5*ones(1,6);
w_1=w;b_1=b;c_1=c;

for k=1:1:1000
    time(k)=k*ts;
    % reference input
    rin(k)=1.0*sign(sin(2*pi*k*ts));
    % nonlinear plant
    yout(k)=(-0.2*yout_1+0.5+u_1)/(1.5+yout_1^2);
    % RBF ynout
    for j=1:1:6
        h(j)=exp(-norm(x-c(:,j))^2/(2*b(j)*b(j)));
    end
    ynout(k)=w*h';
    % update RBF w
    erbf=yout(k)-ynout(k);
    for j=1:1:6
        dw(j)=eta*erbf*h(j);
    end
    w=w_1+dw;
    w_1=w;
    % update RBF b
    for j=1:1:6
        db(j)=eta*erbf*w(j)*h(j)*(norm(x-c(:,j))^2/(b(j)^3));
    end
    b=b_1+db;
    b1_1=b;
    % update RBF c
    for j=1:1:6
        for i=1:1:3
            dc(i,j)=eta*erbf*w(j)*h(j)*((x(i)-c(i,j))/(b(j)^2));
        end
    end
    c=c_1+dc;
    c_1=c;
    % compute Jacobian
    yu=0;
    for j=1:1:6
        yu=yu+w(j)*h(j)*(-x(1)+c(1,j))/(b(j)^2);
    end
    dyu(k)=yu;    
    % error
    e(k)=rin(k)-yout(k);    
    % PID input
    xc(1)=e(k)-e_1;
    xc(2)=e(k);
    xc(3)=e(k)-2*e_1+e_2;
    % update PID parameters
    kp(k)=kp_1+eta*e(k)*dyu(k)*xc(1);
    ki(k)=ki_1+eta*e(k)*dyu(k)*xc(2);
    kd(k)=kd_1+eta*e(k)*dyu(k)*xc(3);   
    % update kp(k-1),ki(k-1),kd(k-1)
    kp_1=kp(k);ki_1=ki(k);kd_1=kd(k);
    % PID output
    du(k)=kp(k)*xc(1)+ki(k)*xc(2)+kd(k)*xc(3);
    u(k)=u_1+du(k);
    % update yout(k-1),u(k-1)
    yout_1=yout(k);
    u_1=u(k);
    % update e(k-1),e(k-2)
    e_1=e(k);
    e_2=e_1; 
    % update RBF x vector
    x(1)=du(k);
    x(2)=yout(k);
    x(3)=yout_1;
end
figure(1);
plot(time,rin,'b',time,yout,'r');
xlabel('time(s)');ylabel('rin,yout');
figure(2);
plot(time,dyu);
xlabel('time(s)');ylabel('Jacobian value');
figure(3);
subplot(311);
plot(time,kp);
xlabel('time(s)');ylabel('kp');
subplot(312);
plot(time,ki);
xlabel('time(s)');ylabel('ki');
subplot(313);
plot(time,kd);
xlabel('time(s)');ylabel('kd');