From 695004df4553e0cd2d35c59d7864be434c4b7bbb Mon Sep 17 00:00:00 2001 From: Nikhil Nair Date: Fri, 24 Dec 2021 11:51:01 +0530 Subject: [PATCH] Added Code --- RBF.py | 87 ++++++++++++++++++++++++++++ nonlinearsystem.py | 37 ++++++++++++ nonlinearsystem_with_adaptive_pid.py | 76 ++++++++++++++++++++++++ nonlinearsystem_with_pid.py | 58 +++++++++++++++++++ 4 files changed, 258 insertions(+) create mode 100644 RBF.py create mode 100644 nonlinearsystem.py create mode 100644 nonlinearsystem_with_adaptive_pid.py create mode 100644 nonlinearsystem_with_pid.py diff --git a/RBF.py b/RBF.py new file mode 100644 index 0000000..1370e5a --- /dev/null +++ b/RBF.py @@ -0,0 +1,87 @@ +import numpy as np +from numpy.lib.function_base import average + +class RBF: + + def __init__(self , aw , av , au , asig , gamma ,h = 3 ): + + # TODO : Initialize all parameters + + self.X = np.zeros((3,1)) + self.h = h + self.mu = np.zeros( (3,h) ) + self.sigma = np.ones( (1,h) ) + + self.K =np.zeros( (3,1) ) + self.Vprev = 0 + self.V = 0 + + self.w = np.zeros( (3, h) ) + self.v = np.zeros( (1, h) ) + self.output = np.zeros( (h,1) ) + # Learning Rates + + self.aw = aw + self.av = av + self.au = au + self.asig = asig + self.gamma = gamma + + + + def HiddenLayer(self): + + # Description : Takes in the state vector at a given time step and computes the output vector for the next layer + + output = np.zeros( (self.h,1) ) + + + for i in range(self.h): + phi_j = np.exp( - np.linalg.norm( self.X - self.mu[:,i] )**2 /( 2*self.sigma[0][i]**2 ) ) + output[i] = phi_j + + self.output = output + + def OutputLayer(self): + + # Description : Takes in output from Hiddenlayer and computes Ki,Kp and Kd values + + self.K = self.w.dot(self.output) + + # print(self.K) + self.Vprev = self.V + self.V = self.v.dot(self.output) + + def Update(self,y_ref,yt_0,yt_1,yt_2,yt_3): + + # Update Params for next episode + + del_TD = 0.5 * ( y_ref - yt_0 )**2 + self.gamma*self.V - self.Vprev + + # Update w matrix + self.w[0] = self.w[0] - self.aw * del_TD*(yt_1 - yt_2)*self.output.T + self.w[1] = self.w[1] + self.aw * del_TD*self.X[0,0]*self.output.T + self.w[2] = self.w[2] + self.aw * del_TD*(yt_1 - 2*yt_2 + yt_3)*self.output.T + + # Updating the v value + v_prev = self.v + self.v = self.v + self.av * del_TD * self.output.T + + # Updating the centers and widths of hidden layers + + # print("Printing Shapes of Stuff") + + # print("Shape of self.au :", v_prev) + + for i in range(self.h): + self.mu[:,i] = self.mu[:,i] + self.au*del_TD*v_prev[0][i]*self.output[i]*(self.X- self.mu[:,i])[:,0]/self.sigma[0][i]**2 + + for i in range(self.h): + self.sigma[0][i] = self.sigma[0][i] + self.asig*del_TD*del_TD*v_prev[0][i]*self.output[i]*( np.linalg.norm(self.X- self.sigma[0][i]) )/self.sigma[0][i]**3 + + + print(self.K) + + + + diff --git a/nonlinearsystem.py b/nonlinearsystem.py new file mode 100644 index 0000000..f829fe4 --- /dev/null +++ b/nonlinearsystem.py @@ -0,0 +1,37 @@ +import matplotlib.pyplot as plt +import numpy as np + +noise = np.random.normal(0,1,100) + + +def y(t): + + + + yt_1 = 1 + + yt_2 = 1 + dt = 1/400 + + y=[] + x=[] + + + for i in range(0, int(t/dt) ): + yt_1,yt_2 = yt_1*yt_2*(yt_1 + 2.5) / ( 1 + yt_1**2 + yt_2**2 ) , yt_1 + np.random.normal(0,0.01) + + # print(yt_1," ",yt_2) + y.append(yt_1) + x.append(i*dt) + + return y,x + + +if __name__=="__main__": + + x,y = y(1) + + plt.plot(y,x) + + plt.show() + diff --git a/nonlinearsystem_with_adaptive_pid.py b/nonlinearsystem_with_adaptive_pid.py new file mode 100644 index 0000000..9457b44 --- /dev/null +++ b/nonlinearsystem_with_adaptive_pid.py @@ -0,0 +1,76 @@ +import matplotlib.pyplot as plt +import numpy as np + +import RBF + + +def y(yd): + + + rbf = RBF.RBF(0.13,0.21,0.25,0.9,0.98) + + t = 1 + yt_1 = 0.9 + yt_2 = 1.1 + yt_3 = 1.1 + dt = 1/800 + + Ki = -0.07709546 + Kd = 0.58844546 + Kp = -0.01747239 + + ut_1 = 0 + + + y=[] + x=[] + + + for i in range(0, int(t/dt) ): + + e_t = yd[i] - yt_1 + del_y = yt_1 - yt_2 + del2_y = yt_1 - 2*yt_2 + yt_3 + + rbf.X[:,0] = [ e_t , -del_y , -del2_y] + rbf.HiddenLayer() + rbf.OutputLayer() + + + ut_1 = ut_1 + rbf.K[1]*e_t - rbf.K[0]*del_y - rbf.K[2]*del2_y + + y1,y2,y3 = yt_1 , yt_2, yt_3 + + yt_1 , yt_2, yt_3 = yt_1*yt_2*(yt_1 + 2.5) / ( 1 + yt_1**2 + yt_2**2 )+ ut_1 +np.random.normal(0,0.01) , yt_1 , yt_2 + + y0 = yt_1 + + + rbf.Update(yd[i],y0,y1,y2,y3) + + y.append(yt_1) + x.append(i*dt) + + return y,x + + +if __name__=="__main__": + + + yd = [2.1 for i in range(100) ] + [3.5 for i in range(100) ] + [2 for i in range(100) ] + [3 for i in range(100) ] + + yd+=yd + + + ## Generate Reference array here + + y,x = y(yd) + + plt.plot(x,yd, label="Reference Signal") + plt.plot(x,y,label ="Output") + + plt.legend() + + + plt.show() + diff --git a/nonlinearsystem_with_pid.py b/nonlinearsystem_with_pid.py new file mode 100644 index 0000000..9ae2c84 --- /dev/null +++ b/nonlinearsystem_with_pid.py @@ -0,0 +1,58 @@ +import matplotlib.pyplot as plt +import numpy as np + + +def y(yd): + + t = 1 + yt_1 = 0.9 + yt_2 = 1.1 + yt_3 = 1.1 + dt = 1/400 + + Ki = 0.8 + Kd = 0.001 + Kp = 0.61 + + ut_1 = 0 + + + y=[] + x=[] + + + for i in range(0, int(t/dt) ): + + e_t = yd[i] - yt_1 + del_y = yt_1 - yt_2 + del2_y = yt_1 - 2*yt_2 + yt_3 + + + ut_1 = ut_1 + Ki*e_t - Kp*del_y - Kd*del2_y + + yt_1,yt_2,yt_3 = yt_1*yt_2*(yt_1 + 2.5) / ( 1 + yt_1**2 + yt_2**2 )+ ut_1 +np.random.normal(0,0.01) , yt_1 , yt_2 + + y.append(yt_1) + x.append(i*dt) + + return y,x + + +if __name__=="__main__": + + + yd = [2.5 for i in range(100) ] + [3.5 for i in range(100) ] + [1 for i in range(100) ] + [3 for i in range(100) ] + + + ## Generate Reference array here + + y,x = y(yd) + + plt.plot(x,yd, label="Reference Signal") + plt.plot(x,y,label ="Output") + + plt.legend() + + + plt.show() +