1

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

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()

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)

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()

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()

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()

pytxt_convertor/test1.py

@@ -0,0 +1,3 @@
+import numpy as np
+
+print(1)

pytxt_convertor/test2.py

@@ -0,0 +1,3 @@
+import numpy as np
+
+print(1)

pytxt_convertor/test3.py

@@ -0,0 +1,3 @@
+import numpy as np
+
+print(1)

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()

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)