diff --git a/.DS_Store b/.DS_Store index 9091ca9..3027fd4 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/__pycache__/control_math.cpython-38.pyc b/__pycache__/control_math.cpython-38.pyc index 78044b0..d970bcc 100644 Binary files a/__pycache__/control_math.cpython-38.pyc and b/__pycache__/control_math.cpython-38.pyc differ diff --git a/control_math.py b/control_math.py index 4d776d2..ecf9788 100644 --- a/control_math.py +++ b/control_math.py @@ -3,6 +3,7 @@ from sympy import simplify, cancel, Poly, fraction, solve import matplotlib.pyplot as plt from scipy.linalg import expm +from scipy import integrate from itertools import chain, zip_longest @@ -145,10 +146,7 @@ def __init__(self, A, B, C, D, dt): identity = np.identity(n_states) self.expM_zoh_x = expm(A * dt) - try: - self.expM_zoh_u = np.linalg.inv(A).dot(self.expM_zoh_x - identity).dot(B) - except: - self.expM_zoh_u = B * dt + self.expM_zoh_u = np.linalg.pinv(A).dot(self.expM_zoh_x - identity).dot(B) n_inputs = self.B.shape[1] M_linear = np.block( [ @@ -173,6 +171,47 @@ def __init__(self, A, B, C, D, dt): self.Ad_step = expM_step[:n_states, :n_states] self.Bd1_step = expM_step[:n_states, n_states:] + def model(t, vX, Force): + vU = Force + vX = vX.reshape(len(self.B), 1) + dx = self.A.dot(vX) + self.B.dot(vU) + return dx + + self.ssmodel = model + + # def __init__(self, mass, damping_ratio, stiffness, R, L, kf, fs_pos, fs_current): + # self.mass = mass + # self.damping_ratio = damping_ratio + # self.stiffness = stiffness + # self.R = R + # self.L = L + # self.kf = kf + # self.A = np.mat( + # [[0, 1], [-self.stiffness / self.mass, -self.damping_ratio / self.mass]] + # ) + # self.B = np.mat([[0], [1 / self.mass]]) + # self.C = np.mat([1, 0]) + # self.D = np.mat([0]) + # + # def model(t, vX, Force): + # vU = Force + # vX = vX.reshape(len(self.B), 1) + # dx = self.A.dot(vX) + self.B.dot(vU) + # return dx + # + # self.ssmodel = model + # self.Xstates = np.zeros((self.A.shape[1], 1)) + # self.T = 1 / fs_current + # + # + # def run(self, t1, force): + # r = integrate.ode(self.ssmodel).set_integrator("dopri5") + # r.set_initial_value(self.Xstates, t1).set_f_params(force) + # r.integrate(r.t + self.T) + # self.Xstates = r.y + # self.pos = self.C.dot(self.Xstates) + # return self.pos + def response(self, u, method="zoh"): last_u = self.last_u u = np.asarray(u).reshape((-1, 1)) @@ -198,6 +237,14 @@ def response(self, u, method="zoh"): + np.dot(self.Bd1_linear, u) ) self.y_output = self.C.dot(x_state) + self.D.dot(u) + elif method == "ode": + r = integrate.ode(self.ssmodel).set_integrator("dopri5") + r.set_initial_value(self.x_state, 0).set_f_params(self.last_u) + r.integrate(r.t + self.dt) + x_state = r.y + self.y_output = self.C.dot(x_state) + # return self.y_output, self.x_state + self.last_u = u self.x_state = x_state return self.y_output, x_state diff --git a/fft_conv.py b/fft_conv.py index 245024d..4ea2bb7 100644 --- a/fft_conv.py +++ b/fft_conv.py @@ -1,10 +1,13 @@ import numpy as np import matplotlib.pyplot as plt +import scipy.signal from scipy.fftpack import fft, ifft, fftshift, fftfreq import time +import numpy -a = np.random.random(200000) -b = np.random.random(200000) +scipy.signal.tf2sos() +a = np.random.random(3) +b = np.random.random(3) T1 = time.time() c = np.correlate(a, b, "full") T2 = time.time() diff --git a/fft_factorization.py b/fft_factorization.py new file mode 100644 index 0000000..93b5c9b --- /dev/null +++ b/fft_factorization.py @@ -0,0 +1,40 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.fftpack import fft, ifft, fftshift, fftfreq +import time + +a = np.array([1, 3, 2, 4]) +b = np.array([5, 3, 2, 4]) +b = a +a0 = np.array([5, 5, 5, 5]) +b0 = np.array([1, 1, 1, 1]) +c = np.correlate(a, b, "full") +c0 = np.correlate(a0, b0, "full") + +cc = np.correlate(a, a, "full") +aa = ifft(np.sqrt((fft(fftshift(cc))))) +np.correlate(aa, np.flip(aa), "full") +np.correlate(aa, aa, "full") +num = len(a) +left_pad = round((num - 1) / 2 + 0.1) +right_pad = num - 1 - left_pad +a1 = np.pad(a, (left_pad, right_pad), "constant", constant_values=0) +b1 = np.pad(np.flip(b), (left_pad, right_pad), "constant", constant_values=0) +ahat = fft(a1) +bhat = fft(b1) +chat = ahat * ahat + +a01 = np.pad(a0, (left_pad, right_pad), "constant", constant_values=0) +b01 = np.pad(np.flip(b0), (left_pad, right_pad), "constant", constant_values=0) +a0hat = fft(a01) +b0hat = fft(b01) +c0hat = a0hat * a0hat + +c1 = np.real(ifft(chat)) +c1 = fftshift(c1) +if num % 2 == 1: + c1 = np.roll(c1, 1) +print(c) +print(c1) +print(np.allclose(c, c1)) +print(c0) diff --git a/pytxt_convertor/.DS_Store b/pytxt_convertor/.DS_Store new file mode 100644 index 0000000..5008ddf Binary files /dev/null and b/pytxt_convertor/.DS_Store differ diff --git a/pytxt_convertor/all_content.txt b/pytxt_convertor/all_content.txt new file mode 100644 index 0000000..3ce8472 --- /dev/null +++ b/pytxt_convertor/all_content.txt @@ -0,0 +1,64 @@ +;start_character;test1.py;end_character; +import numpy as np + +print(1) +;start_character;test2.py;end_character; +import numpy as np + +print(1) +;start_character;test3.py;end_character; +import numpy as np + +print(1) +;start_character;llllllllllllllllllllllllllllllllllllllllll.py;end_character; +def get_text_file(filename): + f = open(filename, "r") + _content = f.read() + f.close() + return _content + + +all_content = get_text_file("all_content.txt") +contents = all_content.split(";start_character;")[1:] +num = len(contents) +for i in range(num - 1): + if i < num - 2: + content = contents[i] + file_contents = content.split((";end_character;\n")) + + else: + content = contents[i] + file_contents = content.split((";end_character;\n")) + file_contents[1] = file_contents[1] + contents[i + 1] + file_name = "1" + file_contents[0] + + file_content = file_contents[1] + + fh = open(file_name, "w", encoding="utf-8") + fh.write(file_content) + fh.close() +;start_character;lllllllllllllllllllllllllllllllllllllllllll.py;end_character; +import os + + +def get_text_file(filename): + f = open(filename, "r") + _content = f.read() + f.close() + return _content + + +path = os.getcwd() +files = os.listdir(path) +files.sort(key=len) +all_content = "" +for file_name in files: + if file_name[-3:] == ".py": + start_character = ";start_character;" + file_name + ";end_character;\n" + content = get_text_file(file_name) + new_content = start_character + content + all_content = all_content + new_content + +fh = open("all_content.txt", "w", encoding="utf-8") +fh.write(all_content) +fh.close() diff --git a/pytxt_convertor/llllllllllllllllllllllllllllllllllllllllll.py b/pytxt_convertor/llllllllllllllllllllllllllllllllllllllllll.py new file mode 100644 index 0000000..9f9e229 --- /dev/null +++ b/pytxt_convertor/llllllllllllllllllllllllllllllllllllllllll.py @@ -0,0 +1,26 @@ +def get_text_file(filename): + f = open(filename, "r") + _content = f.read() + f.close() + return _content + + +all_content = get_text_file("all_content.txt") +contents = all_content.split(";start_character;")[1:] +num = len(contents) +for i in range(num - 1): + if i < num - 2: + content = contents[i] + file_contents = content.split((";end_character;\n")) + + else: + content = contents[i] + file_contents = content.split((";end_character;\n")) + file_contents[1] = file_contents[1] + contents[i + 1] + file_name = "1" + file_contents[0] + + file_content = file_contents[1] + + fh = open(file_name, "w", encoding="utf-8") + fh.write(file_content) + fh.close() diff --git a/pytxt_convertor/lllllllllllllllllllllllllllllllllllllllllll.py b/pytxt_convertor/lllllllllllllllllllllllllllllllllllllllllll.py new file mode 100644 index 0000000..8e91fc2 --- /dev/null +++ b/pytxt_convertor/lllllllllllllllllllllllllllllllllllllllllll.py @@ -0,0 +1,24 @@ +import os + + +def get_text_file(filename): + f = open(filename, "r") + _content = f.read() + f.close() + return _content + + +path = os.getcwd() +files = os.listdir(path) +files.sort(key=len) +all_content = "" +for file_name in files: + if file_name[-3:] == ".py": + start_character = ";start_character;" + file_name + ";end_character;\n" + content = get_text_file(file_name) + new_content = start_character + content + all_content = all_content + new_content + +fh = open("all_content.txt", "w", encoding="utf-8") +fh.write(all_content) +fh.close() diff --git a/pytxt_convertor/test1.py b/pytxt_convertor/test1.py new file mode 100644 index 0000000..756cbb9 --- /dev/null +++ b/pytxt_convertor/test1.py @@ -0,0 +1,3 @@ +import numpy as np + +print(1) diff --git a/pytxt_convertor/test2.py b/pytxt_convertor/test2.py new file mode 100644 index 0000000..756cbb9 --- /dev/null +++ b/pytxt_convertor/test2.py @@ -0,0 +1,3 @@ +import numpy as np + +print(1) diff --git a/pytxt_convertor/test3.py b/pytxt_convertor/test3.py new file mode 100644 index 0000000..756cbb9 --- /dev/null +++ b/pytxt_convertor/test3.py @@ -0,0 +1,3 @@ +import numpy as np + +print(1) diff --git a/sys_id_fft.py b/sys_id_fft.py index 6831325..75629bf 100644 --- a/sys_id_fft.py +++ b/sys_id_fft.py @@ -8,11 +8,11 @@ k = 0 b = 0 -# A = np.array([[0, 1], [-k / m, -b / m]]) -# B = np.array([[0], [1 / m]]) -# C = np.array([1, 0]) -# D = np.array([0]) -# rigid_plant_num, rigid_plant_den = ss2tf(A, B, C, D) +A = np.array([[0, 1], [-k / m, -b / m]]) +B = np.array([[0], [1 / m]]) +C = np.array([1, 0]) +D = np.array([0]) +rigid_plant_num, rigid_plant_den = ss2tf(A, B, C, D) # res_freq = 200 anti_res_freq = 150 @@ -28,17 +28,17 @@ ) res_den = np.array([1, 2 * beta2 * res_omega, res_omega ** 2, 0, 0]) -# num = np.convolve(res_num, rigid_plant_num.flatten()) -# den = np.convolve(res_den, rigid_plant_den.flatten()) +num = np.convolve(res_num, rigid_plant_num.flatten()) +den = np.convolve(res_den, rigid_plant_den.flatten()) -num = res_num -den = res_den -A, B, C, D = tf2ss(num, den) +# num = res_num +# den = res_den +A, B, C, D = tf2ss(res_num, res_den) ss = StateSpaceModel(A, B, C, D, DT) """Chirp参数""" start_freq = 50 -end_freq = 5000 +end_freq = 5100 start_freq_ = 0.8 * start_freq end_freq_ = 1.1 * end_freq # 扫频时间 @@ -57,7 +57,7 @@ y = np.zeros_like(u) for i in range(len(u)): input_sig = u[i] - y_output, x_state = ss.response(input_sig, method="zoh2") + y_output, x_state = ss.response(input_sig, method="linear") y[i] = y_output p = y y = np.diff(y, 2) / DT / DT @@ -95,7 +95,7 @@ * (1 - np.e ** (-1j * 2 * pi * f_u * DT)) ** 2 / (1j * 2 * pi * f_u * DT) ** 2 ) -fw = fw / dd_decay / zoh_decay +fw = fw / zoh_decay / zoh_decay # / zoh_decay plt.figure() plt.plot(t, u, label="u") plt.legend() @@ -112,22 +112,22 @@ plt.xscale("log") plt.plot(f_u, np.angle(fw, deg=True)) # dd_decay -plt.figure(figsize=(14, 4)) -plt.subplot(121) -plt.xscale("log") -plt.plot(f_u, 20 * np.log10(np.abs(dd_decay))) - -plt.subplot(122) -plt.xscale("log") -plt.plot(f_u, np.angle(dd_decay, deg=True)) - -# dd_decay -plt.figure(figsize=(14, 4)) -plt.subplot(121) -plt.xscale("log") -plt.plot(f_u, 20 * np.log10(np.abs(zoh_decay))) - -plt.subplot(122) -plt.xscale("log") -plt.plot(f_u, np.angle(zoh_decay, deg=True)) +# plt.figure(figsize=(14, 4)) +# plt.subplot(121) +# plt.xscale("log") +# plt.plot(f_u, 20 * np.log10(np.abs(dd_decay))) +# +# plt.subplot(122) +# plt.xscale("log") +# plt.plot(f_u, np.angle(dd_decay, deg=True)) +# +# # dd_decay +# plt.figure(figsize=(14, 4)) +# plt.subplot(121) +# plt.xscale("log") +# plt.plot(f_u, 20 * np.log10(np.abs(zoh_decay))) +# +# plt.subplot(122) +# plt.xscale("log") +# plt.plot(f_u, np.angle(zoh_decay, deg=True)) plt.show() diff --git a/test_file.py b/test_file.py index 72b0ed2..01d380d 100644 --- a/test_file.py +++ b/test_file.py @@ -74,8 +74,8 @@ input_sig = u[i] y_output, x_state = sys_ss.response(input_sig, method="zoh2") y[i] = y_output -# y = np.diff(y, 2) / DT / DT -# y = np.pad(y, (2, 0), "constant", constant_values=(0, 0)) +y = np.diff(y, 2) / DT / DT +y = np.pad(y, (2, 0), "constant", constant_values=(0, 0)) u_detrend = u - np.mean(u)