diff --git a/example2/MIMO_recursive_least_square.py b/example2/MIMO_recursive_least_square.py new file mode 100644 index 000000000..115c92fff --- /dev/null +++ b/example2/MIMO_recursive_least_square.py @@ -0,0 +1,167 @@ +import os +import numpy as np +import matplotlib.pyplot as plt +import numpy.matlib + +#the least_square +a = np.array([1, -0.8, -0.2, 0.6]) +b = np.array([ + [[3, -3.5, -1.5, 0.0], + [1, -0.2, -0.5, 0]], + [[0, -4, -2, -1], + [1, -1.5, 0.5, 0.2]] + ]) + +d = np.array([[1, 1],[2, 1]]) +nb = np.array([[2, 2],[2, 3]]) + +len_a = len(a) -1; len_b = len(b) ; r = 2; m = 2; L =400 +mb = np.max(d + nb) # d + nb的最大元素 + +Input_init = np.zeros((r, mb)) #the init_value of input +Output_init = np.zeros((len_a, m)) # the init_value of ouput +white_noise_init = np.zeros((len_a, m)) + + +white_noise = np.array([np.sqrt(0.1)*np.random.normal(0, 1, L), + np.sqrt(0.1)*np.random.normal(0, 1, L)]) # the white_noise +print("the white_noise:", white_noise) +white_noise_1 = np.array([np.random.normal(0, 1, L),np.random.normal(0, 1, L)]) + +theta1 = []; theta2 = [] +for i in range(r): + theta1 = np.append(theta1, b[0][i][d[0][i] - 1: d[0][i] + nb[0][i]] ) + theta2 = np.append(theta2, b[1][i][d[1][i] - 1: d[1][i] + nb[1][i]] ) + + +theta_temp = np.append(theta1, theta2) +theta = np.append(a[1: len_a + 1], theta_temp) + +theta_init = np.zeros((len_a + sum(sum(nb)) + 4, 1)) + +#P = np.eye(len_a + len_b + 1, len_a + len_b + 1) +P = np.eye(len_a + sum(sum(nb)) + 4, len_a + sum(sum(nb)) + 4) + +phi = np.array([]) # the +thetae_a1 = [] +thetae_a2 = [] +thetae_a3 = [] + +thetae_b1_1 = [] +thetae_b1_2 = [] +thetae_b1_3 = [] + +thetae_b2_1 = [] +thetae_b2_2 = [] +thetae_b2_3 = [] + +thetae_b3_1 = [] +thetae_b3_2 = [] +thetae_b3_3 = [] + +thetae_b4_1 = [] +thetae_b4_2 = [] +thetae_b4_3 = [] +thetae_b4_4 = [] + + +for i in range(L): + # Y = AX + xi(noise) + temp_x1_1 = []; temp_x2_1 = [] + for l in range(r): + temp_x1_1 = np.append(temp_x1_1, Input_init[0][d[0][l] - 1: d[0][l] + nb[0][l]]) + temp_x2_1 = np.append(temp_x2_1, Input_init[1][d[1][l] - 1: d[1][l] + nb[1][l]]) + + temp_x1 = np.append(temp_x1_1, np.zeros(len(temp_x1_1))) + temp_x2 = np.append(temp_x2_1, np.zeros(len(temp_x2_1))) + temp_x = np.append(temp_x1, temp_x2) + temp = np.append(-Output_init, temp_x) # X + temp = temp.reshape(2, 16) + + matrix_temp = np.mat(temp) + + e = np.append(white_noise[:, 1], white_noise_init ).reshape(4, 2).transpose() + matrix_e = np.mat(e) + matrix_a = np.mat(a.reshape(4, 1)) + noise = matrix_e * matrix_a + print("the type of temp:", temp) + print("the shape of temp: ", np.shape(temp)) + print("the shape of theta: ", np.shape(theta)) + #temp_y = np.dot(temp, theta.transpose()) + noise # Y + matrix_theta = np.mat(theta) + print("the shape of matrix_theta: ", np.shape(matrix_theta)) + + temp_y = matrix_temp * matrix_theta.transpose() + noise + #temp_y = np.dot(temp, theta.transpose()) + print("the temp_y:", temp_y) + #temp_matrix = np.matrix(temp_y) + P_matrix = np.matrix(P) + + matrix_mi = np.matlib.eye(m, m) + + K = (P_matrix * matrix_temp.transpose()) * np.linalg.inv((matrix_mi + matrix_temp * P_matrix * matrix_temp.transpose())) + thetae_temp = theta_init + K * (temp_y - matrix_temp * theta_init) + + + thetae_a1.append(thetae_temp[0, 0]) + thetae_a2.append(thetae_temp[1, 0]) + thetae_a3.append(thetae_temp[2, 0]) + + thetae_b1_1.append(thetae_temp[3, 0]) + thetae_b1_2.append(thetae_temp[4, 0]) + thetae_b1_3.append(thetae_temp[5, 0]) + + thetae_b2_1.append(thetae_temp[6, 0]) + thetae_b2_2.append(thetae_temp[7, 0]) + thetae_b2_3.append(thetae_temp[8, 0]) + + thetae_b3_1.append(thetae_temp[9, 0]) + thetae_b3_2.append(thetae_temp[10, 0]) + thetae_b3_3.append(thetae_temp[11, 0]) + + thetae_b4_1.append(thetae_temp[12, 0]) + thetae_b4_2.append(thetae_temp[13, 0]) + thetae_b4_3.append(thetae_temp[14, 0]) + thetae_b4_4.append(thetae_temp[15, 0]) + + P = (np.eye(len_a + sum(sum(nb)) + 4) - K * temp) * P + + #update the data + theta_1 = thetae_temp + + for k in range(r - 1, -1, -1): + for j in range(mb - 1, -1, -1): + Input_init[k][j] = Input_init[k][j -1] + + Input_init[k][0] = white_noise[k][i] + + print("the Input_init:", Input_init) + for k in range(m -1, 0 , -1): + for j in range(len_a - 1, 0, -1): + Output_init[j][k] = Output_init[j - 1][k] + white_noise_init[j][k] = white_noise_init[j - 1][k] + + Output_init[k][0] = temp_y[k, :] + white_noise_init[0][k] = white_noise[k][i] + print("the onput_init:", Output_init) + + + +# the np.narray to np.matrix + +plt.figure(1) +ax = plt.subplot(1, 1, 1) + +plt.sca(ax) +ax.plot(thetae_a1, 'Red', label = 'thetae_a1') +ax.plot(thetae_a2, 'Blue', label = 'thetae_a2') +ax.plot(thetae_a3, 'Green', label = 'thetae_a3') +#ax.plot(thetae_b1_1, 'Yellow', label = 'thetae_b1_1') + +plt.title('thetae') +plt.xlabel("k") +plt.ylabel("value") + +ax.legend() +plt.show() + diff --git a/example2/M_sequence_and_inverse_M_sequence.py b/example2/M_sequence_and_inverse_M_sequence.py new file mode 100644 index 000000000..bddf1ed6e --- /dev/null +++ b/example2/M_sequence_and_inverse_M_sequence.py @@ -0,0 +1,51 @@ +import os +import numpy as np +import matplotlib.pyplot as plt + +L = 30 # the len of sequence +x1 = 1; x2 = 1; x3 = 1; x4 = 0 # the init_valce of register +#print("the x4 :" , x4) +S = 1 +U = [] # Inverse_M_sequence +M = [] # M_sequence +for i in range(1, L + 1, 1): + IM = S ^ x4 + if IM == 0: + U.append(-1) # Inverse_M_sequence + else: + U.append(1) # Inverse_M_sequence + + S = not(S) + m = x3 ^ x4 + M.append( m ) # M_sequence + x4 = x3; x3 = x2; x2 = x1 ; x1 = m + +print("the M_sequence:", M) +print("the_inverse_M_sequence:", U) +#figure +plt.figure(1) +ax = plt.subplot(2, 1, 1) +ax1 = plt.subplot(2, 1, 2) + +plt.sca(ax) +ax.plot(M, 'Red', label = 'M_sequence') +plt.title('M_sequence') +#plt.xlabel("k") +plt.ylabel("amplitude") +ax.legend() + +plt.sca(ax1) +ax1.plot(U, 'Blue', label = 'Inverse_M_sequence') +plt.title('Inverse_M_sequence') +plt.xlabel("k") +plt.ylabel("amplitude") +plt.legend() + + +plt.show() + +exit() + + + + diff --git a/example2/Noise_Singal_ratio_SISO.py b/example2/Noise_Singal_ratio_SISO.py new file mode 100644 index 000000000..c07d56edd --- /dev/null +++ b/example2/Noise_Singal_ratio_SISO.py @@ -0,0 +1,33 @@ +import os +import numpy as np +import matplotlib.pyplot as plt + +a = np.array([1, -0.4]); b = np.array([1]); c = np.array([1, -0.4]) +na = len(a) -1; nb = len(b) - 1; nc = len(c) - 1 +#print("the na: ", na) + +n = max(na, nb, nc) +print("the max of (na, nb, nc):", n) +a0 = a; b0 = b; c0 = c +for i in range(na, n, 1): + a0 = np.append(a0, 0) + +for i in range(nb, n, 1): + b0 = np.append(b0, 0) + +for i in range(nc, n, 1): + c0 = np.append(c0, 0) + +print("the a0: ", b0) + +p = []; qg = []; qh = [] +deltau2 = 1; deltav2 = 1 +for i in range(n): + p.append(a0[i]) + qg.append(b0[i]) + qh.append(c0[i]) + +for i in range(n - 1, 0 , -1): + for j in range(0, 2, 1): + p_2[j, i] = p[0] * p[i] + diff --git a/example2/White_Noise_And_Colored_Noise.py b/example2/White_Noise_And_Colored_Noise.py new file mode 100644 index 000000000..545acf973 --- /dev/null +++ b/example2/White_Noise_And_Colored_Noise.py @@ -0,0 +1,82 @@ +import os +import numpy as np +import matplotlib.pyplot as plt + +L = 500 # the len of noise + +# e(k) = c/d * f(k) f(k) is white_noise and e(k) is colored_noise +d = np.array([1, -1.5, 0.7, 0.1]) # the c +c = np.array([1, 0.5, 0.2]) # the d + +nd = len(d) - 1 # +nc = len(c) - 1 # + +white_noise_init = np.zeros(nc) # the init_value of white_noise + +colored_noise_init = np.zeros(nd) # the init_valuw of colored_noise + +white_noise = np.random.normal(0, 1, L) ## the normal distribution + +colored_noise = [] # colored_noise +random_walk = [0.0] +for i in range(L): + + old_total_noise = random_walk[i] + new_noise = white_noise[i] + random_walk.append(old_total_noise + new_noise) + + noise_1 = np.dot(-d[1 : nd + 1], colored_noise_init.transpose()) ##d * colored_noise_init + white_noise_init_array = np.insert(white_noise_init, 0, white_noise[i]) + noise_2 = np.dot(white_noise_init_array, c.transpose()) # c * white_noise + + colored_noise.append(noise_1 + noise_2) ## save the colored_noise + #update the data + for j in range(nd - 1, 0, -1): + colored_noise_init[j] = colored_noise_init[j - 1] + + colored_noise_init[0] = noise_2 + noise_1 + + for k in range(nc-1, 0, -1): + white_noise_init[k] = white_noise_init[k - 1] + + white_noise_init[0] = white_noise[i] + + +random_walk = np.array(random_walk) + +#figure +plt.figure(1) +ax = plt.subplot(2, 1, 1) +ax1 = plt.subplot(2, 1, 2) + +plt.sca(ax) +ax.plot(white_noise, 'Red', label = 'white_noise') +plt.title('white_noise') +#plt.xlabel("k") +plt.ylabel("amplitude") +ax.legend() + +plt.sca(ax1) +ax1.plot(colored_noise, 'Blue', label = 'colored_noise') +plt.title('colored_noise') +plt.xlabel("k") +plt.ylabel("amplitude") +plt.legend() + +plt.figure(2) + +ax3 = plt.subplot(1, 1, 1) +ax3.plot(random_walk, 'Green' ,label = "random_walk") +plt.title('random_walk') +plt.xlabel("k") +plt.ylabel("amplitude") +plt.legend() + +plt.show() + +exit() + + + + + diff --git a/example2/adaptive_mpc.py b/example2/adaptive_mpc.py new file mode 100644 index 000000000..e85d16cc7 --- /dev/null +++ b/example2/adaptive_mpc.py @@ -0,0 +1,25 @@ +import os + +import numpy as np +import matplotlib.pyplot as plt +from scipy.linalg import toeplitz +from scipy.linalg import pinv +from scipy.linalg import hankel +from scipy.linalg import svd +from scipy.sparse import triu +from control import tf, ss, mixsyn, step_response +#define the hankel matrix +def hank(g, k): + H = hankel(g, [1: len(g)/2, (1+k): len(g)/2 + k]) + return H + + +def ERAOKIDMPC(u, y, r, Nc, Np, lb, ub): + g = y * pinv(triu(toeplitz(u))) + + H0 = hank(g, 0) + H1 = hank(g, 1) + try: + [U, S, V] = + + \ No newline at end of file diff --git a/example2/forgetting_factor_recursive_least_square.py b/example2/forgetting_factor_recursive_least_square.py new file mode 100644 index 000000000..4ba08ed7f --- /dev/null +++ b/example2/forgetting_factor_recursive_least_square.py @@ -0,0 +1,90 @@ +import os +import numpy as np +import matplotlib.pyplot as plt +import numpy.matlib +#the least_square +a = np.array([1, -1.5, 0.7]); b = np.array([1, 0.5]); d = 3 # + +len_a = len(a) -1; len_b = len(b) - 1 ; L = 1500 + +Input_init = np.zeros(d + len_b) #the init_value of input +Output_init = np.zeros(len_a) # the init_value of ouput + + +white_noise = np.random.normal(0, 1, L) # the white_noise +white_noise_1 = np.sqrt(0.1) * np.random.normal(0, 1, L) + +#theta = np.append(a[1: len_a + 1], b) #the permeter of object + +theta_1 = np.zeros(len_a + len_b + 1) +P = np.eye(len_a + len_b + 1, len_a + len_b + 1) + +forgetting_factor = 0.98 #the forgetting_factor +phi = np.array([]) # the +thetae_1 = [] +thetae_2 = [] +thetae_3 = [] +thetae_4 = [] + +theta_real_1 = [] +theta_real_2 = [] +theta_real_3 = [] +theta_real_4 = [] + +for i in range(L): + if i == 500: + a = [1, -1, 0.4]; b = [1.5, 0.2] + + theta = np.append(a[1: len_a + 1], b) #the permeter of object + #print("the theta: ", theta) + theta_real_1.append(theta[0]) + theta_real_2.append(theta[1]) + theta_real_3.append(theta[2]) + theta_real_4.append(theta[3]) + + temp = np.append(-Output_init, Input_init[d - 1: d + len_b]) # X + temp_y = np.dot(temp, theta.transpose()) + white_noise_1[i] # Y + + phi = np.concatenate((phi, temp)) + temp_matrix = np.matrix(temp) + P_matrix = np.matrix(P) + + K = (P_matrix * temp_matrix.transpose()) /(forgetting_factor + temp_matrix * P_matrix *temp_matrix.transpose()) + thetae_temp = theta_1 + K * (temp_y - temp * theta_1) + + thetae_1.append(thetae_temp[0,0]) + thetae_2.append(thetae_temp[1,1]) + thetae_3.append(thetae_temp[2,2]) + thetae_4.append(thetae_temp[3,3]) + P = (np.eye(len_a + len_b + 1) - K * temp) * P /forgetting_factor + + #update the data + theta_1 = thetae_temp + + for k in range(d + len_b - 1, 0, -1): + Input_init[k] = Input_init[k - 1] + Input_init[0] = white_noise[i] + + for j in range(len_a - 1, 0, -1): + Output_init[j] = Output_init[j - 1] + Output_init[0] = temp_y + + +plt.figure(1) +ax = plt.subplot(1, 1, 1) + +plt.sca(ax) +ax.plot(thetae_1, 'Red', label = 'thetae_1') +ax.plot(thetae_2, 'Blue', label = 'thetae_2') +ax.plot(thetae_3, 'Green', label = 'thetae_3') +ax.plot(thetae_4, 'Yellow', label = 'thetae_4') +ax.plot(theta_real_1, 'brown', label = 'theta_real_1', linestyle ="-" ) +ax.plot(theta_real_2, 'cyan', label = 'theta_real_2', linestyle="-" ) +ax.plot(theta_real_3, 'burlywood', label = 'theta_real_3', linestyle="-" ) +ax.plot(theta_real_4, 'coral', label = 'theta_real_4', linestyle="-" ) +plt.title('thetae') +plt.xlabel("k") +plt.ylabel("value") + +ax.legend() +plt.show() diff --git a/example2/least_square.py b/example2/least_square.py new file mode 100644 index 000000000..0bdf9a5a7 --- /dev/null +++ b/example2/least_square.py @@ -0,0 +1,65 @@ +import os +import numpy as np +import matplotlib.pyplot as plt + +#the least_square +a = np.array([1, -1.5, 0.7]); b = np.array([1, 0.5]); d = 3 # + +len_a = len(a) -1; len_b = len(b) - 1 ; L =500 +Input_init = np.zeros(d + len_b) #the init_value of input +Output_init = np.zeros(len_a) # the init_value of ouput + +x1 = 1; x2 = 1; x3 = 1; x4 = 0; S = 1 # the init_value of move_register and square_wave + +white_noise = np.random.normal(0, 1, L) # the white_noise + +phi = np.array([]) # the +y = np.array([]) +U = np.array([]) #the inverse_M_sequence +theta = np.append(a[1: len_a + 1], b) #the permeter of object +for i in range(L): + # Y = AX + xi(noise) + temp = np.append(-Output_init, Input_init[d - 1: d + len_b]) # X + phi = np.concatenate((phi, temp)) + #print("the phi: ", phi) + + temp_y = np.dot(temp, theta.transpose()) + white_noise[i] # Y + #save the y + y = np.append(y, temp_y) + + #the inverse_M_sequence + IM = S ^ x4 + if IM == 0: + U = -1 + else: + U = 1 + S = not(S); M = x3 ^ x4 + + #update the data + x4 = x3; x3 = x2; x2 = x1; x1 = M + + for k in range(d + len_b - 1, 0, -1): + Input_init[k] = Input_init[k - 1] + Input_init[0] = U + + for j in range(len_a - 1, 0, -1): + Output_init[j] = Output_init[j - 1] + Output_init[0] = temp_y + +# the np.narray to np.matrix +y_matrix = np.matrix(y.reshape(len(y), 1)) + +phi_matrix = np.matrix(phi.reshape((L, 4))) + + +print("the phi_matrix_transpose*phi_matrix:", np.matmul(phi_matrix.transpose(), phi_matrix)) + + +temp_phi_phi = np.linalg.inv(np.matmul(phi_matrix.transpose(), phi_matrix)) +temp_phi_phi_phi = np.dot(temp_phi_phi, phi_matrix.transpose()) + +theta_estimate = np.dot(temp_phi_phi_phi, y) + +print("the theta_estimate: ", theta_estimate) + + diff --git a/example2/recursive_extended_least_square.py b/example2/recursive_extended_least_square.py new file mode 100644 index 000000000..5e1eb805d --- /dev/null +++ b/example2/recursive_extended_least_square.py @@ -0,0 +1,93 @@ +import os +import numpy as np +import matplotlib.pyplot as plt +import numpy.matlib +#the least_square +a = np.array([1, -1.5, 0.7]); b = np.array([1, 0.5]); c = np.array([1, -1, 0.2]);d = 3 # + +len_a = len(a) -1; len_b = len(b) - 1 ; len_c = len(c) - 1; L = 1500 + +Input_init = np.zeros(d + len_b) #the init_value of input +Output_init = np.zeros(len_a) # the init_value of ouput + +white_noise_init = np.zeros(len_c) +white_noise_extimate_init = np.zeros(len_c) + +white_noise = np.random.normal(0, 1, L) # the white_noise +white_noise_1 = np.sqrt(0.1) * np.random.normal(0, 1, L) + +theta_temp = np.append(a[1: len_a + 1], b) +theta = np.append(theta_temp, c[1: len_c + 1]) #the permeter of object + +theta_1 = np.zeros(len_a + len_b + 1 + len_c) +P = np.eye(len_a + len_b + 1 + len_c, len_a + len_b + 1 + len_c) + +forgetting_factor = 1 #the forgetting_factor +phi = np.array([]) # the +thetae_1 = [] +thetae_2 = [] +thetae_3 = [] +thetae_4 = [] +thetae_5 = [] +thetae_6 = [] + + +for i in range(L): + + temp_temp = np.append(-Output_init, Input_init[d - 1: d + len_b]) + temp = np.append(temp_temp, white_noise_init) # X + + temp_y = np.dot(temp, theta.transpose()) + white_noise_1[i] # Y + + temp_estimate_temp = np.append(-Output_init, Input_init[d - 1: d + len_b]) + temp_estimate = np.append(temp_estimate_temp, white_noise_extimate_init) + + + temp_estimate_matrix = np.matrix(temp_estimate) + P_matrix = np.matrix(P) + + K = (P_matrix * temp_estimate_matrix.transpose()) /(forgetting_factor + temp_estimate_matrix * P_matrix * temp_estimate_matrix.transpose()) + thetae_temp = theta_1 + K * (temp_y - temp_estimate * theta_1) + + #print("the size of thetae: ", thetae_temp.shape) + thetae_1.append(thetae_temp[0,0]) + thetae_2.append(thetae_temp[1,1]) + thetae_3.append(thetae_temp[2,2]) + thetae_4.append(thetae_temp[3,3]) + thetae_5.append(thetae_temp[4,4]) + thetae_6.append(thetae_temp[5,5]) + + P = (np.eye(len_a + len_b + len_c + 1) - K * temp_estimate_matrix) * P /forgetting_factor + ##the estimate of white + white_noise_estimate = temp_y - temp_estimate * thetae_temp + + #update the data + theta_1 = thetae_temp + + for k in range(d + len_b - 1, 0, -1): + Input_init[k] = Input_init[k - 1] + Input_init[0] = white_noise[i] + + for j in range(len_a - 1, 0, -1): + Output_init[j] = Output_init[j - 1] + Output_init[0] = temp_y + + +plt.figure(1) +ax = plt.subplot(1, 1, 1) + +plt.sca(ax) +ax.plot(thetae_1, 'Red', label = 'thetae_1') +ax.plot(thetae_2, 'Blue', label = 'thetae_2') +ax.plot(thetae_3, 'Green', label = 'thetae_3') +ax.plot(thetae_4, 'Yellow', label = 'thetae_4') +ax.plot(thetae_5, 'brown', label = 'thetae_5') +ax.plot(thetae_6, 'cyan', label = 'thetae_6') +#ax.plot(theta_real_3, 'burlywood', label = 'theta_real_3', linestyle="-" ) +#ax.plot(theta_real_4, 'coral', label = 'theta_real_4', linestyle="-" ) +plt.title('thetae') +plt.xlabel("k") +plt.ylabel("value") + +ax.legend() +plt.show() diff --git a/example2/recursive_gradient_correction.py b/example2/recursive_gradient_correction.py new file mode 100644 index 000000000..37d69cf31 --- /dev/null +++ b/example2/recursive_gradient_correction.py @@ -0,0 +1,70 @@ +import os +import numpy as np +import matplotlib.pyplot as plt +import numpy.matlib +#the least_square +a = np.array([1, -1.5, 0.7]); b = np.array([1, 0.5]);d = 3 # + +len_a = len(a) -1; len_b = len(b) - 1 ; L = 1500 + +Input_init = np.zeros(d + len_b) #the init_value of input +Output_init = np.zeros(len_a) # the init_value of ouput + +white_noise = np.random.normal(0, 1, L) # the white_noise + +theta = np.append(a[1: len_a + 1], b) + +theta_1 = np.zeros(len_a + len_b + 1) + +alpha = 1 +c = 0.1 + +phi = np.array([]) # the +thetae_1 = [] +thetae_2 = [] +thetae_3 = [] +thetae_4 = [] + + +for i in range(L): + + temp = np.append(-Output_init, Input_init[d - 1: d + len_b]) + + temp_y = np.dot(temp, theta.transpose())# Y + + thetae_temp = theta_1 + alpha * temp * (temp_y - temp * theta_1)/(np.dot(temp, temp) + c) + + #print("the size of thetae: ", thetae_temp.shape) + thetae_1.append(thetae_temp[0,0]) + thetae_2.append(thetae_temp[1,1]) + thetae_3.append(thetae_temp[2,2]) + thetae_4.append(thetae_temp[3,3]) + + #update the data + theta_1 = thetae_temp + + for k in range(d + len_b - 1, 0, -1): + Input_init[k] = Input_init[k - 1] + Input_init[0] = white_noise[i] + + for j in range(len_a - 1, 0, -1): + Output_init[j] = Output_init[j - 1] + Output_init[0] = temp_y + + +plt.figure(1) +ax = plt.subplot(1, 1, 1) + +plt.sca(ax) +ax.plot(thetae_1, 'Red', label = 'thetae_1') +ax.plot(thetae_2, 'Blue', label = 'thetae_2') +ax.plot(thetae_3, 'Green', label = 'thetae_3') +ax.plot(thetae_4, 'Yellow', label = 'thetae_4') +#ax.plot(theta_real_3, 'burlywood', label = 'theta_real_3', linestyle="-" ) +#ax.plot(theta_real_4, 'coral', label = 'theta_real_4', linestyle="-" ) +plt.title('thetae') +plt.xlabel("k") +plt.ylabel("value") + +ax.legend() +plt.show() diff --git a/example2/recursive_least_square.py b/example2/recursive_least_square.py new file mode 100644 index 000000000..86c437d49 --- /dev/null +++ b/example2/recursive_least_square.py @@ -0,0 +1,76 @@ +import os +import numpy as np +import matplotlib.pyplot as plt +import numpy.matlib + +#the least_square +a = np.array([1, -1.5, 0.7]); b = np.array([1, 0.5]); d = 3 # + +len_a = len(a) -1; len_b = len(b) - 1 ; L =400 +Input_init = np.zeros(d + len_b) #the init_value of input +Output_init = np.zeros(len_a) # the init_value of ouput + + +white_noise = np.random.normal(0, 1, L) # the white_noise +white_noise_1 = np.sqrt(0.1) * np.random.normal(0, 1, L) + +theta = np.append(a[1: len_a + 1], b) #the permeter of object + +theta_1 = np.zeros(len_a + len_b + 1) + +P = np.eye(len_a + len_b + 1, len_a + len_b + 1) + +phi = np.array([]) # the +thetae_1 = [] +thetae_2 = [] +thetae_3 = [] +thetae_4 = [] + + +for i in range(L): + # Y = AX + xi(noise) + temp = np.append(-Output_init, Input_init[d - 1: d + len_b]) # X + temp_y = np.dot(temp, theta.transpose()) + white_noise_1[i] # Y + + phi = np.concatenate((phi, temp)) + temp_matrix = np.matrix(temp) + P_matrix = np.matrix(P) + + K = (P_matrix * temp_matrix.transpose()) /(1 + temp_matrix * P_matrix *temp_matrix.transpose()) + thetae_temp = theta_1 + K * (temp_y - temp * theta_1) + + thetae_1.append(thetae_temp[0,0]) + thetae_2.append(thetae_temp[1,1]) + thetae_3.append(thetae_temp[2,2]) + thetae_4.append(thetae_temp[3,3]) + P = (np.eye(len_a + len_b + 1) - K * temp) * P + + #update the data + theta_1 = thetae_temp + + for k in range(d + len_b - 1, 0, -1): + Input_init[k] = Input_init[k - 1] + Input_init[0] = white_noise[i] + + for j in range(len_a - 1, 0, -1): + Output_init[j] = Output_init[j - 1] + Output_init[0] = temp_y + +# the np.narray to np.matrix + +plt.figure(1) +ax = plt.subplot(1, 1, 1) + +plt.sca(ax) +ax.plot(thetae_1, 'Red', label = 'thetae_1') +ax.plot(thetae_2, 'Blue', label = 'thetae_2') +ax.plot(thetae_3, 'Green', label = 'thetae_3') +ax.plot(thetae_4, 'Yellow', label = 'thetae_4') +plt.title('thetae') +plt.xlabel("k") +plt.ylabel("value") + +ax.legend() +plt.show() + + diff --git a/examples/M_sequence_and_inverse_M_sequence.py b/examples/M_sequence_and_inverse_M_sequence.py new file mode 100644 index 000000000..bddf1ed6e --- /dev/null +++ b/examples/M_sequence_and_inverse_M_sequence.py @@ -0,0 +1,51 @@ +import os +import numpy as np +import matplotlib.pyplot as plt + +L = 30 # the len of sequence +x1 = 1; x2 = 1; x3 = 1; x4 = 0 # the init_valce of register +#print("the x4 :" , x4) +S = 1 +U = [] # Inverse_M_sequence +M = [] # M_sequence +for i in range(1, L + 1, 1): + IM = S ^ x4 + if IM == 0: + U.append(-1) # Inverse_M_sequence + else: + U.append(1) # Inverse_M_sequence + + S = not(S) + m = x3 ^ x4 + M.append( m ) # M_sequence + x4 = x3; x3 = x2; x2 = x1 ; x1 = m + +print("the M_sequence:", M) +print("the_inverse_M_sequence:", U) +#figure +plt.figure(1) +ax = plt.subplot(2, 1, 1) +ax1 = plt.subplot(2, 1, 2) + +plt.sca(ax) +ax.plot(M, 'Red', label = 'M_sequence') +plt.title('M_sequence') +#plt.xlabel("k") +plt.ylabel("amplitude") +ax.legend() + +plt.sca(ax1) +ax1.plot(U, 'Blue', label = 'Inverse_M_sequence') +plt.title('Inverse_M_sequence') +plt.xlabel("k") +plt.ylabel("amplitude") +plt.legend() + + +plt.show() + +exit() + + + + diff --git a/examples/Noise_Singal_ratio_SISO.py b/examples/Noise_Singal_ratio_SISO.py new file mode 100644 index 000000000..c07d56edd --- /dev/null +++ b/examples/Noise_Singal_ratio_SISO.py @@ -0,0 +1,33 @@ +import os +import numpy as np +import matplotlib.pyplot as plt + +a = np.array([1, -0.4]); b = np.array([1]); c = np.array([1, -0.4]) +na = len(a) -1; nb = len(b) - 1; nc = len(c) - 1 +#print("the na: ", na) + +n = max(na, nb, nc) +print("the max of (na, nb, nc):", n) +a0 = a; b0 = b; c0 = c +for i in range(na, n, 1): + a0 = np.append(a0, 0) + +for i in range(nb, n, 1): + b0 = np.append(b0, 0) + +for i in range(nc, n, 1): + c0 = np.append(c0, 0) + +print("the a0: ", b0) + +p = []; qg = []; qh = [] +deltau2 = 1; deltav2 = 1 +for i in range(n): + p.append(a0[i]) + qg.append(b0[i]) + qh.append(c0[i]) + +for i in range(n - 1, 0 , -1): + for j in range(0, 2, 1): + p_2[j, i] = p[0] * p[i] + diff --git a/examples/White_Noise_And_Colored_Noise.py b/examples/White_Noise_And_Colored_Noise.py new file mode 100644 index 000000000..545acf973 --- /dev/null +++ b/examples/White_Noise_And_Colored_Noise.py @@ -0,0 +1,82 @@ +import os +import numpy as np +import matplotlib.pyplot as plt + +L = 500 # the len of noise + +# e(k) = c/d * f(k) f(k) is white_noise and e(k) is colored_noise +d = np.array([1, -1.5, 0.7, 0.1]) # the c +c = np.array([1, 0.5, 0.2]) # the d + +nd = len(d) - 1 # +nc = len(c) - 1 # + +white_noise_init = np.zeros(nc) # the init_value of white_noise + +colored_noise_init = np.zeros(nd) # the init_valuw of colored_noise + +white_noise = np.random.normal(0, 1, L) ## the normal distribution + +colored_noise = [] # colored_noise +random_walk = [0.0] +for i in range(L): + + old_total_noise = random_walk[i] + new_noise = white_noise[i] + random_walk.append(old_total_noise + new_noise) + + noise_1 = np.dot(-d[1 : nd + 1], colored_noise_init.transpose()) ##d * colored_noise_init + white_noise_init_array = np.insert(white_noise_init, 0, white_noise[i]) + noise_2 = np.dot(white_noise_init_array, c.transpose()) # c * white_noise + + colored_noise.append(noise_1 + noise_2) ## save the colored_noise + #update the data + for j in range(nd - 1, 0, -1): + colored_noise_init[j] = colored_noise_init[j - 1] + + colored_noise_init[0] = noise_2 + noise_1 + + for k in range(nc-1, 0, -1): + white_noise_init[k] = white_noise_init[k - 1] + + white_noise_init[0] = white_noise[i] + + +random_walk = np.array(random_walk) + +#figure +plt.figure(1) +ax = plt.subplot(2, 1, 1) +ax1 = plt.subplot(2, 1, 2) + +plt.sca(ax) +ax.plot(white_noise, 'Red', label = 'white_noise') +plt.title('white_noise') +#plt.xlabel("k") +plt.ylabel("amplitude") +ax.legend() + +plt.sca(ax1) +ax1.plot(colored_noise, 'Blue', label = 'colored_noise') +plt.title('colored_noise') +plt.xlabel("k") +plt.ylabel("amplitude") +plt.legend() + +plt.figure(2) + +ax3 = plt.subplot(1, 1, 1) +ax3.plot(random_walk, 'Green' ,label = "random_walk") +plt.title('random_walk') +plt.xlabel("k") +plt.ylabel("amplitude") +plt.legend() + +plt.show() + +exit() + + + + + diff --git a/examples/adaptive_mpc.py b/examples/adaptive_mpc.py new file mode 100644 index 000000000..e85d16cc7 --- /dev/null +++ b/examples/adaptive_mpc.py @@ -0,0 +1,25 @@ +import os + +import numpy as np +import matplotlib.pyplot as plt +from scipy.linalg import toeplitz +from scipy.linalg import pinv +from scipy.linalg import hankel +from scipy.linalg import svd +from scipy.sparse import triu +from control import tf, ss, mixsyn, step_response +#define the hankel matrix +def hank(g, k): + H = hankel(g, [1: len(g)/2, (1+k): len(g)/2 + k]) + return H + + +def ERAOKIDMPC(u, y, r, Nc, Np, lb, ub): + g = y * pinv(triu(toeplitz(u))) + + H0 = hank(g, 0) + H1 = hank(g, 1) + try: + [U, S, V] = + + \ No newline at end of file diff --git a/examples/forgetting_factor_recursive_least_square.py b/examples/forgetting_factor_recursive_least_square.py new file mode 100644 index 000000000..4ba08ed7f --- /dev/null +++ b/examples/forgetting_factor_recursive_least_square.py @@ -0,0 +1,90 @@ +import os +import numpy as np +import matplotlib.pyplot as plt +import numpy.matlib +#the least_square +a = np.array([1, -1.5, 0.7]); b = np.array([1, 0.5]); d = 3 # + +len_a = len(a) -1; len_b = len(b) - 1 ; L = 1500 + +Input_init = np.zeros(d + len_b) #the init_value of input +Output_init = np.zeros(len_a) # the init_value of ouput + + +white_noise = np.random.normal(0, 1, L) # the white_noise +white_noise_1 = np.sqrt(0.1) * np.random.normal(0, 1, L) + +#theta = np.append(a[1: len_a + 1], b) #the permeter of object + +theta_1 = np.zeros(len_a + len_b + 1) +P = np.eye(len_a + len_b + 1, len_a + len_b + 1) + +forgetting_factor = 0.98 #the forgetting_factor +phi = np.array([]) # the +thetae_1 = [] +thetae_2 = [] +thetae_3 = [] +thetae_4 = [] + +theta_real_1 = [] +theta_real_2 = [] +theta_real_3 = [] +theta_real_4 = [] + +for i in range(L): + if i == 500: + a = [1, -1, 0.4]; b = [1.5, 0.2] + + theta = np.append(a[1: len_a + 1], b) #the permeter of object + #print("the theta: ", theta) + theta_real_1.append(theta[0]) + theta_real_2.append(theta[1]) + theta_real_3.append(theta[2]) + theta_real_4.append(theta[3]) + + temp = np.append(-Output_init, Input_init[d - 1: d + len_b]) # X + temp_y = np.dot(temp, theta.transpose()) + white_noise_1[i] # Y + + phi = np.concatenate((phi, temp)) + temp_matrix = np.matrix(temp) + P_matrix = np.matrix(P) + + K = (P_matrix * temp_matrix.transpose()) /(forgetting_factor + temp_matrix * P_matrix *temp_matrix.transpose()) + thetae_temp = theta_1 + K * (temp_y - temp * theta_1) + + thetae_1.append(thetae_temp[0,0]) + thetae_2.append(thetae_temp[1,1]) + thetae_3.append(thetae_temp[2,2]) + thetae_4.append(thetae_temp[3,3]) + P = (np.eye(len_a + len_b + 1) - K * temp) * P /forgetting_factor + + #update the data + theta_1 = thetae_temp + + for k in range(d + len_b - 1, 0, -1): + Input_init[k] = Input_init[k - 1] + Input_init[0] = white_noise[i] + + for j in range(len_a - 1, 0, -1): + Output_init[j] = Output_init[j - 1] + Output_init[0] = temp_y + + +plt.figure(1) +ax = plt.subplot(1, 1, 1) + +plt.sca(ax) +ax.plot(thetae_1, 'Red', label = 'thetae_1') +ax.plot(thetae_2, 'Blue', label = 'thetae_2') +ax.plot(thetae_3, 'Green', label = 'thetae_3') +ax.plot(thetae_4, 'Yellow', label = 'thetae_4') +ax.plot(theta_real_1, 'brown', label = 'theta_real_1', linestyle ="-" ) +ax.plot(theta_real_2, 'cyan', label = 'theta_real_2', linestyle="-" ) +ax.plot(theta_real_3, 'burlywood', label = 'theta_real_3', linestyle="-" ) +ax.plot(theta_real_4, 'coral', label = 'theta_real_4', linestyle="-" ) +plt.title('thetae') +plt.xlabel("k") +plt.ylabel("value") + +ax.legend() +plt.show() diff --git a/examples/least_square.py b/examples/least_square.py new file mode 100644 index 000000000..0bdf9a5a7 --- /dev/null +++ b/examples/least_square.py @@ -0,0 +1,65 @@ +import os +import numpy as np +import matplotlib.pyplot as plt + +#the least_square +a = np.array([1, -1.5, 0.7]); b = np.array([1, 0.5]); d = 3 # + +len_a = len(a) -1; len_b = len(b) - 1 ; L =500 +Input_init = np.zeros(d + len_b) #the init_value of input +Output_init = np.zeros(len_a) # the init_value of ouput + +x1 = 1; x2 = 1; x3 = 1; x4 = 0; S = 1 # the init_value of move_register and square_wave + +white_noise = np.random.normal(0, 1, L) # the white_noise + +phi = np.array([]) # the +y = np.array([]) +U = np.array([]) #the inverse_M_sequence +theta = np.append(a[1: len_a + 1], b) #the permeter of object +for i in range(L): + # Y = AX + xi(noise) + temp = np.append(-Output_init, Input_init[d - 1: d + len_b]) # X + phi = np.concatenate((phi, temp)) + #print("the phi: ", phi) + + temp_y = np.dot(temp, theta.transpose()) + white_noise[i] # Y + #save the y + y = np.append(y, temp_y) + + #the inverse_M_sequence + IM = S ^ x4 + if IM == 0: + U = -1 + else: + U = 1 + S = not(S); M = x3 ^ x4 + + #update the data + x4 = x3; x3 = x2; x2 = x1; x1 = M + + for k in range(d + len_b - 1, 0, -1): + Input_init[k] = Input_init[k - 1] + Input_init[0] = U + + for j in range(len_a - 1, 0, -1): + Output_init[j] = Output_init[j - 1] + Output_init[0] = temp_y + +# the np.narray to np.matrix +y_matrix = np.matrix(y.reshape(len(y), 1)) + +phi_matrix = np.matrix(phi.reshape((L, 4))) + + +print("the phi_matrix_transpose*phi_matrix:", np.matmul(phi_matrix.transpose(), phi_matrix)) + + +temp_phi_phi = np.linalg.inv(np.matmul(phi_matrix.transpose(), phi_matrix)) +temp_phi_phi_phi = np.dot(temp_phi_phi, phi_matrix.transpose()) + +theta_estimate = np.dot(temp_phi_phi_phi, y) + +print("the theta_estimate: ", theta_estimate) + + diff --git a/examples/python_test.py b/examples/python_test.py new file mode 100644 index 000000000..f408cd0d5 --- /dev/null +++ b/examples/python_test.py @@ -0,0 +1,51 @@ +import os +import numpy as np +import matplotlib.pyplot as plt +import numpy.matlib + +e = np.array([1, 2, 3]) +f = np.array([2, 3, 4]) +p = np.matlib.eye(3, 3) +#print("the fist rows:", p[0,:]) +print ("the type of p:", type(p)) +g = np.zeros(3) +print("the g:", g) +#h = np.matrix.ye(3) + +print("the_res: ", p * f.transpose()) + + +a = np.append(e[1:3], f,axis = 0) +m = np.array([[1,2,3], [2,2,3], [2,3,4]]) +print("the inverse:", type(np.linalg.inv(m))) +#g = np.vdot(e, f.transpose()) +print("the g: ", e.transpose() * f) + +print("the a: ", a) + +for i in range(len(e) -1, 0, -1): + e[i] = e[i - 1] + print("the e:", e) + +#e = np.delete(e, 0) +#e = np.insert(e, 0 ,5) +e[0] = 5 +print("the e:" , e) + + +f = np.insert(f, 0, 5) +print("the f :", f) + +f_list = list(f) + + +f_list.insert(0, 1) +print("the f:", f_list) + +f_array = np.asarray(f_list) + +print("the f:", f) + +#g = np.dot(e, f.transpose()) + +#print("the c:", g) \ No newline at end of file diff --git a/examples/recursive_least_square.py b/examples/recursive_least_square.py new file mode 100644 index 000000000..86c437d49 --- /dev/null +++ b/examples/recursive_least_square.py @@ -0,0 +1,76 @@ +import os +import numpy as np +import matplotlib.pyplot as plt +import numpy.matlib + +#the least_square +a = np.array([1, -1.5, 0.7]); b = np.array([1, 0.5]); d = 3 # + +len_a = len(a) -1; len_b = len(b) - 1 ; L =400 +Input_init = np.zeros(d + len_b) #the init_value of input +Output_init = np.zeros(len_a) # the init_value of ouput + + +white_noise = np.random.normal(0, 1, L) # the white_noise +white_noise_1 = np.sqrt(0.1) * np.random.normal(0, 1, L) + +theta = np.append(a[1: len_a + 1], b) #the permeter of object + +theta_1 = np.zeros(len_a + len_b + 1) + +P = np.eye(len_a + len_b + 1, len_a + len_b + 1) + +phi = np.array([]) # the +thetae_1 = [] +thetae_2 = [] +thetae_3 = [] +thetae_4 = [] + + +for i in range(L): + # Y = AX + xi(noise) + temp = np.append(-Output_init, Input_init[d - 1: d + len_b]) # X + temp_y = np.dot(temp, theta.transpose()) + white_noise_1[i] # Y + + phi = np.concatenate((phi, temp)) + temp_matrix = np.matrix(temp) + P_matrix = np.matrix(P) + + K = (P_matrix * temp_matrix.transpose()) /(1 + temp_matrix * P_matrix *temp_matrix.transpose()) + thetae_temp = theta_1 + K * (temp_y - temp * theta_1) + + thetae_1.append(thetae_temp[0,0]) + thetae_2.append(thetae_temp[1,1]) + thetae_3.append(thetae_temp[2,2]) + thetae_4.append(thetae_temp[3,3]) + P = (np.eye(len_a + len_b + 1) - K * temp) * P + + #update the data + theta_1 = thetae_temp + + for k in range(d + len_b - 1, 0, -1): + Input_init[k] = Input_init[k - 1] + Input_init[0] = white_noise[i] + + for j in range(len_a - 1, 0, -1): + Output_init[j] = Output_init[j - 1] + Output_init[0] = temp_y + +# the np.narray to np.matrix + +plt.figure(1) +ax = plt.subplot(1, 1, 1) + +plt.sca(ax) +ax.plot(thetae_1, 'Red', label = 'thetae_1') +ax.plot(thetae_2, 'Blue', label = 'thetae_2') +ax.plot(thetae_3, 'Green', label = 'thetae_3') +ax.plot(thetae_4, 'Yellow', label = 'thetae_4') +plt.title('thetae') +plt.xlabel("k") +plt.ylabel("value") + +ax.legend() +plt.show() + +