diff --git a/3/Active.py b/3/Active.py new file mode 100644 index 0000000..8d7140b --- /dev/null +++ b/3/Active.py @@ -0,0 +1,53 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.signal import StateSpace, lsim + +# Define system matrices +A = np.array([[0, 0, 1, 0], + [0, 0, 0, 1], + [-10, 10, -2, 2], + [60, -660, 12, -12]]) + +b1 = np.array([[0], [0], [0.0033], [-0.02]]) +b2 = np.array([[0], [0], [0], [600]]) +B = np.hstack((b1, b2)) +C = np.array([[1, 0, 0, 0]]) +D = np.array([[0]]) + +# Create state space system +active_suspension = StateSpace(A, B, C, D) + +# Define time vector and initial conditions +t = np.arange(0, 7.01, 0.01) +x0 = np.array([0.2, 0, 0, 0]) + +# Initial response simulation +t, y, x = lsim(active_suspension, 0, t, X0=x0) + +# Plot initial response +plt.figure() +plt.plot(t, x[:, 0], 'k', label='x1') +plt.plot(t, x[:, 1], 'k-.', label='x2') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.legend(['x1', 'x2']) +plt.title('Initial Response') +plt.show() + +# Generate input signal u +u = 0.1 * (np.sin(5*t) + np.sin(9*t) + np.sin(13*t) + np.sin(17*t) + np.sin(21*t)) + +# Simulate the system with input u +t, y, x = lsim(active_suspension, u, t) + +# Plot the simulation result +plt.figure() +plt.plot(t, x[:, 0], 'k', label='x1') +plt.plot(t, x[:, 1], 'k-.', label='x2') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.legend(['x1', 'x2']) +plt.title('Simulation Result') +plt.show() diff --git a/3/DCmotor.py b/3/DCmotor.py new file mode 100644 index 0000000..bdf5b0a --- /dev/null +++ b/3/DCmotor.py @@ -0,0 +1,53 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.signal import lsim, square + +# Simulation of the DC motor +# Copyright Hamid D. Taghirad 2013 + +# Define system matrices +A = np.array([[0, 1, 0], + [0, 0, 4.438], + [0, -12, -24]]) + +b1 = np.array([[0], [0], [20]]) +b2 = np.array([[0], [-7.396], [0]]) +B = np.hstack((b1, b2)) + +C = np.array([[1, 0, 0], + [0, 1, 0]]) + +D = np.array([[0]]) + +# Create state space system +DC_motor = (A, B, C, D) + +# Define time vector +t = np.arange(0, 4.01, 0.01) +N = len(t) + +# A Simple way to generate input u +u = np.zeros((2, N)) +for i in range(N): + if t[i] < 2: + u[0, i] = 3 + else: + u[0, i] = -3 + +# A Professional way to generate input u +u = 6 * square(2 * np.pi * 0.25 * t) - 3 # Generate square wave with period 4 and amplitude 6 + +# Simulate the system +t, y, x = lsim(DC_motor, u, t) + +# Plot the result +plt.figure() +plt.plot(t, x[:, 0], 'k', label='theta') +plt.plot(t, x[:, 1], 'k-.', label='omega') +plt.plot(t, x[:, 2], 'k:', label='i') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.legend(['theta', 'omega', 'i']) +plt.title('Simulation Result - DC Motor') +plt.show() diff --git a/3/DCmotor_transfun.py b/3/DCmotor_transfun.py new file mode 100644 index 0000000..3de341c --- /dev/null +++ b/3/DCmotor_transfun.py @@ -0,0 +1,37 @@ +import numpy as np +import control + +# Transfer functions for the DC motor + +# Define the state-space matrices +A = np.array([[0, 1, 0], + [0, 0, 4.438], + [0, -12, -24]]) + +b1 = np.array([[0], + [0], + [20]]) + +b2 = np.array([[0], + [-7.396], + [0]]) + +B = np.hstack((b1, b2)) +C = np.array([[1, 0, 0]]) # Note only \theta is used as output +D = np.array([[0, 0]]) + +# Create state-space system +DCM = control.ss(A, B, C, D) + +# Conversion to transfer function +DCM_tf = control.tf(DCM) + +# Conversion to zero-pole-gain form +DCM_zpk = control.zpk(DCM) + +# Display the results +print("Transfer Function:") +print(DCM_tf) + +print("\nZero-Pole-Gain:") +print(DCM_zpk) diff --git a/3/activate_this.py b/3/activate_this.py new file mode 100644 index 0000000..8d80882 --- /dev/null +++ b/3/activate_this.py @@ -0,0 +1,26 @@ +import numpy as np +import scipy.io.wavfile +import matplotlib.pyplot as plt + +sampling_rate, data = scipy.io.wavfile.read('voice1.wav') +print(f"sampling rate: {sampling_rate}hz") # frequency (sample per second) +print('data type:', data.dtype) +print('data shape:', data.shape) +# TODO: Calculate length of the signal in seconds +# print(f"length = {length}s") +N, no_channels = data.shape # signal length and no. of channels +print('signal length:', N) +channel0 = data[:,0] +channel1 = data[:,1] +scale = np.linspace(-2,4,N) +plt.plot(np.arange(N), scale) +plt.show() +print('shape_old:', scale.shape) +scale.shape = (N,1) +print('shape_new:', scale.shape) +data_new = data[::-1] +data_new1 = np.int16(scale * data) +scipy.io.wavfile.write('reverse.wav', sampling_rate , data_new) +scipy.io.wavfile.write('scale.wav', sampling_rate , data_new1) +scipy.io.wavfile.write('fast.wav', sampling_rate * 2 , data_new1) +scipy.io.wavfile.write('slow.wav', sampling_rate // 2 , data_new1) \ No newline at end of file diff --git a/3/inverted_pendulum.py b/3/inverted_pendulum.py new file mode 100644 index 0000000..cb77226 --- /dev/null +++ b/3/inverted_pendulum.py @@ -0,0 +1,53 @@ +import numpy as np +from scipy.integrate import solve_ivp +import matplotlib.pyplot as plt + + +def inverted_pendulum(t, x): + # State variable x = [x, theta, v, omega] + g = 9.8 + l = 1 + m = 1 + M = 1 + d1 = M + m * (1 - np.cos(x[1]) ** 2) + d2 = l * d1 + + F = 0 # No input + + xp = np.zeros(4) + xp[0] = x[2] + xp[1] = x[3] + xp[2] = (F + m * l * x[3] ** 2 * np.sin(x[1]) - m * g * np.sin(x[1]) * np.cos(x[1])) / d1 + xp[3] = (-F * np.cos(x[1]) - m * l * x[3] ** 2 * np.sin(x[1]) * np.cos(x[1]) + (M + m) * g * np.sin(x[1])) / d2 + + return xp + + +# Initial conditions: x, theta, v, omega +x0 = [0, np.pi / 4, 0, 0] + +# Time span for the simulation +t_span = (0, 10) +t_eval = np.linspace(*t_span, 300) + +# Solve the differential equation +sol = solve_ivp(inverted_pendulum, t_span, x0, t_eval=t_eval) + +# Plotting the results +plt.figure(figsize=(10, 8)) +plt.subplot(2, 1, 1) +plt.plot(sol.t, sol.y[0], label='x (position)') +plt.plot(sol.t, sol.y[1], label='theta (angle)') +plt.legend() +plt.xlabel('Time [s]') +plt.ylabel('Position/Angle') + +plt.subplot(2, 1, 2) +plt.plot(sol.t, sol.y[2], label='v (velocity)') +plt.plot(sol.t, sol.y[3], label='omega (angular velocity)') +plt.legend() +plt.xlabel('Time [s]') +plt.ylabel('Velocity/Angular Velocity') + +plt.tight_layout() +plt.show() diff --git a/3/invpend_solver.py b/3/invpend_solver.py new file mode 100644 index 0000000..5c391fe --- /dev/null +++ b/3/invpend_solver.py @@ -0,0 +1,45 @@ +import numpy as np +from scipy.integrate import solve_ivp +import matplotlib.pyplot as plt + + +def inverted_pendulum(t, x): + # State variable x = [x, theta, v, omega] + g = 9.8 + l = 1 + m = 1 + M = 1 + d1 = M + m * (1 - np.cos(x[1]) ** 2) + d2 = l * d1 + + F = 0 # No input + + xp = np.zeros(4) + xp[0] = x[2] + xp[1] = x[3] + xp[2] = (F + m * l * x[3] ** 2 * np.sin(x[1]) - m * g * np.sin(x[1]) * np.cos(x[1])) / d1 + xp[3] = (-F * np.cos(x[1]) - m * l * x[3] ** 2 * np.sin(x[1]) * np.cos(x[1]) + (M + m) * g * np.sin(x[1])) / d2 + + return xp + + +# Initial conditions: x, theta, v, omega +x0 = [0, 0.1, 0, 0] + +# Time span for the simulation +t_span = (0, 1) +t_eval = np.linspace(*t_span, 300) # Generating 300 time points within the span + +# Solve the differential equation +sol = solve_ivp(inverted_pendulum, t_span, x0, t_eval=t_eval, max_step=1e-2) + +# Plotting the results +plt.figure(figsize=(10, 6)) +plt.plot(sol.t, sol.y[0], 'k', label='x (m)') +plt.plot(sol.t, sol.y[1], '-.k', label='theta (rad)') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State Variables') +plt.legend() +plt.title('Simulation of Inverted Pendulum') +plt.show() diff --git a/3/robot_model.py b/3/robot_model.py new file mode 100644 index 0000000..f8635bc --- /dev/null +++ b/3/robot_model.py @@ -0,0 +1,60 @@ +import numpy as np + +def robot_model(t, x): + # State variable x = [theta_1, theta_2, omega_1, omega_2] + g = 9.81 + l1 = 1 + l2 = 0.5 + m1 = 2 + m2 = 1 + I1 = 1e-2 + I2 = 5e-3 + D = 2 + + M = np.zeros((2, 2)) + M[0, 0] = m1 * (l1 / 2)**2 + m2 * (l1**2 + (l2 / 2)**2) + m2 * l1 * l2 * np.cos(x[1]) + I1 + I2 + M[0, 1] = m2 * (l2 / 2)**2 + 0.5 * m2 * l1 * l2 * np.cos(x[1]) + I2 + M[1, 0] = M[0, 1] + M[1, 1] = m2 * (l2 / 2)**2 + I2 + + V = np.zeros((2, 1)) + V[0, 0] = -m2 * l1 * l2 * np.sin(x[1]) * x[2] * x[3] - 0.5 * m2 * l1 * l2 * np.sin(x[1]) * x[3]**2 + V[1, 0] = -0.5 * m2 * l1 * l2 * np.sin(x[1]) * x[2] * x[3] + + G = np.zeros((2, 1)) + G[0, 0] = (m1 * l1 / 2 + m2 * l1) * g * np.cos(x[0]) + m2 * g * l2 / 2 * np.cos(x[0] + x[1]) + G[1, 0] = m2 * g * l2 / 2 * np.cos(x[0] + x[1]) + + Q = np.zeros((2, 1)) # No input + Q = Q - D * np.array([[x[2]], [x[3]]]) + + # Solve for the accelerations + xy = np.linalg.pinv(M).dot(Q - V - G) + + xp = np.array([x[2], x[3], xy[0, 0], xy[1, 0]]) + return xp + +# Example usage: integrate over time using solve_ivp +from scipy.integrate import solve_ivp +import matplotlib.pyplot as plt + +# Initial conditions: theta1, theta2, omega1, omega2 +x0 = [0, 0.1, 0, 0] + +# Time span for the simulation +t_span = (0, 10) +t_eval = np.linspace(*t_span, 300) + +# Solve the differential equation +sol = solve_ivp(robot_model, t_span, x0, t_eval=t_eval) + +# Plotting the results +plt.figure(figsize=(10, 6)) +plt.plot(sol.t, sol.y[0], label='theta_1 (rad)') +plt.plot(sol.t, sol.y[1], label='theta_2 (rad)') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State Variables') +plt.legend() +plt.title('Simulation of 2R Robot') +plt.show() diff --git a/3/robot_solver.py b/3/robot_solver.py new file mode 100644 index 0000000..33114fb --- /dev/null +++ b/3/robot_solver.py @@ -0,0 +1,58 @@ +import numpy as np +from scipy.integrate import solve_ivp +import matplotlib.pyplot as plt + +def robot_model(t, x): + # State variable x = [theta_1, theta_2, omega_1, omega_2] + g = 9.81 + l1 = 1 + l2 = 0.5 + m1 = 2 + m2 = 1 + I1 = 1e-2 + I2 = 5e-3 + D = 2 + + M = np.zeros((2, 2)) + M[0, 0] = m1 * (l1 / 2)**2 + m2 * (l1**2 + (l2 / 2)**2) + m2 * l1 * l2 * np.cos(x[1]) + I1 + I2 + M[0, 1] = m2 * (l2 / 2)**2 + 0.5 * m2 * l1 * l2 * np.cos(x[1]) + I2 + M[1, 0] = M[0, 1] + M[1, 1] = m2 * (l2 / 2)**2 + I2 + + V = np.zeros((2, 1)) + V[0, 0] = -m2 * l1 * l2 * np.sin(x[1]) * x[2] * x[3] - 0.5 * m2 * l1 * l2 * np.sin(x[1]) * x[3]**2 + V[1, 0] = -0.5 * m2 * l1 * l2 * np.sin(x[1]) * x[2] * x[3] + + G = np.zeros((2, 1)) + G[0, 0] = (m1 * l1 / 2 + m2 * l1) * g * np.cos(x[0]) + m2 * g * l2 / 2 * np.cos(x[0] + x[1]) + G[1, 0] = m2 * g * l2 / 2 * np.cos(x[0] + x[1]) + + Q = np.zeros((2, 1)) # No input + Q = Q - D * np.array([[x[2]], [x[3]]]) + + # Solve for the accelerations + xy = np.linalg.pinv(M).dot(Q - V - G) + + xp = np.array([x[2], x[3], xy[0, 0], xy[1, 0]]) + return xp + +# Initial conditions: theta1, theta2, omega1, omega2 +x0 = [-np.pi / 3, np.pi / 3, 0, 0] + +# Time span for the simulation +t_span = (0, 5) +t_eval = np.linspace(*t_span, 300) # Generate 300 time points within the span + +# Solve the differential equation +sol = solve_ivp(robot_model, t_span, x0, t_eval=t_eval) + +# Plotting the results +plt.figure(figsize=(10, 6)) +plt.plot(sol.t, np.degrees(sol.y[0]), 'k', label='theta_1 (degrees)') +plt.plot(sol.t, np.degrees(sol.y[1]), '-.k', label='theta_2 (degrees)') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State Variables') +plt.legend() +plt.title('Simulation of 2R Robot') +plt.show() diff --git a/3/tank_model.py b/3/tank_model.py new file mode 100644 index 0000000..3b9fbeb --- /dev/null +++ b/3/tank_model.py @@ -0,0 +1,32 @@ +import numpy as np +from scipy.integrate import solve_ivp +import matplotlib.pyplot as plt + +def tank_model(t, x): + # State variable x = [l]: Tank level + A = 1.0 + C = 2.0 + F_in = 0 # No disturbance input + u = 0.1 # Constant opening for valve + + xp = 1 / A * (F_in - C * u * np.sqrt(x[0])) + return [xp] + +# Initial condition: tank level +x0 = [1.0] # Initial level of the tank + +# Time span for the simulation +t_span = (0, 10) +t_eval = np.linspace(*t_span, 300) # Generate 300 time points within the span + +# Solve the differential equation +sol = solve_ivp(tank_model, t_span, x0, t_eval=t_eval) + +# Plotting the results +plt.figure(figsize=(10, 6)) +plt.plot(sol.t, sol.y[0], 'k') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Tank Level (l)') +plt.title('Simulation of Tank Level') +plt.show() diff --git a/3/tank_solver.py b/3/tank_solver.py new file mode 100644 index 0000000..b323acc --- /dev/null +++ b/3/tank_solver.py @@ -0,0 +1,32 @@ +import numpy as np +from scipy.integrate import solve_ivp +import matplotlib.pyplot as plt + +def tank_model(t, x): + # State variable x = [l]: Tank level + A = 1.0 + C = 2.0 + F_in = 0 # No disturbance input + u = 0.1 # Constant opening for valve + + xp = 1 / A * (F_in - C * u * np.sqrt(x[0])) + return [xp] + +# Initial condition: tank level +x0 = [100.0] # Initial level of the tank + +# Time span for the simulation +t_span = (0, 100) +t_eval = np.linspace(*t_span, 1000) # Generate 1000 time points within the span + +# Solve the differential equation +sol = solve_ivp(tank_model, t_span, x0, t_eval=t_eval, max_step=0.1) + +# Plotting the results +plt.figure(figsize=(10, 6)) +plt.plot(sol.t, sol.y[0], 'k') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Tank Level (m)') +plt.title('Simulation of Tank Level') +plt.show() diff --git a/3/train.py b/3/train.py new file mode 100644 index 0000000..e114ed5 --- /dev/null +++ b/3/train.py @@ -0,0 +1,61 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import solve_ivp +from scipy.signal import lsim, StateSpace + +# Define the state-space model +A = np.array([ + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1, -1, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 1, -1, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 1, -1, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 1, -1], + [0, -12.5, 0, 0, 0, -0.75, 0.75, 0, 0, 0], + [0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0, 0], + [0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0], + [0, 0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75], + [0, 0, 0, 0, 62.5, 0, 0, 0, 3.75, -3.75] +]) +b1 = np.array([0, 0, 0, 0, 0.005, 0, 0, 0, 0, 0]).reshape(-1, 1) # Force input +b2 = np.array([0, 0, 0, 0, 250, 0, 0, 0, 0, -1250]).reshape(-1, 1) # Constant input +B = np.hstack((b1, b2)) +C = np.array([1, 0, 0, 0, 0, 0, 0, 0, 0, 0]) +D = 0 + +train_model = StateSpace(A, b1, C, D) # Note only the first input is used +t = np.arange(0, 7.01, 0.01) # Time vector + +# Initial conditions +x0 = np.array([20, 20, 20, 20, 20, 0, 0, 0, 0, 0]) + +# Simulate the initial response +_, y, x = lsim(train_model, U=0, T=t, X0=x0) + +# Plot the initial response +plt.figure() +plt.plot(t, x[:, 0], 'k', label='x1') +plt.plot(t, x[:, 4], 'k-.', label='x5') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.legend() +plt.title('Initial Response') +plt.show() + +# Generate input u +u = 0.1 * (np.sin(5*t) + np.sin(9*t) + np.sin(13*t) + np.sin(17*t) + np.sin(21*t)) + +# Simulate the system with the generated input +active_suspension = StateSpace(A, b1, C, D) # Using the same state-space model for lsim +_, y, x = lsim(active_suspension, U=u, T=t) + +# Plot the result +plt.figure() +plt.plot(t, x[:, 0], 'k', label='x1') +plt.plot(t, x[:, 1], 'k-.', label='x2') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.legend() +plt.title('Response to Input u') +plt.show() diff --git a/3/train_linear.py b/3/train_linear.py new file mode 100644 index 0000000..8d65ae4 --- /dev/null +++ b/3/train_linear.py @@ -0,0 +1,66 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.signal import StateSpace, lsim + +# Define the state-space model +A = np.array([ + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1, -1, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 1, -1, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 1, -1, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 1, -1], + [0, -12.5, 0, 0, 0, -0.75, 0.75, 0, 0, 0], + [0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0, 0], + [0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0], + [0, 0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75], + [0, 0, 0, 0, 62.5, 0, 0, 0, 3.75, -3.75] +]) +b1 = np.array([0, 0, 0, 0, 0, 0.005, 0, 0, 0, 0]).reshape(-1, 1) # Force input +b2 = np.array([0, 0, 0, 0, 0, 250, 0, 0, 0, -1250]).reshape(-1, 1) # Constant input +C = np.array([1, 0, 0, 0, 0, 0, 0, 0, 0, 0]) +D = np.array([0]) + +# Constant input +u = 750 +b = b1 * u + b2 + +# Define the state-space system +train_model = StateSpace(A, b, C, D) + +# Time vector +t = np.arange(0, 7.001, 0.001) + +# Initial conditions +x0 = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) + +# Simulate the forced response +_, y, x = lsim(train_model, T=t, U=np.zeros_like(t), X0=x0) + +# Plot the forced response +plt.figure() +plt.plot(t, x[:, 1], 'k', label='x2') +plt.plot(t, x[:, 4], 'k-.', label='x5') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.legend() +plt.title('Forced Response') +plt.show() + +# Generate input u +u = 0.1 * (np.sin(5*t) + np.sin(9*t) + np.sin(13*t) + np.sin(17*t) + np.sin(21*t)) + +# Simulate the system with the generated input +active_suspension = StateSpace(A, b1, C, D) # Using the same state-space model for lsim +_, y, x = lsim(active_suspension, U=u, T=t, X0=x0) + +# Plot the result +plt.figure() +plt.plot(t, x[:, 0], 'k', label='x1') +plt.plot(t, x[:, 1], 'k-.', label='x2') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.legend() +plt.title('Response to Input u') +plt.show() diff --git a/4/DCmotor_jordan.py b/4/DCmotor_jordan.py new file mode 100644 index 0000000..f4e40aa --- /dev/null +++ b/4/DCmotor_jordan.py @@ -0,0 +1,34 @@ +import numpy as np +from scipy.signal import StateSpace +from scipy.linalg import eig + +# Define the state-space model matrices +A = np.array([ + [0, 1, 0], + [0, 0, 4.438], + [0, -12, -24] +]) +b1 = np.array([0, 0, 20]).reshape(-1, 1) # Reshape to column vector +b2 = np.array([0, -7.396, 0]).reshape(-1, 1) # Reshape to column vector +B = np.hstack((b1, b2)) # Combine b1 and b2 into B +C = np.array([ + [1, 0, 0], + [0, 1, 0] +]) +D = np.array([[0, 0], [0, 0]]) # D matrix should be 2x2 zero matrix + +# Create the state-space model +DC_motor = StateSpace(A, B, C, D) + +# Compute the eigenvalues and eigenvectors +eigenvalues, v = eig(A) +eigenvalues_transposed, w = eig(A.T) + +# Print the results +print("Eigenvalues and right eigenvectors:") +print(eigenvalues) +print(v) + +print("Eigenvalues and left eigenvectors (transposed):") +print(eigenvalues_transposed) +print(w) diff --git a/4/ex3_1.py b/4/ex3_1.py new file mode 100644 index 0000000..735d5f4 --- /dev/null +++ b/4/ex3_1.py @@ -0,0 +1,25 @@ +import numpy as np +from scipy.linalg import obsv, null_space, matrix_rank + +# Define the matrices +A = np.array([[-3/2, 1/2], + [1/2, -3/2]]) +C = np.array([[1, -1]]) + +# Compute observability matrix +O = obsv(A, C) + +# Compute rank of observability matrix +rank_O = matrix_rank(O) + +# Compute null space of observability matrix +null_O = null_space(O) + +# Display results +print("Observability matrix O:") +print(O) + +print("\nRank of O:", rank_O) + +print("\nNull space of O:") +print(null_O) diff --git a/4/ex3_2.py b/4/ex3_2.py new file mode 100644 index 0000000..a9b40a8 --- /dev/null +++ b/4/ex3_2.py @@ -0,0 +1,40 @@ +import numpy as np +from sympy import symbols, exp, integrate + +# Define the matrices and variables +A = np.array([[1, 0], + [1, 1]]) +B = np.array([[1], + [1]]) +u = 1 +x0 = np.array([[1], + [1]]) +t = symbols('t') + +# Compute phi = expm(A*t) +phi = np.array(exp(A * t), dtype='float') + +# Compute x1 = expm(-A*t) * B * u +x1 = np.dot(np.dot(exp(-A * t), B), u) + +# Compute x_zs = int(x1) +x_zs = integrate(x1, t) + +# Compute x_zi = phi * x0 +x_zi = np.dot(phi, x0) + +# Compute x = x_zi + x_zs +x = x_zi + x_zs + +# Print the results +print("phi =") +print(phi) + +print("\nx_zs =") +print(x_zs) + +print("\nx_zi =") +print(x_zi) + +print("\nx =") +print(x) diff --git a/4/ex3_3.py b/4/ex3_3.py new file mode 100644 index 0000000..c915ad3 --- /dev/null +++ b/4/ex3_3.py @@ -0,0 +1,14 @@ +from sympy import symbols, exp +import numpy as np + +# Define the matrix A and symbolic variable t +A = np.array([[0, 6], + [-1, -5]]) +t = symbols('t') + +# Compute the matrix exponential expm(A*t) +exp_A_t = exp(t * A) + +# Print the result +print("Matrix exponential expm(A*t):") +print(exp_A_t) diff --git a/4/ex3_8.py b/4/ex3_8.py new file mode 100644 index 0000000..26db712 --- /dev/null +++ b/4/ex3_8.py @@ -0,0 +1,32 @@ +import numpy as np +from scipy.signal import ss2tf, tf2ss, place_poles, tf2ss + +# Define the state-space system matrices +A = np.array([[0, 1], + [-2, -3]]) +B = np.array([[1], + [1]]) +C = np.array([[1, 0]]) +D = np.array([[0]]) + +# Create the state-space system +sys = ss(A, B, C, D) + +# Compute eigenvalues of matrix A +eigs = np.linalg.eigvals(A) + +# Compute poles of the system +poles = sys.pole() + +# Compute transmission zeros of the system +zeros = sys.zero() + +# Print results +print("Eigenvalues of A:") +print(eigs) + +print("\nPoles of the system:") +print(poles) + +print("\nTransmission zeros of the system:") +print(zeros) diff --git a/4/jordan_forms.py b/4/jordan_forms.py new file mode 100644 index 0000000..7ca7ad9 --- /dev/null +++ b/4/jordan_forms.py @@ -0,0 +1,46 @@ +import numpy as np + +# Example 1: Inverted Pendulum +A1 = np.array([[0, 1, 0, 0], + [0, 0, -9.8, 0], + [0, 0, 0, 1], + [0, 0, 19.6, 0]]) +B1 = np.array([[0], + [1], + [0], + [1]]) +C1 = np.array([[1, 0, 0, 0], + [0, 0, 1, 0]]) + +# Compute the Jordan form and transformation matrices for Example 1 +T1, J1 = np.linalg.eig(A1) + +# Transform B and C using the Jordan transformation matrix T1 +T1_inv = np.linalg.inv(T1) +B1n = np.dot(T1_inv, B1) +C1n = np.dot(C1, T1) + +# Print results for Example 1 +print("Example 1: Inverted Pendulum") +print("Jordan form T:") +print(T1) +print("\nTransformed Bn:") +print(B1n) +print("\nTransformed Cn:") +print(C1n) + +print("\n----------------------------------------\n") + +# Example 2: Example 3-13 +A2 = np.array([[0, 1, 0, 3], + [0, -1, 1, 10], + [0, 0, 0, 1], + [0, 0, -1, -2]]) + +# Compute the Jordan form and transformation matrices for Example 2 +T2, J2 = np.linalg.eig(A2) + +# Print results for Example 2 +print("Example 2: Example 3-13") +print("Jordan form T:") +print(T2) diff --git a/5_6/DCmotor.py b/5_6/DCmotor.py new file mode 100644 index 0000000..bdf5b0a --- /dev/null +++ b/5_6/DCmotor.py @@ -0,0 +1,53 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.signal import lsim, square + +# Simulation of the DC motor +# Copyright Hamid D. Taghirad 2013 + +# Define system matrices +A = np.array([[0, 1, 0], + [0, 0, 4.438], + [0, -12, -24]]) + +b1 = np.array([[0], [0], [20]]) +b2 = np.array([[0], [-7.396], [0]]) +B = np.hstack((b1, b2)) + +C = np.array([[1, 0, 0], + [0, 1, 0]]) + +D = np.array([[0]]) + +# Create state space system +DC_motor = (A, B, C, D) + +# Define time vector +t = np.arange(0, 4.01, 0.01) +N = len(t) + +# A Simple way to generate input u +u = np.zeros((2, N)) +for i in range(N): + if t[i] < 2: + u[0, i] = 3 + else: + u[0, i] = -3 + +# A Professional way to generate input u +u = 6 * square(2 * np.pi * 0.25 * t) - 3 # Generate square wave with period 4 and amplitude 6 + +# Simulate the system +t, y, x = lsim(DC_motor, u, t) + +# Plot the result +plt.figure() +plt.plot(t, x[:, 0], 'k', label='theta') +plt.plot(t, x[:, 1], 'k-.', label='omega') +plt.plot(t, x[:, 2], 'k:', label='i') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.legend(['theta', 'omega', 'i']) +plt.title('Simulation Result - DC Motor') +plt.show() diff --git a/5_6/ex4_3.py b/5_6/ex4_3.py new file mode 100644 index 0000000..1d1b6e1 --- /dev/null +++ b/5_6/ex4_3.py @@ -0,0 +1,17 @@ +import numpy as np + +# Define the system matrices +A = np.array([[-3/2, 1/2], + [1/2, -3/2]]) +C = np.array([[1, -1]]) + +# Compute observability matrix and related operations +O = np.vstack((C, np.dot(C, A))) +rank_O = np.linalg.matrix_rank(O) +null_O = np.linalg.matrix_rank(O) == 0 + +# Print results +print("Observability matrix O:") +print(O) +print("\nRank of observability matrix O:", rank_O) +print("\nIs observability matrix O null:", null_O) diff --git a/5_6/ex4_9.py b/5_6/ex4_9.py new file mode 100644 index 0000000..fa6b5a6 --- /dev/null +++ b/5_6/ex4_9.py @@ -0,0 +1,18 @@ +import numpy as np + +# Define the system matrices +A = np.array([[-3/2, 1/2], + [1/2, -3/2]]) +B = np.array([[1/2], + [1/2]]) + +# Compute controllability matrix and related operations +C = np.hstack((B, np.dot(A, B))) +rank_C = np.linalg.matrix_rank(C) +null_C = np.linalg.matrix_rank(C) == 0 + +# Print results +print("Controllability matrix C:") +print(C) +print("\nRank of controllability matrix C:", rank_C) +print("\nIs controllability matrix C null:", null_C) diff --git a/5_6/ex5_1.py b/5_6/ex5_1.py new file mode 100644 index 0000000..6b6d692 --- /dev/null +++ b/5_6/ex5_1.py @@ -0,0 +1,137 @@ +import numpy as np +import scipy.signal as signal + +# Define system matrices for sys1 +A1 = np.array([[0, 1, 0], + [0, 0, 1], + [-5, -11, -6]]) +B1 = np.array([[0], + [0], + [1]]) +C1 = np.array([[1, 0, 1]]) +D1 = np.array([[0]]) + +# Create state-space system sys1 +sys1 = signal.StateSpace(A1, B1, C1, D1) + +# Convert sys1 to transfer function +tf1 = signal.ss2tf(sys1.A, sys1.B, sys1.C, sys1.D) + +# Display results for sys1 +print("System 1 (sys1):") +print("State-space representation (A, B, C, D):") +print(sys1.A) +print(sys1.B) +print(sys1.C) +print(sys1.D) +print("\nTransfer function:") +print(tf1) + +# Define system matrices for sys2 +a2 = np.array([[0, 0, -5], + [1, 0, -11], + [0, 1, -6]]) +b2 = np.array([[1], + [0], + [1]]) +c2 = np.array([[0, 0, 1]]) +d2 = np.array([[0]]) + +# Create state-space system sys2 +sys2 = signal.StateSpace(a2, b2, c2, d2) + +# Convert sys2 to transfer function +tf2 = signal.ss2tf(sys2.A, sys2.B, sys2.C, sys2.D) + +# Display results for sys2 +print("\nSystem 2 (sys2):") +print("State-space representation (a, b, c, d):") +print(sys2.A) +print(sys2.B) +print(sys2.C) +print(sys2.D) +print("\nTransfer function:") +print(tf2) + +# Define system matrices for sys3 +A3 = np.array([[0, 1, 0], + [0, 0, 1], + [-5, -11, -6]]) +B3 = np.array([[1], + [-6], + [26]]) +C3 = np.array([[1, 0, 0]]) +D3 = np.array([[0]]) + +# Create state-space system sys3 +sys3 = signal.StateSpace(A3, B3, C3, D3) + +# Convert sys3 to transfer function +tf3 = signal.ss2tf(sys3.A, sys3.B, sys3.C, sys3.D) + +# Display results for sys3 +print("\nSystem 3 (sys3):") +print("State-space representation (A, B, C, D):") +print(sys3.A) +print(sys3.B) +print(sys3.C) +print(sys3.D) +print("\nTransfer function:") +print(tf3) + +# Compute observability for sys3 +O3 = signal.obsv(sys3) +print("\nObservability matrix (O3):") +print(O3) + +# Define system matrices for sys4 +a4 = np.array([[0, 0, -5], + [1, 0, -11], + [0, 1, -6]]) +b4 = np.array([[1], + [0], + [0]]) +c4 = np.array([[1, -6, 26]]) +d4 = np.array([[0]]) + +# Create state-space system sys4 +sys4 = signal.StateSpace(a4, b4, c4, d4) + +# Convert sys4 to transfer function +tf4 = signal.ss2tf(sys4.A, sys4.B, sys4.C, sys4.D) + +# Display results for sys4 +print("\nSystem 4 (sys4):") +print("State-space representation (a, b, c, d):") +print(sys4.A) +print(sys4.B) +print(sys4.C) +print(sys4.D) +print("\nTransfer function:") +print(tf4) + +# Compute controllability for sys4 +C4 = signal.ctrb(sys4) +print("\nControllability matrix (C4):") +print(C4) + +# Transfer function representation +num = [1, 0, 1] +den = [1, 6, 11, 5] + +# Convert transfer function to state-space +A_tf, B_tf, C_tf, D_tf = signal.tf2ss(num, den) +sys_tf = signal.StateSpace(A_tf, B_tf, C_tf, D_tf) + +# Convert state-space back to transfer function +tf_tf = signal.ss2tf(sys_tf.A, sys_tf.B, sys_tf.C, sys_tf.D) + +# Display results for transfer function conversion +print("\nTransfer function representation:") +print("State-space representation (A_tf, B_tf, C_tf, D_tf):") +print(A_tf) +print(B_tf) +print(C_tf) +print(D_tf) +print("\nTransfer function:") +print(tf_tf) diff --git a/5_6/ex5_2.py b/5_6/ex5_2.py new file mode 100644 index 0000000..ae5196b --- /dev/null +++ b/5_6/ex5_2.py @@ -0,0 +1,141 @@ +import numpy as np +import scipy.signal as signal + +# Example 5-1 +# Define system matrices for sys1 +A1 = np.array([[0, 1, 0], + [0, 0, 1], + [-5, -11, -6]]) +B1 = np.array([[0], + [0], + [1]]) +C1 = np.array([[1, 0, 1]]) +D1 = np.array([[0]]) + +# Create state-space system sys1 +sys1 = signal.StateSpace(A1, B1, C1, D1) + +# Convert sys1 to transfer function +tf1 = signal.ss2tf(sys1.A, sys1.B, sys1.C, sys1.D) + +# Display results for sys1 +print("System 1 (sys1):") +print("State-space representation (A, B, C, D):") +print(sys1.A) +print(sys1.B) +print(sys1.C) +print(sys1.D) +print("\nTransfer function:") +print(tf1) + +# Example 5-1 (Alternative representation) +# Define system matrices for sys2 +a2 = np.array([[0, 0, -5], + [1, 0, -11], + [0, 1, -6]]) +b2 = np.array([[1], + [0], + [1]]) +c2 = np.array([[0, 0, 1]]) +d2 = np.array([[0]]) + +# Create state-space system sys2 +sys2 = signal.StateSpace(a2, b2, c2, d2) + +# Convert sys2 to transfer function +tf2 = signal.ss2tf(sys2.A, sys2.B, sys2.C, sys2.D) + +# Display results for sys2 +print("\nSystem 2 (sys2):") +print("State-space representation (a, b, c, d):") +print(sys2.A) +print(sys2.B) +print(sys2.C) +print(sys2.D) +print("\nTransfer function:") +print(tf2) + +# Example 5-1 (Alternative representation) +# Define system matrices for sys3 +A3 = np.array([[0, 1, 0], + [0, 0, 1], + [-5, -11, -6]]) +B3 = np.array([[1], + [-6], + [26]]) +C3 = np.array([[1, 0, 0]]) +D3 = np.array([[0]]) + +# Create state-space system sys3 +sys3 = signal.StateSpace(A3, B3, C3, D3) + +# Convert sys3 to transfer function +tf3 = signal.ss2tf(sys3.A, sys3.B, sys3.C, sys3.D) + +# Display results for sys3 +print("\nSystem 3 (sys3):") +print("State-space representation (A, B, C, D):") +print(sys3.A) +print(sys3.B) +print(sys3.C) +print(sys3.D) +print("\nTransfer function:") +print(tf3) + +# Compute observability for sys3 +O3 = signal.obsv(sys3) +print("\nObservability matrix (O3):") +print(O3) + +# Example 5-1 (Alternative representation) +# Define system matrices for sys4 +a4 = np.array([[0, 0, -5], + [1, 0, -11], + [0, 1, -6]]) +b4 = np.array([[1], + [0], + [0]]) +c4 = np.array([[1, -6, 26]]) +d4 = np.array([[0]]) + +# Create state-space system sys4 +sys4 = signal.StateSpace(a4, b4, c4, d4) + +# Convert sys4 to transfer function +tf4 = signal.ss2tf(sys4.A, sys4.B, sys4.C, sys4.D) + +# Display results for sys4 +print("\nSystem 4 (sys4):") +print("State-space representation (a, b, c, d):") +print(sys4.A) +print(sys4.B) +print(sys4.C) +print(sys4.D) +print("\nTransfer function:") +print(tf4) + +# Compute controllability for sys4 +C4 = signal.ctrb(sys4) +print("\nControllability matrix (C4):") +print(C4) + +# Transfer function representation +num = [1, 0, 1] +den = [1, 6, 11, 5] + +# Convert transfer function to state-space +A_tf, B_tf, C_tf, D_tf = signal.tf2ss(num, den) +sys_tf = signal.StateSpace(A_tf, B_tf, C_tf, D_tf) + +# Convert state-space back to transfer function +tf_tf = signal.ss2tf(sys_tf.A, sys_tf.B, sys_tf.C, sys_tf.D) + +# Display results for transfer function conversion +print("\nTransfer function representation:") +print("State-space representation (A_tf, B_tf, C_tf, D_tf):") +print(A_tf) +print(B_tf) +print(C_tf) +print(D_tf) +print("\nTransfer function:") +print(tf_tf) diff --git a/5_6/ex5_3.py b/5_6/ex5_3.py new file mode 100644 index 0000000..f3b5e68 --- /dev/null +++ b/5_6/ex5_3.py @@ -0,0 +1,77 @@ +import numpy as np +import scipy.signal as signal + +# Define numerator and denominator arrays for transfer function +num1 = [1] +num2 = [1, 0] +den1 = np.convolve([1, 0, 1], [1, 1]) +den2 = [1, 3, 2] + +# Create transfer function sys +sys = signal.TransferFunction(num1, den1) +sys.num = num2 # Assign the second numerator + +# Display sys as a transfer function +print("Transfer function (sys):") +print(sys) + +# Convert sys to zero-pole-gain form (ZPK) +sys1 = signal.TransferFunction(num1, den1).to_zpk() +print("\nZero-pole-gain form (sys1):") +print(sys1) + +# Convert sys to state-space form (SS) +sys2 = signal.TransferFunction(num1, den1).to_ss() +sys2_minreal = signal.minreal(sys2) # Compute minimal realization of SS form + +print("\nState-space form (sys2):") +print(sys2) +print("\nMinimal realization (sys2_minreal):") +print(sys2_minreal) + +# Define symbolic variable 's' +s = signal.TransferFunction([1, 0], [1]) + +# Define and simplify symbolic system mysys +mysys = [1/((s**2 + 1)*(s + 1)), s/((s + 1)*(s + 2))] +myss = signal.minreal(signal.TransferFunction(mysys)) + +print("\nSymbolic system (mysys):") +print(mysys) +print("\nMinimal realization (myss):") +print(myss) + +# Define state-space matrices for sys +A = np.array([[0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1], + [-2, -3, -3, -3]]) +B = np.array([[0], + [0], + [0], + [1]]) +C = np.array([[2, 1, 0, 0], + [0, 1, 0, 1]]) +D = np.array([[0], + [0]]) + +# Create state-space system sys +sys = signal.StateSpace(A, B, C, D) + +# Convert sys to transfer function +tf_sys = signal.ss2tf(sys.A, sys.B, sys.C, sys.D) + +# Display results for sys +print("\nState-space representation (sys):") +print("A:\n", sys.A) +print("B:\n", sys.B) +print("C:\n", sys.C) +print("D:\n", sys.D) + +print("\nTransfer function (tf_sys):") +print(tf_sys) + +# Convert sys to zero-pole-gain form (ZPK) +zpk_sys = signal.ss2zpk(sys.A, sys.B, sys.C, sys.D) +print("\nZero-pole-gain form (zpk_sys):") +print(zpk_sys) diff --git a/5_6/ex5_4.py b/5_6/ex5_4.py new file mode 100644 index 0000000..2d098c9 --- /dev/null +++ b/5_6/ex5_4.py @@ -0,0 +1,73 @@ +import numpy as np +import scipy.signal as signal + +# Define numerator and denominator arrays for transfer functions +num = [[1, 2], [-1, 1]] +den = [[1, 1], [1, 2], [1, 1], [1, 3]] + +# Create transfer function sys +sys = signal.TransferFunction(num, den) + +# Display sys as a transfer function +print("Transfer function (sys):") +print(sys) + +# Convert sys to state-space form and compute minimal realization +sys2 = signal.minreal(signal.ss(sys)) + +print("\nMinimal realization (sys2):") +print(sys2) + +# Define symbolic variable 's' +s = signal.TransferFunction([1, 0], [1]) + +# Define and simplify symbolic system mysys +mysys = [[1/(s + 1), 2/(s + 2)], + [-1/(s + 1), 1/(s + 3)]] +myss = signal.minreal(signal.ss(mysys)) + +print("\nSymbolic system (mysys):") +print(mysys) +print("\nMinimal realization (myss):") +print(myss) + +# Clear command window (not necessary in Python) + +# Define symbolic variable 's' +s = signal.TransferFunction([1, 0], [1]) + +# Define transfer functions G11, G12, G21, G22 +G11 = 1 / (s + 1) +G12 = 2 / (s + 2) +G21 = -1 / (s + 1) +G22 = 1 / (s + 3) + +# Define interconnection strings +systemnames = 'G11 G12 G21 G22' +inputvar = '[u{2}]' +input_to_G11 = '[u(1)]' +input_to_G12 = '[u(2)]' +input_to_G21 = '[u(1)]' +input_to_G22 = '[u(2)]' +outputvar = '[G11+G12; G21+G22]' + +# Interconnect systems +sysic = signal.InterconnectedSystem([G11, G12, G21, G22], + connections=[input_to_G11, input_to_G12, + input_to_G21, input_to_G22], + inplist=inputvar, + outlist=outputvar) + +# Display interconnected system +print("\nInterconnected system (sysic):") +print(sysic) + +# Set input and output names +sysic.InputName = ['u1', 'u2'] +sysic.OutputName = ['y1', 'y2'] + +# Convert interconnected system to state-space and compute minimal realization +G = signal.minreal(signal.ss(sysic)) + +print("\nMinimal realization of interconnected system (G):") +print(G) diff --git a/5_6/ex5_6.py b/5_6/ex5_6.py new file mode 100644 index 0000000..9602fb8 --- /dev/null +++ b/5_6/ex5_6.py @@ -0,0 +1,19 @@ +import numpy as np + +# Define the matrix A +A = np.array([[-1, -2], + [1, -4]]) + +# Define the identity matrix Q (since Q = eye(2) in MATLAB) +Q = np.eye(2) + +# Solve the Lyapunov equation P = A*P*A' + Q using numpy's continuous Lyapunov solver +P = np.linalg.solve(-A.T, Q) + +# Compute the determinant of P +det_P = np.linalg.det(P) + +# Display results +print("Matrix P:") +print(P) +print("\nDeterminant of P:", det_P) diff --git a/7/Active_s1.py b/7/Active_s1.py new file mode 100644 index 0000000..50d8142 --- /dev/null +++ b/7/Active_s1.py @@ -0,0 +1,64 @@ +import numpy as np +import scipy.signal as signal +import matplotlib.pyplot as plt + +# Define system matrices +A = np.array([[0, 0, 1, 0], + [0, 0, 0, 1], + [-10, 10, -2, 2], + [60, -660, 12, -12]]) + +b1 = np.array([[0], + [0], + [0.0033], + [-0.02]]) + +b2 = np.array([[0], + [0], + [0], + [600]]) + +B = np.hstack((b1, b2)) + +C = np.array([[1, 0, 0, 0]]) +D = np.array([[0]]) + +# Create state-space system +active_suspension = signal.StateSpace(A, B, C, D) + +# Simulation time +t = np.arange(0, 7, 0.01) + +# Initial conditions +x0 = np.array([0.2, 0, 0, 0]) + +# Simulate initial response +t_initial, x_initial = signal.initial(active_suspension, X0=x0, T=t) + +# Plot initial response +plt.figure(figsize=(10, 6)) +plt.plot(t_initial, x_initial[:, 0], 'k', label='x_1') +plt.plot(t_initial, x_initial[:, 1], 'k-.', label='x_2') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.title('Initial Response of Active Suspension System') +plt.legend() +plt.show() + +# Generate input signal u +u = 0.1 * (np.sin(5*t) + np.sin(9*t) + np.sin(13*t) + np.sin(17*t) + np.sin(21*t)) + +# Simulate the system with input signal u +t_simulation, x_simulation, _ = signal.lsim(active_suspension, u, t, X0=x0) + +# Plot simulation results +plt.figure(figsize=(10, 6)) +plt.plot(t_simulation, x_simulation[:, 0], 'k', label='x_1') +plt.plot(t_simulation, x_simulation[:, 1], 'k-.', label='x_2') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.title('Simulation of Active Suspension System') +plt.legend() +plt.show() diff --git a/7/CL_DCAMOTOR_SOLVER.py b/7/CL_DCAMOTOR_SOLVER.py new file mode 100644 index 0000000..3cba8f1 --- /dev/null +++ b/7/CL_DCAMOTOR_SOLVER.py @@ -0,0 +1,53 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import odeint + +# Define the function representing the ODEs of the DC motor +def DC_motor(x, t): + # Constants + K = 0.1 # Motor constant + R = 1 # Resistance + L = 0.5 # Inductance + J = 0.01 # Moment of inertia + B = 0.1 # Damping coefficient + + # State variables + theta = x[0] + omega = x[1] + i = x[2] + + # Controller (proportional control) + Kp = 10 # Proportional gain + V = Kp * (np.pi/2 - theta) # Control input + + # Dynamic equations + dtheta_dt = omega + domega_dt = (-B/J) * omega + (K/J) * i + di_dt = (-R/L) * i + (V/L) + + return [dtheta_dt, domega_dt, di_dt] + +# Initial conditions and time span +x0 = [0, 0, 0] # Initial state: theta(0) = 0, omega(0) = 0, i(0) = 0 +t = np.linspace(0, 3, 300) # Time span from 0 to 3 seconds + +# Solve the ODEs using odeint +sol = odeint(DC_motor, x0, t) + +# Extract results +theta_deg = sol[:, 0] * 180 / np.pi # Convert theta from radians to degrees + +# Plot the results +plt.figure(figsize=(10, 6)) +plt.plot(t, theta_deg, 'k') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Angular displacement $\\theta$ (degrees)') +plt.title('Closed-Loop DC Motor Simulation') +plt.xlim([0, 3]) +plt.ylim([-10, 100]) +plt.xticks(np.arange(0, 3.5, 0.5)) +plt.yticks(np.arange(-10, 110, 10)) +plt.gca().set_yticklabels(['{:.0f}'.format(x) for x in plt.gca().get_yticks()]) +plt.tight_layout() +plt.show() diff --git a/7/CL_DCmotor_w_solver.py b/7/CL_DCmotor_w_solver.py new file mode 100644 index 0000000..6a06acd --- /dev/null +++ b/7/CL_DCmotor_w_solver.py @@ -0,0 +1,55 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import odeint + +# Define the function representing the ODEs of the DC motor with additional input +def DC_motor_w(x, t): + # Constants + K = 0.1 # Motor constant + R = 1 # Resistance + L = 0.5 # Inductance + J = 0.01 # Moment of inertia + B = 0.1 # Damping coefficient + + # State variables + theta = x[0] + omega = x[1] + i = x[2] + w = x[3] # Additional input + + # Controller (proportional control) + Kp = 10 # Proportional gain + V = Kp * (np.pi/2 - theta) # Control input + + # Dynamic equations + dtheta_dt = omega + domega_dt = (-B/J) * omega + (K/J) * i + di_dt = (-R/L) * i + (V/L) + dw_dt = w # Additional input dynamics + + return [dtheta_dt, domega_dt, di_dt, dw_dt] + +# Initial conditions and time span +x0 = [0, 0, 0, 0.01] # Initial state: theta(0) = 0, omega(0) = 0, i(0) = 0, w(0) = 0.01 +t = np.linspace(0, 3, 300) # Time span from 0 to 3 seconds + +# Solve the ODEs using odeint +sol = odeint(DC_motor_w, x0, t) + +# Extract results +theta_deg = sol[:, 0] * 180 / np.pi # Convert theta from radians to degrees + +# Plot the results +plt.figure(figsize=(10, 6)) +plt.plot(t, theta_deg, 'k') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Angular displacement $\\theta$ (degrees)') +plt.title('Closed-Loop DC Motor Simulation with Additional Input') +plt.xlim([0, 3]) +plt.ylim([-10, 100]) +plt.xticks(np.arange(0, 3.5, 0.5)) +plt.yticks(np.arange(-10, 110, 10)) +plt.gca().set_yticklabels(['{:.0f}'.format(x) for x in plt.gca().get_yticks()]) +plt.tight_layout() +plt.show() diff --git a/7/CL_Invepend_solver.py b/7/CL_Invepend_solver.py new file mode 100644 index 0000000..4d7e9fb --- /dev/null +++ b/7/CL_Invepend_solver.py @@ -0,0 +1,64 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import odeint + + +# Define the function for the inverted pendulum dynamics +def inverted_pendulum_k1(x, t): + # Parameters + m = 0.5 # Mass of the pendulum + M = 1.0 # Mass of the cart + L = 0.5 # Length to the center of mass of the pendulum + g = 9.81 # Acceleration due to gravity + + # State variables + theta = x[2] + theta_dot = x[3] + + # Controller gains + k1 = 8.1563 + k2 = 1.4653 + + # State-space representation + A = np.array([[0, 1, 0, 0], + [0, 0, -m * g / M, 0], + [0, 0, 0, 1], + [0, 0, (M + m) * g / (L * M), 0]]) + + B = np.array([[0], + [1 / M], + [0], + [-1 / (L * M)]]) + + C = np.array([[1, 0, 0, 0], + [0, 0, 1, 0]]) + + D = np.zeros((2, 1)) + + # Controller input + u = -k1 * x[0] - k2 * x[1] + + # State-space equations + dxdt = np.dot(A, x) + np.dot(B, u) + + return dxdt + + +# Initial conditions and time span +x0 = np.array([0, 0, 0.26, 0]) +tspan = np.linspace(0, 4, 401) + +# Solve the differential equations using odeint +x = odeint(inverted_pendulum_k1, x0, tspan) + +# Plotting the results +plt.figure(figsize=(10, 6)) + +plt.plot(tspan, x[:, 0], 'k', label='x (m)') +plt.plot(tspan, x[:, 2] * 180 / np.pi, '-.k', label=r'$\theta$ (degrees)') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State Variables') +plt.legend() +plt.title('Simulation of closed-loop Inverted Pendulum') +plt.show() diff --git a/7/DC_motor.py b/7/DC_motor.py new file mode 100644 index 0000000..eabcbbc --- /dev/null +++ b/7/DC_motor.py @@ -0,0 +1,33 @@ +import numpy as np + + +# Define the function for the DC motor dynamics with disturbance +def DC_motor_w(t, x): + # Parameters and system matrices + A = np.array([[0, 1, 0, 0], + [0, 0, 4.438, -7.396], + [0, -12, -24, 0], + [0, 0, 0, -1]]) + + B = np.array([[0, 0], + [0, -7.396], + [20, 0], + [0, 0]]) + + k = np.array([3.0000, 0.8796, 0.1529, -1.8190]) + + theta_d = 0 # Desired angular position + Tl = 0.01 # Step disturbance + + # Calculate control input v1 + v1 = 2.255 * Tl - k[0] * (x[0] - theta_d) - k[1] * x[1] - k[2] * x[2] + + # Calculate control input v2 (assuming this was intended) + v2 = 2.255 * Tl - np.dot(k, x) + + u = np.array([v1, Tl]) + + # State-space equations + xp = np.dot(A, x) + np.dot(B, u) + + return xp diff --git a/7/DC_motor_w.py b/7/DC_motor_w.py new file mode 100644 index 0000000..e69de29 diff --git a/7/Invpend_solver2.py b/7/Invpend_solver2.py new file mode 100644 index 0000000..71b2708 --- /dev/null +++ b/7/Invpend_solver2.py @@ -0,0 +1,41 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import odeint + + +# Define the function for the inverted pendulum dynamics +def inverted_pendulum_k2(x, t): + # Parameters and system matrices + A = np.array([[0, 0, 1, 0], + [0, 0, 0, 1], + [-10, 10, -2, 2], + [60, -660, 12, -12]]) + + b1 = np.array([0, 0, 0.0033, -0.02]) + b2 = np.array([0, 0, 0, 600]) + B = np.column_stack((b1, b2)) + + # Control input + u = np.array([0]) # No control input defined in the MATLAB script + + # State-space equations + xp = np.dot(A, x) + np.dot(B, u) + + return xp + + +# Initial conditions and time span +x0 = np.array([0, 0, 0.6, 0]) +t = np.linspace(0, 3, 301) # Time points for simulation + +# Solve the differential equation using odeint +x = odeint(inverted_pendulum_k2, x0, t) + +# Plot the results +plt.plot(t, x[:, 0], 'k', label='x (m)') +plt.plot(t, x[:, 2], '-.k', label='theta (rad)') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State Variables') +plt.legend() +plt.show() diff --git a/7/active_fb.py b/7/active_fb.py new file mode 100644 index 0000000..6fcb976 --- /dev/null +++ b/7/active_fb.py @@ -0,0 +1,42 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.signal import ss, place_poles, lsim + +# Define system matrices +A = np.array([[0, 0, 1, -1, 0], + [0, 0, 1, 0, 0], + [-10, 0, -2, 2, 0], + [720, -660, 12, -12, 0], + [1, 0, 0, 0, 0]]) + +b1 = np.array([[0], [0], [0.00333], [-0.02], [0]]) +b2 = np.array([[0], [-1], [0], [0], [0]]) +b3 = np.array([[0], [0], [0], [0], [1]]) + +pd = np.array([-5, -25 + 25j, -25 - 25j, -3 + 3j, -3 - 3j]) + +# Perform pole placement +k = place_poles(A.T, b1.T, pd).gain_matrix.T + +# Closed-loop system +Acl = A - np.dot(b1, k) +Bcl = 0.1 * b2 # Assuming a scaling factor of 0.1 for b2 +C = np.array([[1, 0, 0, 0, 0]]) +D = np.array([[0]]) +ld = 0.1 + +# Create state-space system +active_fb = ss(Acl, Bcl, C, D) + +# Simulate impulse response +t = np.linspace(0, 5, 1000) # Time vector +t, x, _ = lsim(active_fb, T=t) + +# Plot the result +plt.plot(t, x[:, 0] + ld, 'k', label='l_1') +plt.plot(t, x[:, 4] - 0.574 * ld, 'k-.', label='x') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State Variables') +plt.legend() +plt.show() diff --git a/7/ex6_10.py b/7/ex6_10.py new file mode 100644 index 0000000..0d0bde0 --- /dev/null +++ b/7/ex6_10.py @@ -0,0 +1,31 @@ +import numpy as np + +# Define system matrices and vector +A = np.array([[-2, -1, 2], + [-1, -2, 2], + [-2, 0, 2]]) +B = np.array([[0, 0], + [0, 1], + [1, 0]]) +f = np.array([[1], + [1]]) + +# Calculate b +b = np.dot(B, f) + +# Eigen decomposition of A +w, v = np.linalg.eig(A) +v_inv = np.linalg.inv(v) + +# Calculate p +p = np.dot(v_inv[:2, :], b) + +# Print results +print("Eigenvalues of A:") +print(w) +print("\nMatrix of eigenvectors W:") +print(w) +print("\nInverse of W:") +print(v_inv) +print("\np (projection of b onto the first two eigenvectors of A):") +print(p) diff --git a/7/ex6_13.py b/7/ex6_13.py new file mode 100644 index 0000000..60ccedd --- /dev/null +++ b/7/ex6_13.py @@ -0,0 +1,96 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import odeint +from control import ss, lqr, initial_response + +# Define system matrices and parameters +A = np.array([[0, 1, 0], + [0, 0, 4.438], + [0, -12, -24]]) +b = np.array([[0], + [0], + [20]]) +R = 1 +C = np.array([[1, 0, 0]]) +D = np.array([[0]]) +x0 = np.array([[-1], [0], [0]]) + +# Define different Q matrices for LQR +Q1 = np.diag([4, 0, 0]) +Q2 = np.diag([9, 0, 0]) +Q3 = np.diag([20, 0, 0]) + +# Calculate gains for LQR controllers +k1, _, _ = lqr(A, b, Q1, R) +k2, _, _ = lqr(A, b, Q2, R) +k3, _, _ = lqr(A, b, Q3, R) + +# Create closed-loop systems +Acl1 = A - np.dot(b, k1) +CL_sys1 = ss(Acl1, b, C, D) +Acl2 = A - np.dot(b, k2) +CL_sys2 = ss(Acl2, b, C, D) +Acl3 = A - np.dot(b, k3) +CL_sys3 = ss(Acl3, b, C, D) + +# Simulate initial response for different Q matrices +t1, y1, x1 = initial_response(CL_sys1, T=np.linspace(0, 2, 100), X0=x0) +u1 = -np.dot(k1, x1.T) + +t2, y2, x2 = initial_response(CL_sys2, T=np.linspace(0, 2, 100), X0=x0) +u2 = -np.dot(k2, x2.T) + +t3, y3, x3 = initial_response(CL_sys3, T=np.linspace(0, 2, 100), X0=x0) +u3 = -np.dot(k3, x3.T) + +# Plot results for angular error and motor voltage +plt.figure(figsize=(12, 6)) +plt.subplot(121) +plt.plot(t1, y1.flatten(), 'k-.', label='Q11=4') +plt.plot(t2, y2.flatten(), 'k', label='Q11=9') +plt.plot(t3, y3.flatten(), 'k--', label='Q11=20') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Angular Error (rad)') +plt.legend(loc='best') + +plt.subplot(122) +plt.plot(t1, u1.flatten(), 'k-.', label='Q11=4') +plt.plot(t2, u2.flatten(), 'k', label='Q11=9') +plt.plot(t3, u3.flatten(), 'k--', label='Q11=20') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Motor Voltage (V)') +plt.legend(loc='best') +plt.tight_layout() + +# Additional Q matrix for comparison +Q4 = np.diag([9, 3, 0]) +k4, _, _ = lqr(A, b, Q4, R) +Acl4 = A - np.dot(b, k4) +CL_sys4 = ss(Acl4, b, C, D) + +# Simulate initial response for Q22=0 and Q22=3 +t4, y4, x4 = initial_response(CL_sys4, T=np.linspace(0, 2, 100), X0=x0) +u4 = -np.dot(k4, x4.T) + +# Plot results for angular error and angular velocity +plt.figure(figsize=(12, 6)) +plt.subplot(121) +plt.plot(t2, y2.flatten(), 'k', label='Q22=0') +plt.plot(t4, y4.flatten(), 'k-.', label='Q22=3') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Angular Error (rad)') +plt.legend(loc='best') + +plt.subplot(122) +plt.plot(t2, x2[:, 1], 'k', label='Q22=0') +plt.plot(t4, x4[:, 1], 'k-.', label='Q22=3') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Angular Velocity (rad/sec)') +plt.legend(loc='best') +plt.tight_layout() + +plt.show() diff --git a/7/ex6_14.py b/7/ex6_14.py new file mode 100644 index 0000000..78842ba --- /dev/null +++ b/7/ex6_14.py @@ -0,0 +1,21 @@ +import numpy as np +from control import ss, lqr + +# Define system matrices and parameters +A = np.array([[0, 1, 0, 0], + [0, 0, -9.8, 0], + [0, 0, 0, 1], + [0, 0, 19.6, 0]]) +b = np.array([[0], + [1], + [0], + [-1]]) +Q = np.diag([4, 0, 8.16, 0]) +R = 1/400 + +# Calculate LQR gain +k, _, _ = lqr(A, b, Q, R) + +# Display the LQR gain matrix +print("LQR Gain Matrix (k):") +print(k) diff --git a/7/ex6_15.py b/7/ex6_15.py new file mode 100644 index 0000000..7890453 --- /dev/null +++ b/7/ex6_15.py @@ -0,0 +1,21 @@ +import numpy as np +from control import ss, lqr + +# Define system matrices and parameters +A = np.array([[0, 1, 0, 0], + [0, 0, 4.438, -7.396], + [0, -12, -24, 0], + [0, 0, 0, -1]]) +b = np.array([[0], + [0], + [20], + [0]]) +Q1 = np.diag([9, 0, 0, 0]) +R = 1 + +# Calculate LQR gain +k, _, _ = lqr(A, b, Q1, R) + +# Display the LQR gain matrix +print("LQR Gain Matrix (k):") +print(k) diff --git a/7/ex6_2.py b/7/ex6_2.py new file mode 100644 index 0000000..0ee2348 --- /dev/null +++ b/7/ex6_2.py @@ -0,0 +1,18 @@ +import numpy as np +from control import ss, place + +# Define system matrices and desired poles +A = np.array([[0, 1, 0], + [0, 0, 4.438], + [0, -12, -24]]) +b = np.array([[0], + [0], + [20]]) +pd = np.array([-24, -3-3j, -3+3j]) # Desired poles + +# Perform pole placement to find the gain matrix +k = place(A, b, pd) + +# Display the gain matrix +print("Gain Matrix (k):") +print(k) diff --git a/7/ex6_3.py b/7/ex6_3.py new file mode 100644 index 0000000..c6ed1aa --- /dev/null +++ b/7/ex6_3.py @@ -0,0 +1,21 @@ +import numpy as np +from control import ss, lqr + +# Define system matrices and cost weights +A = np.array([[0, 1, 0, 0], + [0, 0, -9.8, 0], + [0, 0, 0, 1], + [0, 0, 19.6, 0]]) +b = np.array([[0], + [1], + [0], + [-1]]) +Q = np.diag([4, 0, 8.16, 0]) +R = 1 / 400 + +# Calculate the LQR gain matrix +k, _, _ = lqr(A, b, Q, R) + +# Display the LQR gain matrix +print("LQR Gain Matrix (k):") +print(k) diff --git a/7/ex6_4.py b/7/ex6_4.py new file mode 100644 index 0000000..03a624b --- /dev/null +++ b/7/ex6_4.py @@ -0,0 +1,31 @@ +import numpy as np + +# Define system matrices +A = np.array([[0, 1, 0, 0], + [0, 0, -9.8, 0], + [0, 0, 0, 1], + [0, 0, 19.6, 0]]) +b = np.array([[0], + [1], + [0], + [-1]]) + +# Controllability matrix +C = np.hstack([b, np.dot(A, b), np.dot(A**2, b), np.dot(A**3, b)]) + +# Given values +a = np.array([0, -19.6, 0, 0]) +alpha = np.array([12.86, 63.065, 149.38, 157.0]) + +# Psi matrix +Psi = np.array([[1, a[0], a[1], a[2]], + [0, 1, a[0], a[1]], + [0, 0, 1, a[0]], + [0, 0, 0, 1]]) + +# Calculate the gain matrix k +k = np.dot((alpha - a), np.linalg.inv(np.dot(C, Psi))) + +# Display the calculated gain matrix k +print("Calculated Gain Matrix (k):") +print(k) diff --git a/7/ex6_5.py b/7/ex6_5.py new file mode 100644 index 0000000..00c10ab --- /dev/null +++ b/7/ex6_5.py @@ -0,0 +1,31 @@ +import numpy as np + +# Define system matrices +A = np.array([[0, 1, 0, 0], + [0, 0, -9.8, 0], + [0, 0, 0, 1], + [0, 0, 19.6, 0]]) +b = np.array([[0], + [1], + [0], + [-1]]) + +# Controllability matrix +C = np.hstack([b, np.dot(A, b), np.dot(A**2, b), np.dot(A**3, b)]) + +# Given values +a = np.array([0, -19.6, 0, 0]) +alpha = np.array([12.86, 63.065, 149.38, 157.0]) + +# Psi_1 matrix +Psi_1 = np.array([[1, -a[0], a[0]**2 - a[1], -a[0]**3 + 2*a[0]*a[1] - a[2]], + [0, 1, -a[0], a[0]**2 - a[1]], + [0, 0, 1, -a[0]], + [0, 0, 0, 1]]) + +# Calculate the gain matrix k +k = np.dot((alpha - a), np.dot(Psi_1, np.linalg.inv(C))) + +# Display the calculated gain matrix k +print("Calculated Gain Matrix (k):") +print(k) diff --git a/7/ex6_6.py b/7/ex6_6.py new file mode 100644 index 0000000..cd3d395 --- /dev/null +++ b/7/ex6_6.py @@ -0,0 +1,24 @@ +import numpy as np + +# Define system matrices +A = np.array([[0, 1, 0, 0], + [0, 0, -9.8, 0], + [0, 0, 0, 1], + [0, 0, 19.6, 0]]) +b = np.array([[0], + [1], + [0], + [-1]]) + +# Eigenvalues of A +e, _ = np.linalg.eig(A) +print("Eigenvalues of A:") +print(e) + +# Desired closed-loop eigenvalues +pd = np.array([-4.43, -4.43, -2-2j, -2+2j]) + +# Calculate the gain matrix k using Ackermann's formula +k = np.linalg.solve(np.transpose(b), np.poly(pd)) +print("\nCalculated Gain Matrix (k):") +print(k) diff --git a/7/ex6_9.py b/7/ex6_9.py new file mode 100644 index 0000000..36031d8 --- /dev/null +++ b/7/ex6_9.py @@ -0,0 +1,55 @@ +import numpy as np + +# Define system matrices +A = np.array([[-2, -1, 2], + [-1, -2, 2], + [-2, 0, 2]]) + +B = np.array([[0, 0], + [0, 1], + [1, 0]]) + +f = np.array([[1], + [1]]) + +# Calculate b and C +b = np.dot(B, f) +C = np.linalg.matrix_power(np.hstack((A, np.dot(B, f))), 3) + +# Define Psi and delta matrices +Psi = np.array([[1, 2, -1], + [0, 1, 2], + [0, 0, 1]]) + +delta = np.array([4, 13, 10]) + +# Calculate M +M = np.dot(delta, np.linalg.inv(np.dot(C, Psi))) + +# Calculate K1 +K1 = np.dot(f.T, M) + +# Desired closed-loop eigenvalues +pd = np.array([-2, -2, -2]) + +# Calculate the gain matrix k using Ackermann's formula +k = np.linalg.solve(np.transpose(b), np.poly(pd)) + +# Calculate K2 +K2 = np.dot(f.T, k) + +# Calculate Ac and its eigenvalues +Ac = A - np.dot(B, K1) +eigenvalues_Ac, _ = np.linalg.eig(Ac) + +# Display results +print("M matrix:") +print(M) +print("\nK1 matrix:") +print(K1) +print("\nGain matrix k:") +print(k) +print("\nK2 matrix:") +print(K2) +print("\nEigenvalues of Ac:") +print(eigenvalues_Ac) diff --git a/7/fig6_5.py b/7/fig6_5.py new file mode 100644 index 0000000..b88a1b0 --- /dev/null +++ b/7/fig6_5.py @@ -0,0 +1,47 @@ +import numpy as np +import matplotlib.pyplot as plt + +# Define range of k values +k = np.arange(0.02, 5.02, 0.02) + +# Initialize x vector +x = np.array([1, 0]) + +# Calculate J1, J2, J3 for different values of r +r_values = [0, 1, 2] +colors = ['k', 'k-.', 'k--'] + +plt.figure(figsize=(10, 6)) + +for i, r in enumerate(r_values): + p11 = 0.5 / k + 0.5 + (r + 1) * k / 2 + r * k ** 2 / 2 + p12 = 0 / (5 * k) + r * k / 2 + p22 = 0.5 / k + 0.5 + r * k / 2 + + J = p11 * x[0] ** 2 + 2 * p12 * x[0] * x[1] + p22 * x[1] ** 2 + plt.plot(k, J, colors[i], linewidth=2, label=f'r={r}') + +plt.grid(True) +plt.xlabel('k') +plt.ylabel('J') +plt.legend() +plt.show() + +# Plotting the second figure +plt.figure(figsize=(10, 6)) + +x = np.array([0, 1]) +r = 2 +p11 = 0.5 / k + 0.5 + (r + 1) * k / 2 + r * k ** 2 / 2 +p12 = 0 / (5 * k) + r * k / 2 +p22 = 0.5 / k + 0.5 + r * k / 2 + +J = p11 * x[0] ** 2 + 2 * p12 * x[0] * x[1] + p22 * x[1] ** 2 +plt.plot(k, J, 'k', linewidth=2, label=f'r={r}') +plt.plot(k, J2, 'k-.', linewidth=2, label='r=1') + +plt.grid(True) +plt.xlabel('k') +plt.ylabel('J') +plt.legend() +plt.show() diff --git a/7/inverted_pendulum_k1.py b/7/inverted_pendulum_k1.py new file mode 100644 index 0000000..18c0d51 --- /dev/null +++ b/7/inverted_pendulum_k1.py @@ -0,0 +1,30 @@ +import numpy as np + + +def inverted_pendulum_k1(t, x): + # Parameters + g = 9.8 + l = 1 + m = 1 + M = 1 + + # State feedback gains (taken from the MATLAB code) + k = [-16.0203, -15.2428, -98.6852, -28.1028] + + # Compute control input F (state feedback) + F = -np.dot(k, x) + + # Intermediate variables for dynamics + d1 = M + m * (1 - np.cos(x[2]) ** 2) + d2 = l * d1 + + # State equations + xp = np.array([ + x[1], + (F + m * l * x[3] ** 2 * np.sin(x[2]) - m * g * np.sin(x[2]) * np.cos(x[2])) / d1, + x[3], + (-F * np.cos(x[2]) - m * l * x[3] ** 2 * np.sin(x[2]) * np.cos(x[2]) + + (M + m) * g * np.sin(x[2])) / d2 + ]) + + return xp diff --git a/7/inverted_pendulum_k2.py b/7/inverted_pendulum_k2.py new file mode 100644 index 0000000..744134f --- /dev/null +++ b/7/inverted_pendulum_k2.py @@ -0,0 +1,21 @@ +from scipy.integrate import odeint + +# Initial state +x0 = [0, 0, 0.6, 0] # Example initial condition: [x=0, v=0, theta=0.6 rad, omega=0] + +# Time span for simulation +tspan = np.linspace(0, 4, 401) # 401 points from t=0 to t=4 + +# Numerically integrate the system +sol = odeint(inverted_pendulum_k2, x0, tspan) + +# Plot results (example) +import matplotlib.pyplot as plt + +plt.plot(tspan, sol[:, 0], 'k', label='x (m)') +plt.plot(tspan, sol[:, 2], 'k-.', label='theta (rad)') +plt.xlabel('Time (sec)') +plt.ylabel('State Variables') +plt.legend() +plt.grid(True) +plt.show() diff --git a/7/train_fb.py b/7/train_fb.py new file mode 100644 index 0000000..1ab4959 --- /dev/null +++ b/7/train_fb.py @@ -0,0 +1,38 @@ +import numpy as np + + +def train_fb(t, x): + # Parameters + A = np.array([ + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1, -1, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 1, -1, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 1, -1, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 1, -1], + [0, -12.5, 0, 0, 0, -0.75, 0.75, 0, 0, 0], + [0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0, 0], + [0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0], + [0, 0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75], + [0, 0, 0, 0, 62.5, 0, 0, 0, 3.75, -3.75] + ]) + + b1 = np.array([0, 0, 0, 0, 0, 0.005, 0, 0, 0, 0]).reshape(-1, 1) # Force input + b2 = np.array([0, 0, 0, 0, 0, 250, 0, 0, 0, -1250]).reshape(-1, 1) # Constant input + + vd = 25 * (1 - np.exp(-t / 40)) + + # State feedback gains (taken from the MATLAB code) + k = np.array([0.4559, 0.3331, 0.2170, 0.1069, 11.5387, + -0.2622, -0.3371, -0.3865, -0.4110, 5.3731]).reshape(-1, 1) + + dx = np.array([x[1] - 20, x[2] - 20, x[3] - 20, x[4] - 20]).reshape(-1, 1) + dv = np.array([x[5] - vd, x[6] - vd, x[7] - vd, x[8] - vd, x[9] - vd]).reshape(-1, 1) + z = x[5] - vd + + X = np.vstack((dx, dv, z)) + + u = -np.dot(k.T, X) + + xp = np.dot(A, x) + np.dot(b1, u) + b2.flatten() + + return xp diff --git a/7/train_fb1.py b/7/train_fb1.py new file mode 100644 index 0000000..8dabfd6 --- /dev/null +++ b/7/train_fb1.py @@ -0,0 +1,40 @@ +from scipy.integrate import odeint +import numpy as np +# Initial state +x0 = [0, 0, 0, 0, 0, 20, 20, 20, 20, 20] # Example initial condition + +# Time span for simulation +tspan = np.linspace(0, 100, 1001) # 1001 points from t=0 to t=100 + +# Numerically integrate the system +sol = odeint(train_fb, x0, tspan) + +# Plot results (example) +import matplotlib.pyplot as plt + +plt.figure(figsize=(10, 6)) + +plt.subplot(2, 1, 1) +plt.plot(tspan, sol[:, 0], 'k', label='x1') +plt.plot(tspan, sol[:, 1], 'b', label='x2') +plt.plot(tspan, sol[:, 2], 'g', label='x3') +plt.plot(tspan, sol[:, 3], 'r', label='x4') +plt.plot(tspan, sol[:, 4], 'c', label='x5') +plt.xlabel('Time (sec)') +plt.ylabel('State Variables') +plt.legend() +plt.grid(True) + +plt.subplot(2, 1, 2) +plt.plot(tspan, sol[:, 5], 'k', label='v1') +plt.plot(tspan, sol[:, 6], 'b', label='v2') +plt.plot(tspan, sol[:, 7], 'g', label='v3') +plt.plot(tspan, sol[:, 8], 'r', label='v4') +plt.plot(tspan, sol[:, 9], 'c', label='v5') +plt.xlabel('Time (sec)') +plt.ylabel('State Variables') +plt.legend() +plt.grid(True) + +plt.tight_layout() +plt.show() diff --git a/7/train_fb2.py b/7/train_fb2.py new file mode 100644 index 0000000..3461698 --- /dev/null +++ b/7/train_fb2.py @@ -0,0 +1,40 @@ +from scipy.integrate import odeint + +# Initial state +x0 = [0, 0, 0, 0, 0, 20, 20, 20, 20, 20] # Example initial condition + +# Time span for simulation +tspan = np.linspace(0, 100, 1001) # 1001 points from t=0 to t=100 + +# Numerically integrate the system +sol = odeint(train_fb2, x0, tspan) + +# Plot results (example) +import matplotlib.pyplot as plt +import numpy as np +plt.figure(figsize=(10, 6)) + +plt.subplot(2, 1, 1) +plt.plot(tspan, sol[:, 0], 'k', label='x1') +plt.plot(tspan, sol[:, 1], 'b', label='x2') +plt.plot(tspan, sol[:, 2], 'g', label='x3') +plt.plot(tspan, sol[:, 3], 'r', label='x4') +plt.plot(tspan, sol[:, 4], 'c', label='x5') +plt.xlabel('Time (sec)') +plt.ylabel('State Variables') +plt.legend() +plt.grid(True) + +plt.subplot(2, 1, 2) +plt.plot(tspan, sol[:, 5], 'k', label='v1') +plt.plot(tspan, sol[:, 6], 'b', label='v2') +plt.plot(tspan, sol[:, 7], 'g', label='v3') +plt.plot(tspan, sol[:, 8], 'r', label='v4') +plt.plot(tspan, sol[:, 9], 'c', label='v5') +plt.xlabel('Time (sec)') +plt.ylabel('State Variables') +plt.legend() +plt.grid(True) + +plt.tight_layout() +plt.show() diff --git a/7/train_fb_solver.py b/7/train_fb_solver.py new file mode 100644 index 0000000..f74c25c --- /dev/null +++ b/7/train_fb_solver.py @@ -0,0 +1,92 @@ +import numpy as np +from scipy.integrate import odeint +import matplotlib.pyplot as plt + + +def train_fb(t, x): + A = np.array([ + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1, -1, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 1, -1, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 1, -1, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 1, -1], + [-12.5, 0, 0, 0, 0, -0.75, 0.75, 0, 0, 0], + [62.5, -62.5, 0, 0, 0, 3.75, -7.5, 3.75, 0, 0], + [0, 62.5, -62.5, 0, 0, 0, 3.75, -7.5, 3.75, 0], + [0, 0, 62.5, -62.5, 0, 0, 0, 3.75, -7.5, 3.75], + [0, 0, 0, 62.5, -62.5, 0, 0, 0, 3.75, -3.75] + ]) + + b1 = np.array([0, 0, 0, 0, 0, 0.005, 0, 0, 0, 0]).reshape(-1, 1) # Force input + b2 = np.array([0, 0, 0, 0, 0, 250, 0, 0, 0, -1250]).reshape(-1, 1) # Constant input + + vd = 25 * (1 - np.exp(-t / 40)) # Desired velocity + + k = np.array([54.5333, 16.2848, -1.3027, -4.3607, 191.7414, + -40.4841, -34.2067, -29.7070, -27.3437, 52.0886]).reshape(-1, 1) # State feedback gains + + dx = np.array([x[1] - 20, x[2] - 20, x[3] - 20, x[4] - 20]).reshape(-1, 1) + dv = np.array([x[6] - vd, x[7] - vd, x[8] - vd, x[9] - vd, x[10] - vd]).reshape(-1, 1) + z = x[6] - vd + + X = np.vstack((dx, dv, z)) + + u = np.dot(k.T, X) + + xp = np.dot(A, x) + np.dot(b1.T, u) + b2.flatten() + + return xp + + +# Initial conditions +x0 = [0, 20, 20, 20, 20, 0, 0, 0, 0, 0] # Initial state [x1, x2, x3, x4, x5, v1, v2, v3, v4, v5] + +# Time span +tspan = np.linspace(0, 300, 1001) # 1001 points from t=0 to t=300 + +# Numerically integrate the system using odeint +sol = odeint(train_fb, x0, tspan) + +# Desired velocity and input force calculation +vd = 25 * (1 - np.exp(-tspan / 40)) +k = np.array([54.5333, 16.2848, -1.3027, -4.3607, 191.7414, + -40.4841, -34.2067, -29.7070, -27.3437, 52.0886]) + +# Calculate forces +Fs = np.zeros((3, len(tspan))) +for i in range(len(tspan)): + dx = sol[i, 1:5] - 20 + X = np.hstack((dx, sol[i, 6:11] - vd[i], sol[i, 6] - vd[i])) + F = -np.dot(k, X) + Fs[0, i] = F[0] + Fs[1, i] = F[3] + Fs[2, i] = 150 * (sol[i, 6] - sol[i, 7]) + +# Plotting +plt.figure(figsize=(12, 10)) + +plt.subplot(3, 1, 1) +plt.plot(tspan, sol[:, 0] / 1000, 'k') +plt.xlabel('Time (sec)') +plt.ylabel('Train Position (Km)') +plt.grid(True) + +plt.subplot(3, 1, 2) +plt.plot(tspan, vd, 'k', label='Desired Velocity') +plt.plot(tspan, sol[:, 6], '-.k', label='Real Velocity') +plt.xlabel('Time (sec)') +plt.ylabel('Train Velocity (m)') +plt.legend() +plt.grid(True) + +plt.subplot(3, 1, 3) +plt.plot(tspan, Fs[0], 'k', label='Input Force') +plt.plot(tspan, Fs[1], '-.k', label='Spring Force 1') +plt.plot(tspan, Fs[2], '--k', label='Damping Force 1') +plt.xlabel('Time (sec)') +plt.ylabel('Force (KN)') +plt.legend() +plt.grid(True) + +plt.tight_layout() +plt.show() diff --git a/7/train_lqr.py b/7/train_lqr.py new file mode 100644 index 0000000..2bb2477 --- /dev/null +++ b/7/train_lqr.py @@ -0,0 +1,51 @@ +import numpy as np +from scipy.integrate import odeint +import matplotlib.pyplot as plt +from scipy.linalg import solve_continuous_are + +# State matrix A +A = np.array([ + [0, 0, 0, 0, 1, -1, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1, -1, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 1, -1, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 1, -1, 0], + [-12.5, 0, 0, 0, -0.75, 0.75, 0, 0, 0, 0], + [62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0, 0, 0], + [0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0, 0], + [0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0], + [0, 0, 0, 62.5, -62.5, 0, 0, 0, 3.75, -3.75], + [0, 0, 0, 0, 0, 0, 0, 0, 0, -1/40] +]) + +# Input matrix B +B = np.array([0, 0, 0, 0, 0.005, 0, 0, 0, 0, 0]).reshape(-1, 1) + +# Cost matrices Q and R +Q = np.diag([3.34**2, 3.34**2, 3.34**2, 3.34**2, 3**2 + 0.5**2, + 2 * 3**2, 2 * 3**2, 2 * 3**2, 3**2, 0.5**2]) +Q[5, 4] = -9 +Q[4, 5] = -9 +Q[6, 5] = -9 +Q[5, 6] = -9 +Q[7, 6] = -9 +Q[6, 7] = -9 +Q[8, 7] = -9 +Q[7, 8] = -9 +Q[9, 8] = -9 +Q[8, 9] = -9 +Q[9, 4] = 0.5**2 +Q[4, 9] = 0.5**2 + +R = 1 / 120**2 +R1 = 35 * R + +# Calculate the LQR gain matrix for R = 1/120^2 +K = solve_continuous_are(A, B, Q, R) +# Calculate the LQR gain matrix for R1 = 35 * 1/120^2 +K1 = solve_continuous_are(A, B, Q, R1) + +# Print the results +print("LQR Gain Matrix K (R = 1/120^2):") +print(K) +print("\nLQR Gain Matrix K1 (R1 = 35 * 1/120^2):") +print(K1) diff --git a/7/train_model.py b/7/train_model.py new file mode 100644 index 0000000..f88576d --- /dev/null +++ b/7/train_model.py @@ -0,0 +1,60 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import odeint + + +def train_model(t, x): + # State matrix A + A = np.array([ + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1, -1, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 1, -1, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 1, -1, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 1, -1], + [0, -12.5, 0, 0, 0, -0.75, 0.75, 0, 0, 0], + [0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0, 0], + [0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0], + [0, 0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75], + [0, 0, 0, 0, 62.5, 0, 0, 0, 3.75, -3.75] + ]) + + # Input matrices b1 and b2 + b1 = np.array([0, 0, 0, 0, 0, 0.005, 0, 0, 0, 0]).reshape(-1, 1) + b2 = np.array([0, 0, 0, 0, 0, 250, 0, 0, 0, -1250]).reshape(-1, 1) + + # Exponentially decreasing input + u = 750 * np.exp(-t / 10) + + # State equations + xp = np.dot(A, x) + b1 * u + b2 + return xp.flatten() + + +# Initial conditions +x0 = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) + +# Time span for simulation +t = np.linspace(0, 40, 1000) # Adjust the end time and number of time points as needed + +# Solve the ODE system +x = odeint(train_model, x0, t) + +# Plotting results (example) +plt.figure(figsize=(10, 6)) + +plt.subplot(211) +plt.plot(t, x[:, 0], 'b-', label='Position x1') +plt.xlabel('Time') +plt.ylabel('Position') +plt.grid() +plt.legend() + +plt.subplot(212) +plt.plot(t, x[:, 5], 'r-', label='Velocity v1') +plt.xlabel('Time') +plt.ylabel('Velocity') +plt.grid() +plt.legend() + +plt.tight_layout() +plt.show() diff --git a/7/train_model1.py b/7/train_model1.py new file mode 100644 index 0000000..fd11065 --- /dev/null +++ b/7/train_model1.py @@ -0,0 +1,60 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import odeint + + +def train_model1(t, x): + # State matrix A + A = np.array([ + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1, -1, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 1, -1, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 1, -1, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 1, -1], + [0, -12.5, 0, 0, 0, -0.75, 0.75, 0, 0, 0], + [0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0, 0], + [0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0], + [0, 0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75], + [0, 0, 0, 0, 62.5, 0, 0, 0, 3.75, -3.75] + ]) + + # Input matrices b1 and b2 + b1 = np.array([0, 0, 0, 0, 0.005, 0, 0, 0, 0, 0]).reshape(-1, 1) + b2 = np.array([0, 0, 0, 0, 250, 0, 0, 0, 0, -1250]).reshape(-1, 1) + + # Constant input + u = 750 + + # State equations + xp = np.dot(A, x) + b1 * u + b2 + return xp.flatten() + + +# Initial conditions +x0 = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) + +# Time span for simulation +t = np.linspace(0, 40, 1000) # Adjust the end time and number of time points as needed + +# Solve the ODE system +x = odeint(train_model1, x0, t) + +# Plotting results (example) +plt.figure(figsize=(10, 6)) + +plt.subplot(211) +plt.plot(t, x[:, 0], 'b-', label='Position x1') +plt.xlabel('Time') +plt.ylabel('Position') +plt.grid() +plt.legend() + +plt.subplot(212) +plt.plot(t, x[:, 5], 'r-', label='Velocity v1') +plt.xlabel('Time') +plt.ylabel('Velocity') +plt.grid() +plt.legend() + +plt.tight_layout() +plt.show() diff --git a/7/train_solver1.py b/7/train_solver1.py new file mode 100644 index 0000000..bcf81c9 --- /dev/null +++ b/7/train_solver1.py @@ -0,0 +1,53 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import odeint + + +# Define the train model function +def train_model1(x, t): + # State matrix A + A = np.array([ + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1, -1, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 1, -1, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 1, -1, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 1, -1], + [0, -12.5, 0, 0, 0, -0.75, 0.75, 0, 0, 0], + [0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0, 0], + [0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0], + [0, 0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75], + [0, 0, 0, 0, 62.5, 0, 0, 0, 3.75, -3.75] + ]) + + # Input matrices b1 and b2 + b1 = np.array([0, 0, 0, 0, 0.005, 0, 0, 0, 0, 0]).reshape(-1, 1) + b2 = np.array([0, 0, 0, 0, 250, 0, 0, 0, 0, -1250]).reshape(-1, 1) + + # Constant input + u = 750 + + # State equations + xp = np.dot(A, x) + b1 * u + b2 + return xp.flatten() + + +# Initial conditions +x0 = np.array([0, 20, 20, 20, 20, 0, 0, 0, 0, 0]) + +# Time span for simulation +t = np.linspace(0, 10, 1000) # Adjust the end time and number of time points as needed + +# Solve the ODE system +x = odeint(train_model1, x0, t) + +# Plotting results +plt.figure(figsize=(10, 6)) + +plt.plot(t, x[:, 1], 'k', label='x2') +plt.plot(t, x[:, 4], 'k-.', label='x5') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State Variables') +plt.legend() +plt.title('Simulation of Train Model') +plt.show() diff --git a/8/CL_DCmotor_LTR_solver.py b/8/CL_DCmotor_LTR_solver.py new file mode 100644 index 0000000..375b38b --- /dev/null +++ b/8/CL_DCmotor_LTR_solver.py @@ -0,0 +1,104 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import odeint + + +# Define global parameter +class Par: + Tl = 0.01 + + +# Define the DC motor model function with observer +def DC_motor_LTR1(x, t): + # System parameters + R = 1.5 # Ohms + L = 0.5 # Henry + J = 0.01 # kg.m^2 + b = 0.1 # N.m.s + Ke = 0.01 # V/rad/sec + Kt = 0.01 # N.m/Amp + + # Observer gains + L1 = 15 + L2 = 12 + L3 = 10 + + # State variables + theta = x[0] + omega = x[1] + i = x[2] + + theta_h = x[3] + omega_h = x[4] + i_h = x[5] + + # Disturbance torque + Tl = Par.Tl * np.exp(-t) + Tl_h = x[6] + + # System dynamics (state equations) + dtheta_dt = omega + domega_dt = (-b / J) * omega + (Kt / J) * (i - i_h) + di_dt = (-R / L) * i - (Ke / L) * omega + (1 / L) * Tl + + # Observer dynamics + dtheta_h_dt = omega_h + domega_h_dt = (-b / J) * omega_h + (Kt / J) * (i_h - L1 * (theta_h - theta)) + di_h_dt = (-R / L) * i_h - (Ke / L) * omega_h + L2 * (omega_h - omega) - L3 * (i_h - i) + + # Disturbance torque estimate dynamics + dTl_h_dt = L3 * (i_h - i) + + return [dtheta_dt, domega_dt, di_dt, dtheta_h_dt, domega_h_dt, di_h_dt, dTl_h_dt] + + +# Initial conditions +x0 = [0, 0, 0, 0, 0, 0, 0] + +# Time span for simulation +t = np.linspace(0, 5, 500) # Adjust the number of time points as needed + +# Solve the ODE system +x = odeint(DC_motor_LTR1, x0, t) + +# Plotting results +plt.figure(figsize=(12, 10)) + +# Subplot for Angular displacement +plt.subplot(221) +plt.plot(t, x[:, 0], 'k', label=r'$\theta$') +plt.plot(t, x[:, 3], '-.k', label=r'$\theta_h$') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Angular displacement (rad)') +plt.legend() + +# Subplot for Angular velocity +plt.subplot(222) +plt.plot(t, x[:, 1], 'k', label=r'$\omega$') +plt.plot(t, x[:, 4], '-.k', label=r'$\omega_h$') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Angular velocity (rad/sec)') +plt.legend() + +# Subplot for Motor Current +plt.subplot(223) +plt.plot(t, x[:, 2], 'k', label='i') +plt.plot(t, x[:, 5], '-.k', label='i_h') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Motor Current (Amp)') +plt.legend() + +# Subplot for Disturbance Torque +plt.subplot(224) +plt.plot(t, Par.Tl * np.exp(-t), 'k', label='Tl') +plt.plot(t, x[:, 6], '-.k', label='Tl_h') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Disturbance torque (N.m)') +plt.legend() + +plt.tight_layout() +plt.show() diff --git a/8/DC_motor_LTR.py b/8/DC_motor_LTR.py new file mode 100644 index 0000000..4372e0e --- /dev/null +++ b/8/DC_motor_LTR.py @@ -0,0 +1,27 @@ +import numpy as np +from scipy.signal import place_poles +from control.matlab import * + +# System matrices +A = np.array([[0, 1, 0, 0], + [0, 0, 4.438, -7.396], + [0, -12, -24, 0], + [0, 0, 0, -1]]) + +b = np.array([[0], [0], [20], [0]]) +c = np.array([[1, 0, 0, 0]]) + +# LQR Design +Q1 = np.diag([9, 0, 0, 0]) +R = 1.0 +K, S, E = lqr(A, b, Q1, R) + +# LQE Design +pd = [-5-5j, -5+5j, -7+7j, -7-7j] +G = place_poles(A.T, c.T, pd).gain_matrix.T + +# Display results +print("LQR gain matrix K:") +print(K) +print("\nLQE gain matrix G:") +print(G) diff --git a/8/DC_motor_LTR1.py b/8/DC_motor_LTR1.py new file mode 100644 index 0000000..0f7b38d --- /dev/null +++ b/8/DC_motor_LTR1.py @@ -0,0 +1,79 @@ +import numpy as np +from scipy.integrate import odeint + + +# Global parameters +class Par: + Tl = 0.01 # Disturbance torque parameter + + +# Function defining the dynamics of the DC motor and observer +def DC_motor_LTR1(t, X): + global Par + + # System matrices + A = np.array([[0, 1, 0], + [0, 0, 4.438], + [0, -12, -24]]) + + B = np.array([[0, 0], + [0, -7.396], + [20, 0]]) + + C = np.array([[1, 0, 0]]) + + # Observer matrices + Ah = np.array([[0, 1, 0, 0], + [0, 0, 4.438, -7.396], + [0, -12, -24, 0], + [0, 0, 0, -1]]) + + Bh = np.array([[0], [0], [20], [0]]) + + Ch = np.array([[1, 0, 0, 0]]) + + # State feedback and observer gains + k = np.array([3.0000, 0.8796, 0.1529, -1.8190]) + G = np.array([[-1.0000], [235.7440], [-978.1707], [-20.4870]]) + + # Extract states from input vector X + x = X[:3] # [θ, ω, i] + xh = X[3:] # [θ, ω, i, Tl] + + # Desired angular position (not used in this example) + theta_d = 0 + + # Exponential disturbance torque + Tl = Par.Tl * np.exp(-t) + + # State feedback control law + v = -np.dot(k, xh) + + # Control input [v; Tl] + u = np.array([v, Tl]) + + # Observer update equation + xhp = np.dot(Ah, xh) + np.dot(Bh, v) + np.dot(G.T, (np.dot(C.T, x) - np.dot(Ch, xh))) + + # System dynamics + xp = np.dot(A, x) + np.dot(B, u) + + # Combined state derivatives + Xp = np.concatenate((xp, xhp)) + + return Xp + + +# Initial conditions and time span +X0 = np.array([0, 0, 0, 0, 0, 0, 0]) # Initial state [θ, ω, i, θ_hat, ω_hat, i_hat, Tl_hat] +t_span = np.linspace(0, 5, 500) # Time span for simulation + +# Solve the ODE system +sol = odeint(DC_motor_LTR1, X0, t_span) + +# Extract results +theta = sol[:, 0] +omega = sol[:, 1] +current = sol[:, 2] + +# Display or plot results as needed diff --git a/8/DC_motor_Obs.py b/8/DC_motor_Obs.py new file mode 100644 index 0000000..37885ec --- /dev/null +++ b/8/DC_motor_Obs.py @@ -0,0 +1,58 @@ +import numpy as np +from scipy.integrate import odeint + + +# Global parameter +class Par: + Tl = 0.01 # Step disturbance torque parameter + + +# Function defining the dynamics of the DC motor and observer +def DC_motor_Obs(t, X): + global Par + + # System matrices + A = np.array([[0, 1, 0], + [0, 0, 4.438], + [0, -12, -24]]) + + B = np.array([[0, 0], + [0, -7.396], + [20, 0]]) + + C = np.array([[1, 0, 0]]) + + # Observer matrices + Ah = np.array([[0, 1, 0, 0], + [0, 0, 4.438, -7.396], + [0, -12, -24, 0], + [0, 0, 0, 0]]) + + Bh = np.array([[0], [0], [20], [0]]) + + Ch = np.array([[1, 0, 0, 0]]) + + # Observer gain matrix + G = np.array([[0, 234.7440, -936.9136, -27.6050]]).T + + # Extract states from input vector X + x = X[:3] # [θ, ω, i] + xh = X[3:] # [θ_hat, ω_hat, i_hat, Tl_hat] + + # Step disturbance torque + Tl = Par.Tl + + # Control input [v; Tl] + v = 0 # Since v is not used in the observer model + + # State equations + xp = np.dot(A, x) + np.dot(B, np.array([v, Tl])) + y = np.dot(C, x) + + # Observer update equation + xhp = np.dot(Ah, xh) + np.dot(Bh, v) + np.dot(G, (y - np.dot(Ch, xh))) + + # Augment the real and estimated states + Xp = np.concatenate((xp, xhp)) + + return Xp diff --git a/8/Invpend_Luen_solver.py b/8/Invpend_Luen_solver.py new file mode 100644 index 0000000..33aaf84 --- /dev/null +++ b/8/Invpend_Luen_solver.py @@ -0,0 +1,90 @@ +import numpy as np +from scipy.integrate import odeint +import matplotlib.pyplot as plt + +# System parameters +M = 0.5 # Mass of the cart (kg) +m = 0.2 # Mass of the pendulum (kg) +b = 0.1 # Damping coefficient (N.s/m) +l = 0.3 # Length to the center of mass of the pendulum (m) +I = 0.006 # Inertia of the pendulum (kg.m^2) +g = 9.81 # Acceleration due to gravity (m/s^2) + +# State feedback and observer gains +K = np.array([0.1291, 0.1446, -3.6742, -2.2147]) +L = np.array([4.9672, 10.0391, 21.4149, 6.1251]) + +# State space matrices +A = np.array([[0, 1, 0, 0], + [0, -(I + m * l ** 2) * (b / (I * (M + m) + m * M * l ** 2)), + (m ** 2 * g * l ** 2) / (I * (M + m) + m * M * l ** 2), 0], + [0, 0, 0, 1], + [0, -(m * l * b) / (I * (M + m) + m * M * l ** 2), (m * g * l * (M + m)) / (I * (M + m) + m * M * l ** 2), + 0]]) + +B = np.array([[0], [(I + m * l ** 2) / (I * (M + m) + m * M * l ** 2)], [0], [m * l / (I * (M + m) + m * M * l ** 2)]]) + +C = np.array([[1, 0, 0, 0]]) + +# Luenberger observer matrix +A_observer = np.array([[0, 1, 0, 0], + [0, -(I + m * l ** 2) * (b / (I * (M + m) + m * M * l ** 2)), + (m ** 2 * g * l ** 2) / (I * (M + m) + m * M * l ** 2), 0], + [0, 0, 0, 1], + [0, -(m * l * b) / (I * (M + m) + m * M * l ** 2), + (m * g * l * (M + m)) / (I * (M + m) + m * M * l ** 2), 0]]) + +B_observer = np.array( + [[0], [(I + m * l ** 2) / (I * (M + m) + m * M * l ** 2)], [0], [m * l / (I * (M + m) + m * M * l ** 2)]]) + +L_observer = np.array([[4.9672], [10.0391], [21.4149], [6.1251]]) + + +# Function defining the dynamics of the inverted pendulum with Luenberger observer +def inverted_pendulum_Luenberger(X, t): + x = X[:4] # State variables [x, dx, theta, dtheta] + xh = X[4:] # Observer variables [xh, dxh, thetah, dthetah] + + # Control input + u = -np.dot(K, xh) + + # System dynamics + dx = np.dot(A, x) + B * u + + # Observer dynamics + dxh = np.dot(A_observer, xh) + np.dot(B_observer, u) + np.dot(L_observer, (x - xh)) + + # Combine state and observer derivatives + dX = np.concatenate((dx, dxh)) + + return dX + + +# Initial conditions +x0 = np.array([0, 0, 0.26, 0]) # Initial state [x, dx, theta, dtheta] +xh0 = np.array([0, 0, 0, 0]) # Initial observer state [xh, dxh, thetah, dthetah] +X0 = np.concatenate((x0, xh0)) # Initial total state vector + +# Time span for simulation +t_span = np.linspace(0, 3, 300) # 300 points from 0 to 3 seconds + +# Numerically integrate the system +X = odeint(inverted_pendulum_Luenberger, X0, t_span) + +# Extract states from the simulation results +x_cart = X[:, 0] +theta_pendulum = X[:, 2] +theta_observer = X[:, 6] + +# Plotting the results +plt.figure(figsize=(10, 6)) + +plt.plot(t_span, theta_pendulum, 'k', label='Pendulum') +plt.plot(t_span, theta_observer, '-.k', label='Observer') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Angular Position (rad)') +plt.legend() +plt.title('Inverted Pendulum: Angular Position') + +plt.show() diff --git a/8/ex7_1.py b/8/ex7_1.py new file mode 100644 index 0000000..252b9fd --- /dev/null +++ b/8/ex7_1.py @@ -0,0 +1,22 @@ +import numpy as np +from scipy.linalg import solve_continuous_are + +# System matrices +A = np.array([[0, 1, 0, 0], + [0, 0, 4.438, -7.396], + [0, -12, -24, 0], + [0, 0, 0, 0]]) + +C = np.array([[1, 0, 0, 0]]) + +# Desired closed-loop eigenvalues +pd = np.array([-5 + 5j, -5 - 5j, -7 + 7j, -7 - 7j]) + +# Compute the optimal gain matrix G using pole placement +G = solve_continuous_are(A.T, C.T, np.eye(4), np.diag(pd.real**2 + pd.imag**2)) + +# Transpose G to match MATLAB's convention (optional step) +G = G.T + +print("Optimal gain matrix G:") +print(G) diff --git a/8/inverted_pendulum_Luenburger.py b/8/inverted_pendulum_Luenburger.py new file mode 100644 index 0000000..36311a8 --- /dev/null +++ b/8/inverted_pendulum_Luenburger.py @@ -0,0 +1,36 @@ +import numpy as np + + +def inverted_pendulum_luenburger(t, X): + # State variable x=[x; v; theta; omega] + x = X[0:4] + psi = X[4] + + g = 9.8 + l = 1 + m = 1 + M = 1 + + d1 = M + m * (1 - np.cos(x[2]) ** 2) + d2 = l * d1 + + k = np.array([-40.0000, -37.3693, -190.6669, -54.7283]) + + dpsi = -40.0 * x[0] - 37.37 * x[1] - 405.9 * x[2] - 58.73 * psi + omega_h = psi + 4 * x[3] + xh = np.array([x[0:3], omega_h]) + + F = -np.dot(k, x) # State feedback + # F = -np.dot(k, xh) # Luenberger Observer Feedback + + xp = np.array([ + x[1], + (F + m * l * x[3] ** 2 * np.sin(x[2]) - m * g * np.sin(x[2]) * np.cos(x[2])) / d1, + x[3], + (-F * np.cos(x[2]) - m * l * x[3] ** 2 * np.sin(x[2]) * np.cos(x[2]) + + (M + m) * g * np.sin(x[2])) / d2 + ]) + + Xp = np.concatenate((xp, [dpsi])) + + return Xp diff --git a/8/train_lqe.py b/8/train_lqe.py new file mode 100644 index 0000000..0e06217 --- /dev/null +++ b/8/train_lqe.py @@ -0,0 +1,31 @@ +import numpy as np +import control as ctrl + +# Define the system matrices +A = np.array([ + [0, 0, 0, 0, 1, -1, 0, 0, 0], + [0, 0, 0, 0, 0, 1, -1, 0, 0], + [0, 0, 0, 0, 0, 0, 1, -1, 0], + [0, 0, 0, 0, 0, 0, 0, 1, -1], + [-12.5, 0, 0, 0, -0.75, 0.75, 0, 0, 0], + [62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0, 0], + [0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0], + [0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75], + [0, 0, 0, 62.5, 0, 0, 0, 3.75, -3.75] +]) + +C = np.array([ + [1, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 1, 0, 0, 0, 0] +]) + +# Define the weight matrices for the LQR observer design +W = np.diag([0, 0, 0, 0, 9, 0, 0, 0, 0]) +V = np.diag([1e-2, 1]) + +# Calculate the LQR observer gain +G, _, _ = ctrl.lqr(A.T, C.T, W, V) + +# Print the observer gain matrix +print("LQR Observer Gain (G):") +print(G) diff --git a/8/train_model_Obs.py b/8/train_model_Obs.py new file mode 100644 index 0000000..24b53cf --- /dev/null +++ b/8/train_model_Obs.py @@ -0,0 +1,90 @@ +import numpy as np +from scipy.integrate import odeint + + +# Define the train model function +def train_model1(X, t): + global Par + + # Parameters and variables + x = X[:10] + xh = X[10:19] + + # System matrices + A = np.array([ + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1, -1, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 1, -1, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 1, -1, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 1, -1], + [0, -12.5, 0, 0, 0, -0.75, 0.75, 0, 0, 0], + [0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0, 0], + [0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0], + [0, 0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75], + [0, 0, 0, 0, 62.5, 0, 0, 0, 3.75, -3.75] + ]) + + b1 = np.array([0, 0, 0, 0, 0, 0.005, 0, 0, 0, 0]) # Force input + b2 = np.array([0, 0, 0, 0, 0, 250, 0, 0, 0, -1250]) # Constant input + + # Control law + if t < 10: + u = Par.F # Constant Force + uh = 0.5 * u + else: + u = 0 + uh = u + + xp = np.dot(A, x) + np.dot(b1, u) + np.dot(b2, u) + + # Output matrix + C = np.array([ + [0, 1, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0] + ]) + y = np.dot(C, x) + dy = np.array([y[0] - 20, y[1]]) + + # Observer matrices + Ah = A.copy() + Bh = np.array([0, 0, 0, 0, 0.005, 0, 0, 0, 0]) + Ch = np.array([ + [1, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 1, 0, 0, 0, 0] + ]) + + # Observer gain matrix G + G = np.array([ + [10.5008, 0.0472], + [4.0624, 0.0100], + [1.2245, 0.0004], + [0.3222, -0.0007], + [118.1098, 1.1441], + [60.1867, 0.5240], + [16.7939, 0.3003], + [-0.0227, 0.2370], + [-4.2587, 0.2213] + ]) + + # Observer dynamics + yh = np.dot(Ch, xh) + xhp = np.dot(Ah, xh) + np.dot(Bh, uh) + np.dot(G, (dy - yh)) + + # Augment the real and estimated states + Xp = np.concatenate((xp, xhp)) + + return Xp + + +# Define initial conditions and time span +X0 = np.zeros(19) # Initial state vector [x, xh] +t_span = np.linspace(0, 20, 1000) # Time span for integration + +# Perform integration using odeint +X = odeint(train_model1, X0, t_span) + +# Print or plot results as desired +# Example: Print the final state +final_state = X[-1] +print("Final state vector:") +print(final_state) diff --git a/8/train_obs_solver1.py b/8/train_obs_solver1.py new file mode 100644 index 0000000..8009ad4 --- /dev/null +++ b/8/train_obs_solver1.py @@ -0,0 +1,115 @@ +import numpy as np +from scipy.integrate import odeint +import matplotlib.pyplot as plt + + +# Define the train model with observer function +def train_model_Obs(X, t): + global Par + + # Parameters and variables + x = X[:10] + xh = X[10:19] + + # System matrices + A = np.array([ + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1, -1, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 1, -1, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 1, -1, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 1, -1], + [0, -12.5, 0, 0, 0, -0.75, 0.75, 0, 0, 0], + [0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0, 0], + [0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0], + [0, 0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75], + [0, 0, 0, 0, 62.5, 0, 0, 0, 3.75, -3.75] + ]) + + b1 = np.array([0, 0, 0, 0, 0, 0.005, 0, 0, 0, 0]) # Force input + b2 = np.array([0, 0, 0, 0, 0, 250, 0, 0, 0, -1250]) # Constant input + + # Control law + if t < 10: + u = Par.F # Constant Force + uh = 0.5 * u + else: + u = 0 + uh = u + + xp = np.dot(A, x) + np.dot(b1, u) + np.dot(b2, u) + + # Output matrix + C = np.array([ + [0, 1, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0] + ]) + y = np.dot(C, x) + dy = np.array([y[0] - 20, y[1]]) + + # Observer matrices + Ah = A.copy() + Bh = np.array([0, 0, 0, 0, 0.005, 0, 0, 0, 0]) + Ch = np.array([ + [1, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 1, 0, 0, 0, 0] + ]) + + # Observer gain matrix G + G = np.array([ + [10.5008, 0.0472], + [4.0624, 0.0100], + [1.2245, 0.0004], + [0.3222, -0.0007], + [118.1098, 1.1441], + [60.1867, 0.5240], + [16.7939, 0.3003], + [-0.0227, 0.2370], + [-4.2587, 0.2213] + ]) + + # Observer dynamics + yh = np.dot(Ch, xh) + xhp = np.dot(Ah, xh) + np.dot(Bh, uh) + np.dot(G, (dy - yh)) + + # Augment the real and estimated states + Xp = np.concatenate((xp, xhp)) + + return Xp + + +# Set initial conditions and parameters +Par = {'F': 1000} # Initial Force +x0 = np.array([0, 20, 20, 20, 20, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0]) # Initial state vector [x, xh] +t_span = np.linspace(0, 20, 1000) # Time span for integration + +# Perform integration using odeint +X = odeint(train_model_Obs, x0, t_span) + +# Extract real and estimated states +x_real = X[:, :10] +x_estimated = X[:, 10:19] + +# Plotting results +plt.figure(figsize=(10, 8)) + +# Plot real and estimated positions +plt.subplot(211) +plt.plot(t_span, x_real[:, 1] - 20, 'k', label='Real x_2') +plt.plot(t_span, x_estimated[:, 0], 'k--', label='Estimated x_2') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Position (x_2)') +plt.legend() + +# Plot real and estimated velocities +plt.subplot(212) +plt.plot(t_span, x_real[:, 5], 'k', label='Real v_1') +plt.plot(t_span, x_estimated[:, 4], 'k--', label='Estimated v_1') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Velocity (v_1)') +plt.legend() + +plt.tight_layout() +plt.show() diff --git a/8/train_solver.py b/8/train_solver.py new file mode 100644 index 0000000..eac8cf2 --- /dev/null +++ b/8/train_solver.py @@ -0,0 +1,70 @@ +import numpy as np +from scipy.integrate import odeint +import matplotlib.pyplot as plt + + +# Define the train model function +def train_model(X, t): + # Parameters and variables + x = X[:10] + + # System matrices + A = np.array([ + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1, -1, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 1, -1, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 1, -1, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 1, -1], + [0, -12.5, 0, 0, 0, -0.75, 0.75, 0, 0, 0], + [0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0, 0], + [0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75, 0], + [0, 0, 0, 62.5, -62.5, 0, 0, 3.75, -7.5, 3.75], + [0, 0, 0, 0, 62.5, 0, 0, 0, 3.75, -3.75] + ]) + + b1 = np.array([0, 0, 0, 0, 0, 0.005, 0, 0, 0, 0]) # Force input + b2 = np.array([0, 0, 0, 0, 0, 250, 0, 0, 0, -1250]) # Constant input + + # Control law + if t < 10: + u = 1000 # Constant Force (adjust as needed) + else: + u = 0 + + xp = np.dot(A, x) + np.dot(b1, u) + np.dot(b2, u) + + # Augment the real and estimated states + Xp = xp + + return Xp + + +# Set initial conditions and parameters +x0 = np.array([0, 20, 20, 20, 20, 0, 0, 0, 0, 0]) # Initial state vector +t_span = np.linspace(0, 100, 1000) # Time span for integration + +# Perform integration using odeint +X = odeint(train_model, x0, t_span) + +# Plotting results +plt.figure(figsize=(10, 8)) + +# Plot locomotive position +plt.subplot(211) +plt.plot(t_span, X[:, 0], 'k', label='x_1') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Locomotive Position (m)') +plt.legend() + +# Plot wagons distance +plt.subplot(212) +plt.plot(t_span, X[:, 1], 'k', label='x_2') +plt.plot(t_span, X[:, 4], 'k-.', label='x_5') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Wagons Distance (m)') +plt.legend() + +plt.tight_layout() +plt.show()