From 316e142e017baf9798880349ecfdd12ba3f30d55 Mon Sep 17 00:00:00 2001 From: Elnaz MohseniNezhad Date: Thu, 23 May 2024 01:06:30 +0330 Subject: [PATCH 1/4] chapter3pyelnaz --- Chapter3/done.txt | 8 ++ Chapter3/python/Active_s.py | 55 ++++++++++++++ Chapter3/python/DCmotor.py | 94 +++++++++++++++++++++++ Chapter3/python/DCmotor_transfun.py | 25 ++++++ Chapter3/python/Invpend_solver.py | 63 +++++++++++++++ Chapter3/python/inverted_pendulum.py | 25 ++++++ Chapter3/python/robot_model.py | 36 +++++++++ Chapter3/python/robot_solver.py | 63 +++++++++++++++ Chapter3/python/tank_model.py | 36 +++++++++ Chapter3/python/tank_solver.py | 37 +++++++++ Chapter3/python/train.py | 78 +++++++++++++++++++ Chapter3/python/train_linear.py | 95 +++++++++++++++++++++++ Chapter3/train.m | 17 ++--- Chapter3/train_linear.m | 110 +++++++++++++-------------- desktop.ini | 2 + 15 files changed, 680 insertions(+), 64 deletions(-) create mode 100644 Chapter3/done.txt create mode 100644 Chapter3/python/Active_s.py create mode 100644 Chapter3/python/DCmotor.py create mode 100644 Chapter3/python/DCmotor_transfun.py create mode 100644 Chapter3/python/Invpend_solver.py create mode 100644 Chapter3/python/inverted_pendulum.py create mode 100644 Chapter3/python/robot_model.py create mode 100644 Chapter3/python/robot_solver.py create mode 100644 Chapter3/python/tank_model.py create mode 100644 Chapter3/python/tank_solver.py create mode 100644 Chapter3/python/train.py create mode 100644 Chapter3/python/train_linear.py create mode 100644 desktop.ini diff --git a/Chapter3/done.txt b/Chapter3/done.txt new file mode 100644 index 0000000..c1164d0 --- /dev/null +++ b/Chapter3/done.txt @@ -0,0 +1,8 @@ +dc_motor done +Active_sp done +train_linear done +train done +tank_solver done +tank model done +robot model done +robot solver done diff --git a/Chapter3/python/Active_s.py b/Chapter3/python/Active_s.py new file mode 100644 index 0000000..37949c2 --- /dev/null +++ b/Chapter3/python/Active_s.py @@ -0,0 +1,55 @@ +import numpy as np +import control as ctrl +import matplotlib.pyplot as plt + +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 = ctrl.ss(A, b2, C, D) + +# Time vector +t = np.arange(0, 7, 0.01) +N = len(t) + + +# Initial state +x0 = np.array([[0.2], [0], [0], [0]]) + +# Simulate initial response +response = ctrl.forced_response(active_suspension, T=t, X0=x0) + + +# Plot state variables x1 and x2 +plt.plot(t, response.states[0], 'k', label='x1') +plt.plot(t, response.states[1], 'k-.', label='x2') +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.grid(True) +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 response of the system +# Simulate the response of the system with the provided input signal +response = ctrl.forced_response(active_suspension, T=t, U=u) + +import matplotlib.pyplot as plt + +# Plot state variables x1 and x2 +plt.plot(t, response.states[0], 'k', label='x1') +plt.plot(t, response.states[1], 'k-.', label='x2') +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.grid(True) +plt.legend() +plt.show() diff --git a/Chapter3/python/DCmotor.py b/Chapter3/python/DCmotor.py new file mode 100644 index 0000000..e267102 --- /dev/null +++ b/Chapter3/python/DCmotor.py @@ -0,0 +1,94 @@ +import numpy as np +import control as ctrl +import matplotlib.pyplot as plt + +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, 0, 0], + # [0, 0, 0]]) +D = np.zeros((2, 1)) +# Create state-space system +DC_motor = ctrl.ss(A, b1, C, D) + +# Simulation +t = np.arange(0, 4, 0.01) +t, yout = ctrl.step_response(DC_motor, T=t) + + +# t = np.arange(0, 4, 0.01) # Create the time array +# N = np.size(t) # Get the number of elements in the array t +# # Or equivalently +# N = len(t) # Get the number of elements in the array t + + + +# Assuming t is already defined +N = len(t) +u = np.zeros((2, N)) # Assuming u has 2 rows (inputs) and N columns (time points) + +for i in range(N): + if t[i] < 2: + u[:, i] = 3 # Set all elements in column i to 3 + else: + u[:, i] = -3 # Set all elements in column i to -3 + + + + + +# Generate a square wave signal +# t = np.arange(0, 4, 0.01) # Time vector from 0 to 4 with 0.01 step +u = -6 * np.sign(np.sin(2 * np.pi * 0.25 * t)) + 3 # Square wave with period 4 seconds, scaled and offset + +# Simulate the response of the system +# _, y, _ = ctrl.forced_response(DC_motor, T=t, U=u) +# y contains the system's output, t contains the time vector, and x contains the states of the system +response = ctrl.forced_response(DC_motor, T=t, U=u) + +# Inspect the response +print(response) + + +# print("Time array:") +# print(t) +# print("\nOutput array:") +# print(yout) + + + +# plt.plot(t, response.states[2], 'k', label=r'$\theta$') +# plt.plot(t, response.states[1], 'k-.', label=r'$\omega$') +# plt.plot(t, response.states[0], 'k:', label='i') +# plt.xlabel('Time (sec)') +# plt.ylabel('State variables') +# plt.grid(True) +# plt.legend() +# plt.show() + +plt.plot(t, -response.states[2], 'k', label='i') +plt.plot(t, -response.states[1], 'k-.', label=r'$\omega$') +plt.plot(t, -response.states[0], 'k:', label=r'$\theta$') +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.grid(True) +plt.legend() +plt.show() +############################################# +# no need to use the DCmotor_transfun function + + diff --git a/Chapter3/python/DCmotor_transfun.py b/Chapter3/python/DCmotor_transfun.py new file mode 100644 index 0000000..dc08a3a --- /dev/null +++ b/Chapter3/python/DCmotor_transfun.py @@ -0,0 +1,25 @@ +import numpy as np +from scipy import signal + +# 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]]) +D = np.array([[0, 0]]) + +# Convert to transfer function +num, den = signal.ss2tf(A, B, C, D) +DCM_tf = signal.TransferFunction(num[0, :], den[0]) + +# Convert to zero-pole-gain form +DCM_zpk = signal.ss2zpk(A, B, C, D) + +# Print transfer function +print("Transfer Function:") +print(DCM_tf) + +# Print zero-pole-gain form +print("\nZero-Pole-Gain Form:") +print(DCM_zpk) diff --git a/Chapter3/python/Invpend_solver.py b/Chapter3/python/Invpend_solver.py new file mode 100644 index 0000000..441a692 --- /dev/null +++ b/Chapter3/python/Invpend_solver.py @@ -0,0 +1,63 @@ +import numpy as np + +tspan = np.array([0, 1]) +x0 = np.array([0, 0.1, 0, 0]) + +import numpy as np + +def inverted_pendulum(x, t): + # Parameters + g = 9.8 + l = 1 + m = 1 + M = 1 + + # Compute intermediate terms + d1 = M + m * (1 - np.cos(x[1])**2) + d2 = l * d1 + + # Input force + F = 0 # No input + + # Compute time derivatives of state variables + xp = np.array([ + x[2], # x' + x[3], # theta' + (F + m * l * x[3]**2 * np.sin(x[1]) - m * g * np.sin(x[1]) * np.cos(x[1])) / d1, # v' + (-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 # omega' + ]) + + return xp + +from scipy.integrate import odeint + +# Define the time points at which you want the solution +t_eval = np.linspace(tspan[0], tspan[1], 100) # Adjust the number of points as needed + +# Solve the ODE using odeint +x = odeint(inverted_pendulum, x0, t_eval) + + + +import matplotlib.pyplot as plt + +# Plot the first and second columns of x against time +plt.plot(t_eval, x[:, 0], 'k', label='x (m)') +plt.plot(t_eval, x[:, 1], '-.k', label=r'$\theta$ (rad)') + +# Add gridlines +plt.grid(True) + +# Set labels for x-axis and y-axis +plt.xlabel('Time (sec)') +plt.ylabel('State Variables') + +# Add legend +plt.legend() + +# Set linewidth for all lines in the plot +plt.setp(plt.gca().lines, linewidth=2) + +# Show the plot +plt.show() diff --git a/Chapter3/python/inverted_pendulum.py b/Chapter3/python/inverted_pendulum.py new file mode 100644 index 0000000..d35045c --- /dev/null +++ b/Chapter3/python/inverted_pendulum.py @@ -0,0 +1,25 @@ + +def inverted_pendulum(x, t): + # Parameters + g = 9.8 + l = 1 + m = 1 + M = 1 + + # Compute intermediate terms + d1 = M + m * (1 - np.cos(x[1])**2) + d2 = l * d1 + + # Input force + F = 0 # No input + + # Compute time derivatives of state variables + xp = np.array([ + x[2], # x' + x[3], # theta' + (F + m * l * x[3]**2 * np.sin(x[1]) - m * g * np.sin(x[1]) * np.cos(x[1])) / d1, # v' + (-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 # omega' + ]) + + return xp \ No newline at end of file diff --git a/Chapter3/python/robot_model.py b/Chapter3/python/robot_model.py new file mode 100644 index 0000000..7ca68fb --- /dev/null +++ b/Chapter3/python/robot_model.py @@ -0,0 +1,36 @@ +import numpy as np + +def robot_model(t, x): + # Parameters + g = 9.81 + l1 = 1 + l2 = 0.5 + m1 = 2 + m2 = 1 + I1 = 1e-2 + I2 = 5e-3 + D = 2 + + # Mass matrix M + M = np.array([[m1*(l1/2)**2 + m2*(l1**2 + (l2/2)**2) + m2*l1*l2*np.cos(x[1]) + I1 + I2, + m2*(l2/2)**2 + 0.5*m2*l1*l2*np.cos(x[1]) + I2], + [m2*(l2/2)**2 + 0.5*m2*l1*l2*np.cos(x[1]) + I2, + m2*(l2/2)**2 + I2]]) + + # Coriolis and centrifugal force vector V + V = np.array([-m2*l1*l2*np.sin(x[1])*x[2]*x[3] - 0.5*m2*l1*l2*np.sin(x[1])*x[3]**2, + -0.5*m2*l1*l2*np.sin(x[1])*x[2]*x[3]]) + + # Gravitational force vector G + G = np.array([(m1*l1/2 + m2*l1)*g*np.cos(x[0]) + m2*g*l2/2*np.cos(x[0] + x[1]), + m2*g*l2/2*np.cos(x[0] + x[1])]) + + # Input vector Q + Q = np.array([0, 0]) # No input + Q -= D * np.array([x[2], x[3]]) + + # Compute acceleration + xy = np.linalg.pinv(M).dot(Q - V - G) + + # Return time derivatives of state variables + return np.array([x[2], x[3], xy[0], xy[1]]) diff --git a/Chapter3/python/robot_solver.py b/Chapter3/python/robot_solver.py new file mode 100644 index 0000000..8d43aa2 --- /dev/null +++ b/Chapter3/python/robot_solver.py @@ -0,0 +1,63 @@ +import numpy as np +from scipy.integrate import odeint +import matplotlib.pyplot as plt + +# Define the robot model differential equation +def robot_model(x, t): + # Parameters + g = 9.81 + l1 = 1 + l2 = 0.5 + m1 = 2 + m2 = 1 + I1 = 1e-2 + I2 = 5e-3 + D = 2 + + # Mass matrix M + M = np.array([[m1*(l1/2)**2 + m2*(l1**2 + (l2/2)**2) + m2*l1*l2*np.cos(x[1]) + I1 + I2, + m2*(l2/2)**2 + 0.5*m2*l1*l2*np.cos(x[1]) + I2], + [m2*(l2/2)**2 + 0.5*m2*l1*l2*np.cos(x[1]) + I2, + m2*(l2/2)**2 + I2]]) + + # Coriolis and centrifugal force vector V + V = np.array([-m2*l1*l2*np.sin(x[1])*x[2]*x[3] - 0.5*m2*l1*l2*np.sin(x[1])*x[3]**2, + -0.5*m2*l1*l2*np.sin(x[1])*x[2]*x[3]]) + + # Gravitational force vector G + G = np.array([(m1*l1/2 + m2*l1)*g*np.cos(x[0]) + m2*g*l2/2*np.cos(x[0] + x[1]), + m2*g*l2/2*np.cos(x[0] + x[1])]) + + # Input vector Q + # Q = np.array([0, 0]) # No input + # Q -= D * np.array([x[2], x[3]]) + # Input vector Q + Q = np.array([0.0, 0.0]) # No input, ensure floats + Q -= D * np.array([x[2], x[3]], dtype=float) # Ensure floats + + + + # Compute acceleration + xy = np.linalg.pinv(M).dot(Q - V - G) + + # Return time derivatives of state variables + return np.array([x[2], x[3], xy[0], xy[1]]) + +# Define the time span +tspan = np.linspace(0, 5, 1000) + +# Define the initial condition +x0 = [-np.pi/3, np.pi/3, 0, 0] + +# Solve the ODE +x = odeint(robot_model, x0, tspan) + +# Plot the joint angles against time +plt.plot(tspan, np.degrees(x[:, 0]), 'k', label=r'$\theta_1$ (degrees)') +plt.plot(tspan, np.degrees(x[:, 1]), '-.k', label=r'$\theta_2$ (degrees)') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State Variables') +plt.legend() +plt.setp(plt.gca().lines, linewidth=2) # Set the linewidth of all lines in the current plot +plt.show() diff --git a/Chapter3/python/tank_model.py b/Chapter3/python/tank_model.py new file mode 100644 index 0000000..9cdb15f --- /dev/null +++ b/Chapter3/python/tank_model.py @@ -0,0 +1,36 @@ +import numpy as np + +def tank_model(t, x): + """ + Model of the tank. + + Parameters: + t : float + Time variable. + x : float + State variable representing the tank level. + + Returns: + xp : float + Derivative of the tank level with respect to time. + """ + # Parameters + A = 1.0 # Cross-sectional area of the tank + C = 2.0 # Coefficient related to the valve and tank properties + F_in = 0.0 # No disturbance input + u = 0.1 # Constant opening for valve + + # Tank model differential equation + xp = 1/A * (F_in - C * u * np.sqrt(x)) + + return xp +######################################## +def tank_model(x, t): + # Parameters + k1 = 0.1 + k2 = 0.2 + + # System of differential equations + dxdt = -k1*x + k2 + + return dxdt \ No newline at end of file diff --git a/Chapter3/python/tank_solver.py b/Chapter3/python/tank_solver.py new file mode 100644 index 0000000..778ee77 --- /dev/null +++ b/Chapter3/python/tank_solver.py @@ -0,0 +1,37 @@ +import matplotlib.pyplot as plt + +plt.clf() +print("tspan=[0, 100]") +print("x0=[100]") +import numpy as np +from scipy.integrate import odeint + +# Define the tank model differential equation +def tank_model(x, t): + # Parameters + k1 = 0.1 + k2 = 0.2 + + # System of differential equations + dxdt = -k1*x + k2 + + return dxdt + +# Define the time span +tspan = np.linspace(0, 100, 1000) # Adjust the number of time points as needed + +# Define the initial condition +x0 = [100] + +# Solve the ODE +x = odeint(tank_model, x0, tspan) + +import matplotlib.pyplot as plt + +# Plot the tank level against time +plt.plot(tspan, x[:, 0], 'k') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Tank Level (m)') +plt.setp(plt.gca().lines, linewidth=2) # Set the linewidth of all lines in the current plot +plt.show() diff --git a/Chapter3/python/train.py b/Chapter3/python/train.py new file mode 100644 index 0000000..445bb6e --- /dev/null +++ b/Chapter3/python/train.py @@ -0,0 +1,78 @@ +import numpy as np +from scipy import signal +import matplotlib.pyplot as plt + +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]) # Force input +b2 = np.array([0, 0, 0, 0, 250, 0, 0, 0, 0, -1250]) # constant input +B = np.column_stack((b1, b2)) +C = np.array([1, 0, 0, 0, 0, 0, 0, 0, 0, 0]) +D = 0 +train_model = signal.StateSpace(A, B[:, 0:1], C, D) + + + +t = np.arange(0, 7.01, 0.01) +N = len(t) + + + + +# Define the initial state vector x0 +x0 = np.array([20, 20, 20, 20, 20, 0, 0, 0, 0, 0]) + +# Simulate the response +t, y, x = signal.lsim(train_model, None, t, X0=x0) + + +# Plot x1 and x5 against time t +plt.plot(t, x[:, 0], 'k', label='x1') +plt.plot(t, x[:, 4], 'k-.', label='x5') +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.grid(True) +plt.legend() +plt.setp(plt.gca().lines, linewidth=2) # Set the linewidth of all lines in the current plot +plt.show() + + + + +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 response +t, y, x = signal.lsim(train_model, U=u, T=t) + +import matplotlib.pyplot as plt + +# Create a new figure +plt.figure() + +# Plot x1 and x2 against time t +plt.plot(t, x[:, 0], 'k', label='x1') +plt.plot(t, x[:, 1], 'k-.', label='x2') + +# Customize the plot +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.grid(True) +plt.legend() + +# Increase line width +plt.setp(plt.gca().lines, linewidth=2) + +# Display the plot +plt.show() diff --git a/Chapter3/python/train_linear.py b/Chapter3/python/train_linear.py new file mode 100644 index 0000000..b6d0d84 --- /dev/null +++ b/Chapter3/python/train_linear.py @@ -0,0 +1,95 @@ +import numpy as np +from scipy.signal import StateSpace, lsim +import matplotlib.pyplot as plt + +# Define the 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]) +b2 = np.array([0, 0, 0, 0, 0, 250, 0, 0, 0, -1250]) +C = np.array([1, 0, 0, 0, 0, 0, 0, 0, 0, 0]) +D = 0 + +# Combine b1 and b2 to form b +b = (b1[:, np.newaxis] * 750) + b2[:, np.newaxis] + +# Create the StateSpace object +train_model = StateSpace(A, b, C, D) + +# Time vector for simulation +t = np.linspace(0, 7, 1000) + +# Initial state +x0 = np.zeros(10) + +# Step input signal +u = np.ones(len(t)) * 750 + +# Simulate the response to the step input signal +t, y, x = lsim(train_model, u, t, X0=x0) + +# Plotting the step response +plt.figure(1) +plt.plot(t, x[:, 1], 'k', t, x[:, 4], 'k-.') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.legend(['x_2', 'x_5']) +plt.show() +#############################################3 +import numpy as np +from scipy.signal import StateSpace, lsim +import matplotlib.pyplot as plt + +# Define the 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]) +b2 = np.array([0, 0, 0, 0, 0, 250, 0, 0, 0, -1250]) +C = np.array([1, 0, 0, 0, 0, 0, 0, 0, 0, 0]) +D = 0 + +# Combine b1 and b2 to form b +b = (b1[:, np.newaxis] * 750) + b2[:, np.newaxis] + +# Create the StateSpace object +train_model = StateSpace(A, b, C, D) + +# Time vector for simulation +t = np.linspace(0, 7, 1000) + +# Initial state +x0 = np.zeros(10) + +# Define the input signal +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 response to the sinusoidal input signal +t, y, x = lsim(train_model, u, t, X0=x0) + +# Plotting the lsim response +plt.figure(2) +plt.plot(t, x[:, 0], 'k', t, x[:, 1], 'k-.') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.legend(['x_1', 'x_2']) +plt.show() diff --git a/Chapter3/train.m b/Chapter3/train.m index 49978eb..5dfd277 100644 --- a/Chapter3/train.m +++ b/Chapter3/train.m @@ -26,13 +26,15 @@ % Initial response % x0=[20;20;20;20;20;0;0;0;0;0]; -[y,t,x]=initial(train_model, x0, t) +[y,t,x]=initial(train_model, x0, t); + + plot(t,x(:,1),'k',t,x(:,5),'k-.'), grid set(findall(figure(1),'type','line'),'linewidth',2) xlabel('Time (sec)') ylabel('State variables') legend('x_1', 'x_5') -break + % % A Simple way to generate input u % @@ -40,16 +42,13 @@ % % Simulate the system % -[y,t,x]=lsim(active_suspension,u,t); +[y,t,x]=lsim(train_model,u,t); % Changed 'active_suspension' to 'train_model' % Plot the result % --------------------------------------- -plot(t,x(:,1),'k',t,x(:,2),'k-.'), grid -set(findall(figure(1),'type','line'),'linewidth',2) +figure; % Create a new figure +plot(t,x(:,1),'k',t,x(:,2),'k-.'), grid % Plot x1 and x2 +set(findall(figure(2),'type','line'),'linewidth',2) % Increase line width xlabel('Time (sec)') ylabel('State variables') legend('x_1', 'x_2') - - - - \ No newline at end of file diff --git a/Chapter3/train_linear.m b/Chapter3/train_linear.m index a3bacca..de30406 100644 --- a/Chapter3/train_linear.m +++ b/Chapter3/train_linear.m @@ -1,56 +1,56 @@ -% -% Simulation of the train -% Copyright Hamid D. Taghirad 2013 -% clear all -% State variable x=[x1 x2 x3 x4 x5 v1 v2 v3 v4 v5]; -A=[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=[0; 0; 0; 0; 0; 0.005; 0; 0; 0; 0]; % Force input -b2=[0; 0; 0; 0; 0; 250; 0; 0; 0; -1250]; % constant input -C=[1 0 0 0 0 0 0 0 0 0]; -D=0; -u=750; % Constant input -b=b1*u+b2; -train_model=ss(A,b,C,D); % Note only Second input is used -t=0:0.001:7; -% -% forced response -% -x0=[0;0;0;0;0;0;0;0;0;0]; -[y,t,x]=step(train_model, t) -plot(t,x(:,2),'k',t,x(:,5),'k-.'), grid -set(findall(figure(1),'type','line'),'linewidth',2) -xlabel('Time (sec)') -ylabel('State variables') -legend('x_2', 'x_5') -break -% -% A Simple way to generate input u -% -u=0.1*(sin(5*t)+sin(9*t)+sin(13*t)+sin(17*t)+sin(21*t)); -% -% Simulate the system -% -[y,t,x]=lsim(active_suspension,u,t); - -% Plot the result -% --------------------------------------- -plot(t,x(:,1),'k',t,x(:,2),'k-.'), grid -set(findall(figure(1),'type','line'),'linewidth',2) -xlabel('Time (sec)') -ylabel('State variables') -legend('x_1', 'x_2') - - - - \ No newline at end of file + +% Define the system matrices +A = [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 = [0; 0; 0; 0; 0; 0.005; 0; 0; 0; 0]; +b2 = [0; 0; 0; 0; 0; 250; 0; 0; 0; -1250]; + +C = [1 0 0 0 0 0 0 0 0 0]; +D = 0; + +u = 750; +b = b1*u + b2; + +% Create the state-space model +train_model = ss(A, b, C, D); + +% Time vector for simulation +t = 0:0.001:7; + +% Initial state +x0 = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0]; + +% Step response +[y, t, x] = step(train_model, t); + +% Plotting the step response +figure(1); +plot(t, x(:,2), 'k', t, x(:,5), 'k-.'); grid on; +set(findall(figure(1), 'type', 'line'), 'linewidth', 2); +xlabel('Time (sec)'); +ylabel('State variables'); +legend('x_2', 'x_5'); + +% Define the new input signal +u = 0.1 * (sin(5*t) + sin(9*t) + sin(13*t) + sin(17*t) + sin(21*t)); + +% Simulate the response to the new input signal +[y, t, x] = lsim(train_model, u, t); + +% Plotting the lsim response +figure(2); +plot(t, x(:,1), 'k', t, x(:,2), 'k-.'); grid on; +set(findall(figure(2), 'type', 'line'), 'linewidth', 2); +xlabel('Time (sec)'); +ylabel('State variables'); +legend('x_1', 'x_2'); diff --git a/desktop.ini b/desktop.ini new file mode 100644 index 0000000..fae1239 --- /dev/null +++ b/desktop.ini @@ -0,0 +1,2 @@ +[.ShellClassInfo] +LocalizedResourceName=matlab From 76f8d52f1fcfb2a01c79df2bede98522a5b42ceb Mon Sep 17 00:00:00 2001 From: Elnaz MohseniNezhad Date: Tue, 4 Jun 2024 01:06:46 +0330 Subject: [PATCH 2/4] Chapter 4, 5 and 6 python codes added by Elnaz MohseniNezhad --- Chapter4/pythonch4/DCmotor_jordan.py | 39 +++++++++ Chapter4/pythonch4/ReadMe.md | 37 +++++++++ Chapter4/pythonch4/ex3_1.py | 29 +++++++ Chapter4/pythonch4/ex3_2.py | 44 ++++++++++ Chapter4/pythonch4/ex3_3.py | 14 ++++ Chapter4/pythonch4/ex3_8.py | 28 +++++++ Chapter4/pythonch4/jordan_forms.py | 118 +++++++++++++++++++++++++++ Chapter5_6/python_ch5_6/ex5_1.py | 97 ++++++++++++++++++++++ Chapter5_6/python_ch5_6/ex5_2.py | 28 +++++++ Chapter5_6/python_ch5_6/ex5_3.py | 48 +++++++++++ Chapter5_6/python_ch5_6/ex5_4.py | 60 ++++++++++++++ Chapter5_6/python_ch5_6/ex5_6.py | 10 +++ Chapter5_6/python_ch5_6/test.py | 52 ++++++++++++ 13 files changed, 604 insertions(+) create mode 100644 Chapter4/pythonch4/DCmotor_jordan.py create mode 100644 Chapter4/pythonch4/ReadMe.md create mode 100644 Chapter4/pythonch4/ex3_1.py create mode 100644 Chapter4/pythonch4/ex3_2.py create mode 100644 Chapter4/pythonch4/ex3_3.py create mode 100644 Chapter4/pythonch4/ex3_8.py create mode 100644 Chapter4/pythonch4/jordan_forms.py create mode 100644 Chapter5_6/python_ch5_6/ex5_1.py create mode 100644 Chapter5_6/python_ch5_6/ex5_2.py create mode 100644 Chapter5_6/python_ch5_6/ex5_3.py create mode 100644 Chapter5_6/python_ch5_6/ex5_4.py create mode 100644 Chapter5_6/python_ch5_6/ex5_6.py create mode 100644 Chapter5_6/python_ch5_6/test.py diff --git a/Chapter4/pythonch4/DCmotor_jordan.py b/Chapter4/pythonch4/DCmotor_jordan.py new file mode 100644 index 0000000..0549d7c --- /dev/null +++ b/Chapter4/pythonch4/DCmotor_jordan.py @@ -0,0 +1,39 @@ +import numpy as np +from scipy.linalg import eig +from scipy.signal import StateSpace + +# Define 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.zeros((2, 1)) # Ensure D is a 2x1 matrix to match the dimensions of C + +# Define state-space system (note only first input is used) +DC_motor = StateSpace(A, b1, C, D) + +# Calculate eigenvalues and eigenvectors +e, v = eig(A) +ee, w = eig(A.T) + +# Sort eigenvalues and corresponding eigenvectors to match MATLAB output +idx = np.argsort(e) +e = np.diag(e[idx]) +v = v[:, idx] + +idx_ee = np.argsort(ee) +ee = np.diag(ee[idx_ee]) +w = w[:, idx_ee] + +print("Eigenvalues and eigenvectors of A:") +print("Eigenvalues (e):") +print(e) +print("Eigenvectors (v):") +print(v) + +print("\nEigenvalues and eigenvectors of A^T:") +print("Eigenvalues (ee):") +print(ee) +print("Eigenvectors (w):") +print(w) diff --git a/Chapter4/pythonch4/ReadMe.md b/Chapter4/pythonch4/ReadMe.md new file mode 100644 index 0000000..93cd195 --- /dev/null +++ b/Chapter4/pythonch4/ReadMe.md @@ -0,0 +1,37 @@ +# Simulation Models in MATLAB + +This repository includes MATLAB scripts for simulating various mechanical and control systems. Detailed descriptions for each script are provided to help users understand their functions and applications. + +## File Descriptions + +### 📄 [DCmotor_jordan.m](./DCmotor_jordan.m) +- **Purpose**: Explores the Jordan form of the DC motor system matrix, useful for advanced control theory and analysis. + +### 📄 [ex3_1.m](./ex3_1.m) +- **Purpose**: Provides a MATLAB implementation of Example 4-3-8 from the "Modern Book", illustrating a specific control scenario or theory application. + +### 📄 [ex3_2.m](./ex3_2.m) +- **Purpose**: Demonstrates the concepts discussed in Example 3-2 of the "Modern Book", focusing on practical application in MATLAB. + +### 📄 [ex3_3.m](./ex3_3.m) +- **Purpose**: Implements Example 3-3 from the "Modern Book", designed to help users understand specific theoretical concepts in control systems. + +### 📄 [ex3_8.m](./ex3_8.m) +- **Purpose**: MATLAB implementation for Example 3-8 of the "Modern Book", aiding in the visualization and analysis of the example's concepts. + +### 📄 [jordan_forms.m](./jordan_forms.m) +- **Purpose**: This script calculates the Jordan forms for matrices provided in examples from a textbook, facilitating a deeper understanding of matrix theory and its application in control systems, specifically demonstrated with an inverted pendulum example. + +## Contribution +Fork this repository to contribute and propose changes or enhancements via pull requests. For major changes, please open an issue first to discuss your proposals. + +## License +Distributed under the MIT License. See `LICENSE` for more information. + +## Contact +- Prof. Hamid D. Taghirad – [Visit Website](https://aras.kntu.ac.ir/taghirad/) +- Project Link: [https://github.com/aras-labs/Modern_Control](https://github.com/aras-labs/Modern_Control) + +![GitHub forks](https://img.shields.io/github/forks/aras-labs/Modern_Control?style=social) +![GitHub stars](https://img.shields.io/github/stars/aras-labs/Modern_Control?style=social) +![GitHub watchers](https://img.shields.io/github/watchers/aras-labs/Modern_Control?style=social) diff --git a/Chapter4/pythonch4/ex3_1.py b/Chapter4/pythonch4/ex3_1.py new file mode 100644 index 0000000..d8a891f --- /dev/null +++ b/Chapter4/pythonch4/ex3_1.py @@ -0,0 +1,29 @@ +import numpy as np +from scipy.linalg import svd, null_space + +# Define matrices +A = np.array([[-3/2, 1/2], [1/2, -3/2]]) +C = np.array([[1, -1]]) + +# Observability matrix +def obsv(A, C): + n = A.shape[0] + O = C + for i in range(1, n): + O = np.vstack((O, np.dot(C, np.linalg.matrix_power(A, i)))) + return O + +O = obsv(A, C) + +# Rank of the observability matrix +rank_O = np.linalg.matrix_rank(O) + +# Null space of the observability matrix +null_O = null_space(O) + +print("Observability matrix (O):") +print(O) +print("\nRank of the observability matrix:") +print(rank_O) +print("\nNull space of the observability matrix:") +print(null_O) diff --git a/Chapter4/pythonch4/ex3_2.py b/Chapter4/pythonch4/ex3_2.py new file mode 100644 index 0000000..60c8fad --- /dev/null +++ b/Chapter4/pythonch4/ex3_2.py @@ -0,0 +1,44 @@ +import numpy as np +import sympy as sp + +# Define matrices and initial conditions +A = np.array([[1, 0], [1, 1]]) +B = np.array([[1], [1]]) +u = 1 +x0 = np.array([[1], [1]]) + +# Define symbolic variable t +t = sp.symbols('t') + +# Convert numpy arrays to sympy matrices +A_sym = sp.Matrix(A) +B_sym = sp.Matrix(B) +x0_sym = sp.Matrix(x0) + +# Compute the matrix exponential exp(A*t) symbolically using sympy +phi = sp.exp(A_sym * t) + +# Compute exp(-A*t) * B * u symbolically using sympy +exp_neg_A_t = sp.exp(-A_sym * t) +x1 = exp_neg_A_t * B_sym * u + +# Integrate x1 with respect to t symbolically +x_zs = sp.integrate(x1, t) + +# Compute phi * x0 symbolically +x_zi = phi * x0_sym + +# Compute the final result x symbolically +x = x_zi + x_zs + +# Print results in the desired format +print("phi (expm(A*t)):") +sp.pprint(phi) +print("\nx1 (expm(-A*t) * B * u):") +sp.pprint(x1) +print("\nx_zs (integral of x1):") +sp.pprint(x_zs) +print("\nx_zi (phi * x0):") +sp.pprint(x_zi) +print("\nx (x_zi + x_zs):") +sp.pprint(x) diff --git a/Chapter4/pythonch4/ex3_3.py b/Chapter4/pythonch4/ex3_3.py new file mode 100644 index 0000000..5edeba4 --- /dev/null +++ b/Chapter4/pythonch4/ex3_3.py @@ -0,0 +1,14 @@ +import sympy as sp + +# Define matrix A +A = sp.Matrix([[0, 6], [-1, -5]]) + +# Define symbolic variables +t = sp.symbols('t') + +# Compute the matrix exponential exp(A*t) +exp_A_t = sp.exp(A * t) + +# Print the result +print("expm(A*t):") +sp.pprint(exp_A_t) diff --git a/Chapter4/pythonch4/ex3_8.py b/Chapter4/pythonch4/ex3_8.py new file mode 100644 index 0000000..ae994e0 --- /dev/null +++ b/Chapter4/pythonch4/ex3_8.py @@ -0,0 +1,28 @@ +import numpy as np +from scipy.signal import StateSpace, tf2zpk + +# Define matrices +A = np.array([[0, 1], [-2, -3]]) +B = np.array([[1], [1]]) +C = np.array([[1, 0]]) +D = np.array([[0]]) + +# Convert to state-space representation +sys = StateSpace(A, B, C, D) + +# Get eigenvalues +eigs = np.linalg.eigvals(A) + +# Get poles +poles = sys.poles + +# Get transfer function representation +tf = sys.to_tf() + +# Calculate zeros +zeros, _, _ = tf2zpk(tf.num, tf.den) + +# Print results +print("Eigenvalues:", eigs) +print("Poles:", poles) +print("Zeros:", zeros) diff --git a/Chapter4/pythonch4/jordan_forms.py b/Chapter4/pythonch4/jordan_forms.py new file mode 100644 index 0000000..58bb24e --- /dev/null +++ b/Chapter4/pythonch4/jordan_forms.py @@ -0,0 +1,118 @@ +#Eigenvalues and Eigenvectors +import numpy as np +from scipy.linalg import eig + +# Matrix A +A = np.array([[0, 1, 0, 0], + [0, 0, -9.8, 0], + [0, 0, 0, 1], + [0, 0, 19.6, 0]]) + +# Compute eigenvalues and eigenvectors +eigenvalues, eigenvectors = eig(A) + +# Diagonal matrix of eigenvalues +J = np.diag(eigenvalues) + +# T is the matrix of eigenvectors +T = eigenvectors + +print("T (eigenvectors):") +print(T) +print("\nJ (Jordan form):") +print(J) +#################### +#Transformation of B +import numpy as np +from scipy.linalg import eig + +# Matrix A and vector B +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]).reshape(-1, 1) + +# Compute eigenvalues and eigenvectors +eigenvalues, eigenvectors = eig(A) +J = np.diag(eigenvalues) +T = eigenvectors + +# Compute Bn = inv(T) * B +T_inv = np.linalg.inv(T) +Bn = np.dot(T_inv, B) + +print("Bn:") +print(Bn) +#################33 +#Transformation of C +import numpy as np +from scipy.linalg import eig + +# Matrix A, vector B, and matrix C +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]).reshape(-1, 1) +C = np.array([[1, 0, 0, 0], + [0, 0, 1, 0]]) + +# Compute eigenvalues and eigenvectors +eigenvalues, eigenvectors = eig(A) +J = np.diag(eigenvalues) +T = eigenvectors + +# Compute Bn = inv(T) * B +T_inv = np.linalg.inv(T) +Bn = np.dot(T_inv, B) + +# Compute Cn = C * T +Cn = np.dot(C, T) + +print("Cn:") +print(Cn) +##################### +#Eigenvalues and Eigenvectors for a Different Matrix A +import numpy as np +from scipy.linalg import eig + +# Matrix A +A = np.array([[0, 1, 0, 3], + [0, -1, 1, 10], + [0, 0, 0, 1], + [0, 0, -1, -2]]) + +# Compute eigenvalues and eigenvectors +eigenvalues, eigenvectors = eig(A) +J = np.diag(eigenvalues) +T = eigenvectors + +print("T (eigenvectors):") +print(T) +print("\nJ (Jordan form):") +print(J) +############# +#Jordan Form Using SymPy +import numpy as np +import sympy as sp + +# Matrix A +A = np.array([[0, 1, 0, 3], + [0, -1, 1, 10], + [0, 0, 0, 1], + [0, 0, -1, -2]]) + +# Convert the numpy array to a SymPy Matrix +A_sym = sp.Matrix(A) +# Compute the Jordan form using SymPy +J, P = A_sym.jordan_form() + +# Convert back to numpy arrays if needed +J_np = np.array(J).astype(np.float64) +P_np = np.array(P).astype(np.float64) + +print("P (Transformation matrix):") +print(P_np) +print("\nJ (Jordan form):") +print(J_np) diff --git a/Chapter5_6/python_ch5_6/ex5_1.py b/Chapter5_6/python_ch5_6/ex5_1.py new file mode 100644 index 0000000..ed6fbe1 --- /dev/null +++ b/Chapter5_6/python_ch5_6/ex5_1.py @@ -0,0 +1,97 @@ +#The outputs from MATLAB and Python are essentially the same, +#with minor differences in formatting and numerical precision. +#The core values and structures are consistent across both implementations. + + + +import numpy as np +import control as ctrl + +# Define the system matrices +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]]) +sys1 = ctrl.ss(A1, B1, C1, D1) +tf1 = ctrl.ss2tf(sys1) + +# Print transfer function for sys1 +print("ans =") +print(tf1) +print("Continuous-time transfer function.\n") + +# Define the second system matrices +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]]) +sys2 = ctrl.ss(A2, B2, C2, D2) +tf2 = ctrl.ss2tf(sys2) + +# Print transfer function for sys2 +print("ans =") +print(tf2) +print("Continuous-time transfer function.\n") + +# Define the third system matrices +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]]) +sys3 = ctrl.ss(A3, B3, C3, D3) +tf3 = ctrl.ss2tf(sys3) + +# Print transfer function for sys3 +print("ans =") +print(tf3) +print("Continuous-time transfer function.\n") + +# Compute and print observability matrix for sys3 +observability_matrix = ctrl.obsv(A3, C3) +print("ans =") +print(observability_matrix) +print("\n") + +# Define the fourth system matrices +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]]) +sys4 = ctrl.ss(A4, B4, C4, D4) +tf4 = ctrl.ss2tf(sys4) + +# Print transfer function for sys4 +print("ans =") +print(tf4) +print("Continuous-time transfer function.\n") + +# Compute and print controllability matrix for sys4 +controllability_matrix = ctrl.ctrb(A4, B4) +print("ans =") +print(controllability_matrix) +print("\n") + +# Define transfer function and convert to state space +num = [1, 0, 1] +den = [1, 6, 11, 5] +sys_tf = ctrl.TransferFunction(num, den) +sys_ss = ctrl.tf2ss(sys_tf) +A, B, C, D = sys_ss.A, sys_ss.B, sys_ss.C, sys_ss.D + +# Print the state-space matrices +print("A =\n") +print(A) +print("\n") +print("B =\n") +print(B) +print("\n") +print("C =\n") +print(C) +print("\n") +print("D =\n") +print(D) +print("\n") + +# Print the full state-space model +print("mysys =\n") +print(sys_ss) diff --git a/Chapter5_6/python_ch5_6/ex5_2.py b/Chapter5_6/python_ch5_6/ex5_2.py new file mode 100644 index 0000000..9c793c2 --- /dev/null +++ b/Chapter5_6/python_ch5_6/ex5_2.py @@ -0,0 +1,28 @@ +import numpy as np +import control as ctrl + +# Define the first system matrices +A1 = np.array([[-1, 1, 0], [0, -1, 0], [0, 0, -2]]) +B1 = np.array([[0], [1], [1]]) +C1 = np.array([[4, -8, 9]]) +D1 = np.array([[0]]) +sys1 = ctrl.ss(A1, B1, C1, D1) +tf1 = ctrl.ss2tf(sys1) + +# Print transfer function for sys1 +print("ans =") +print(tf1) +print("Continuous-time transfer function.\n") + +# Define the second system matrices +A2 = np.array([[-1, 0, 0], [1, -1, 0], [0, 0, -2]]) +B2 = np.array([[4], [-8], [9]]) +C2 = np.array([[0, 1, 1]]) +D2 = np.array([[0]]) +sys2 = ctrl.ss(A2, B2, C2, D2) +tf2 = ctrl.ss2tf(sys2) + +# Print transfer function for sys2 +print("ans =") +print(tf2) +print("Continuous-time transfer function.\n") diff --git a/Chapter5_6/python_ch5_6/ex5_3.py b/Chapter5_6/python_ch5_6/ex5_3.py new file mode 100644 index 0000000..b30d405 --- /dev/null +++ b/Chapter5_6/python_ch5_6/ex5_3.py @@ -0,0 +1,48 @@ +import numpy as np +import control as ctrl + +# Define numerator and denominator lists for each SISO system +num1 = [1] # First transfer function numerator +den1 = np.convolve([1, 0, 1], [1, 1]).tolist() # First transfer function denominator + +num2 = [1, 0] # Second transfer function numerator +den2 = [1, 3, 2] # Second transfer function denominator + +# Combine them into MIMO system +num = [[num1], [num2]] +den = [[den1], [den2]] + +# Create transfer function +sys = ctrl.tf(num, den) + +print(sys) +#################################### +import numpy as np +import control as ctrl + +# Define numerator and denominator lists for each SISO system +num1 = [1] # First transfer function numerator +den1 = np.convolve([1, 0, 1], [1, 1]).tolist() # First transfer function denominator + +num2 = [1, 0] # Second transfer function numerator +den2 = [1, 3, 2] # Second transfer function denominator + +# Combine them into MIMO system +num = [[num1], [num2]] +den = [[den1], [den2]] + +# Create transfer function +sys = ctrl.tf(num, den) + +# Convert transfer function to zero-pole-gain form +z1, p1, k1 = ctrl.tf2zpk(num[0][0], den[0][0]) +z2, p2, k2 = ctrl.tf2zpk(num[1][0], den[1][0]) + +# Create zero-pole-gain system +sys1_1 = ctrl.ZerosPolesGain(z1, p1, k1) +sys1_2 = ctrl.ZerosPolesGain(z2, p2, k2) + +print(sys1_1) +print(sys1_2) + + diff --git a/Chapter5_6/python_ch5_6/ex5_4.py b/Chapter5_6/python_ch5_6/ex5_4.py new file mode 100644 index 0000000..aa9a980 --- /dev/null +++ b/Chapter5_6/python_ch5_6/ex5_4.py @@ -0,0 +1,60 @@ +import control as ctrl + +# Define numerator and denominator matrices +num = [[[1], [2]], [[-1], [1]]] +den = [[[1, 1], [1, 2]], [[1, 1], [1, 3]]] + +# Create transfer function system +sys = ctrl.TransferFunction(num, den) +print('sys =') +print(sys) + +# Convert to state-space representation and minimal realization +sys2 = ctrl.minreal(ctrl.ss(sys)) +print('\nsys2 =') +print(sys2) + +# Define s in the Laplace domain +s = ctrl.TransferFunction([1, 0], [1]) + +# Define the system matrix +mysys = ctrl.TransferFunction([[1/(s + 1), 2/(s + 2)], [-1/(s + 1), 1/(s + 3)]]) +print('\nmysys =') +print(mysys) + +# Convert to state-space representation and minimal realization +myss = ctrl.minreal(ctrl.ss(mysys)) +print('\nmyss =') +print(myss) + +# Define the zpk system +s = ctrl.TransferFunction([1, 0], [1]) + +# Define transfer functions +G11 = 1/(s + 1) +G12 = 2/(s + 2) +G21 = -1/(s + 1) +G22 = 1/(s + 3) + +# Define interconnection structure +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]' + +# Interconnection +sysic = ctrl.InterconnectedSystem( + (G11, G12, G21, G22), inputvar, input_to_G11, input_to_G12, input_to_G21, input_to_G22, outputvar +) + +# Set input and output names +sysic.InputName = ['u1', 'u2'] +sysic.OutputName = ['y1', 'y2'] + +# Convert to state-space representation and minimal realization +G = ctrl.minreal(ctrl.ss(sysic)) +print('\nans =') +print(G) diff --git a/Chapter5_6/python_ch5_6/ex5_6.py b/Chapter5_6/python_ch5_6/ex5_6.py new file mode 100644 index 0000000..8f74754 --- /dev/null +++ b/Chapter5_6/python_ch5_6/ex5_6.py @@ -0,0 +1,10 @@ +import numpy as np +from scipy.linalg import solve_continuous_lyapunov + +A = np.array([[-1, -2], [1, -4]]) +Q = np.eye(2) +P = solve_continuous_lyapunov(A.T, -Q) +P *= 60 # Scale P by 60 as in the MATLAB code + +print("P =\n", np.round(P).astype(int)) +print("\ndet(P) =", int(round(np.linalg.det(P)))) diff --git a/Chapter5_6/python_ch5_6/test.py b/Chapter5_6/python_ch5_6/test.py new file mode 100644 index 0000000..f699287 --- /dev/null +++ b/Chapter5_6/python_ch5_6/test.py @@ -0,0 +1,52 @@ +import numpy as np + +# Define the state-space matrices +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]]) + +# Function to compute transfer function for each output +def compute_transfer_function(A, B, C, D): + s = np.linalg.eigvals(A) # Get eigenvalues for s domain + I = np.eye(A.shape[0]) + transfer_functions = [] + + for i in range(C.shape[0]): # Iterate over each output + for j in range(B.shape[1]): # Iterate over each input + # Compute transfer function H(s) = C(sI - A)^-1 B + D + C_i = C[i, :].reshape(1, -1) + B_j = B[:, j].reshape(-1, 1) + H_s = [] + + for s_val in s: + sI_minus_A = s_val * I - A + sI_minus_A_inv = np.linalg.inv(sI_minus_A) + H_s_val = np.dot(C_i, np.dot(sI_minus_A_inv, B_j)) + D[i, j] + H_s.append(H_s_val[0, 0]) + + transfer_functions.append(H_s) + + return transfer_functions + +# Compute transfer functions +tf_list = compute_transfer_function(A, B, C, D) + +# Print transfer functions +for i, tf in enumerate(tf_list): + output_index = i // B.shape[1] + 1 + input_index = i % B.shape[1] + 1 + print(f"Transfer Function for output {output_index}, input {input_index}:") + print(tf) + print() From 38cb82c7a6947b1e73c6af0c452875c72cd6978c Mon Sep 17 00:00:00 2001 From: Elnaz MohseniNezhad Date: Fri, 5 Jul 2024 14:28:16 +0330 Subject: [PATCH 3/4] Chapter 7 python codes added by Elnaz MohseniNezhad --- Chapter7/done.txt | 9 ++ Chapter7/pythonch7/Active_s.py | 55 +++++++++ Chapter7/pythonch7/CL_DCmotor_solver.py | 131 +++++++++++++++++++++ Chapter7/pythonch7/CL_DCmotor_w_solver.m | 16 +++ Chapter7/pythonch7/CL_Invpend_solver.m | 17 +++ Chapter7/pythonch7/DC_motor.m | 17 +++ Chapter7/pythonch7/DC_motor_w.m | 19 +++ Chapter7/pythonch7/Invpend_solver2.py | 38 ++++++ Chapter7/pythonch7/ReadMe.md | 107 +++++++++++++++++ Chapter7/pythonch7/actives_fb.py | 46 ++++++++ Chapter7/pythonch7/ex6_10.py | 20 ++++ Chapter7/pythonch7/ex6_13.py | 134 ++++++++++++++++++++++ Chapter7/pythonch7/ex6_14.py | 25 ++++ Chapter7/pythonch7/ex6_15.py | 23 ++++ Chapter7/pythonch7/ex6_2.py | 18 +++ Chapter7/pythonch7/ex6_3.py | 19 +++ Chapter7/pythonch7/ex6_4.py | 28 +++++ Chapter7/pythonch7/ex6_5.py | 28 +++++ Chapter7/pythonch7/ex6_6.py | 24 ++++ Chapter7/pythonch7/ex6_9.py | 50 ++++++++ Chapter7/pythonch7/fig6_5.py | 49 ++++++++ Chapter7/pythonch7/inverted_pendulum_k1.m | 24 ++++ Chapter7/pythonch7/inverted_pendulum_k2.m | 24 ++++ Chapter7/pythonch7/train_fb.m | 38 ++++++ Chapter7/pythonch7/train_fb1.m | 34 ++++++ Chapter7/pythonch7/train_fb2.m | 33 ++++++ Chapter7/pythonch7/train_fb_solver.py | 97 ++++++++++++++++ Chapter7/pythonch7/train_lqr.py | 50 ++++++++ Chapter7/pythonch7/train_model.m | 29 +++++ Chapter7/pythonch7/train_model1.m | 25 ++++ Chapter7/pythonch7/train_solver1.py | 45 ++++++++ 31 files changed, 1272 insertions(+) create mode 100644 Chapter7/done.txt create mode 100644 Chapter7/pythonch7/Active_s.py create mode 100644 Chapter7/pythonch7/CL_DCmotor_solver.py create mode 100644 Chapter7/pythonch7/CL_DCmotor_w_solver.m create mode 100644 Chapter7/pythonch7/CL_Invpend_solver.m create mode 100644 Chapter7/pythonch7/DC_motor.m create mode 100644 Chapter7/pythonch7/DC_motor_w.m create mode 100644 Chapter7/pythonch7/Invpend_solver2.py create mode 100644 Chapter7/pythonch7/ReadMe.md create mode 100644 Chapter7/pythonch7/actives_fb.py create mode 100644 Chapter7/pythonch7/ex6_10.py create mode 100644 Chapter7/pythonch7/ex6_13.py create mode 100644 Chapter7/pythonch7/ex6_14.py create mode 100644 Chapter7/pythonch7/ex6_15.py create mode 100644 Chapter7/pythonch7/ex6_2.py create mode 100644 Chapter7/pythonch7/ex6_3.py create mode 100644 Chapter7/pythonch7/ex6_4.py create mode 100644 Chapter7/pythonch7/ex6_5.py create mode 100644 Chapter7/pythonch7/ex6_6.py create mode 100644 Chapter7/pythonch7/ex6_9.py create mode 100644 Chapter7/pythonch7/fig6_5.py create mode 100644 Chapter7/pythonch7/inverted_pendulum_k1.m create mode 100644 Chapter7/pythonch7/inverted_pendulum_k2.m create mode 100644 Chapter7/pythonch7/train_fb.m create mode 100644 Chapter7/pythonch7/train_fb1.m create mode 100644 Chapter7/pythonch7/train_fb2.m create mode 100644 Chapter7/pythonch7/train_fb_solver.py create mode 100644 Chapter7/pythonch7/train_lqr.py create mode 100644 Chapter7/pythonch7/train_model.m create mode 100644 Chapter7/pythonch7/train_model1.m create mode 100644 Chapter7/pythonch7/train_solver1.py diff --git a/Chapter7/done.txt b/Chapter7/done.txt new file mode 100644 index 0000000..0610b73 --- /dev/null +++ b/Chapter7/done.txt @@ -0,0 +1,9 @@ +Active_s done +active_fb done +CL_DCmotor_solver failed +ex6_2 done +ex6_3 done +ex6_4 done +ex6_5 done +ex6_6 done +ex6_9 done \ No newline at end of file diff --git a/Chapter7/pythonch7/Active_s.py b/Chapter7/pythonch7/Active_s.py new file mode 100644 index 0000000..37949c2 --- /dev/null +++ b/Chapter7/pythonch7/Active_s.py @@ -0,0 +1,55 @@ +import numpy as np +import control as ctrl +import matplotlib.pyplot as plt + +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 = ctrl.ss(A, b2, C, D) + +# Time vector +t = np.arange(0, 7, 0.01) +N = len(t) + + +# Initial state +x0 = np.array([[0.2], [0], [0], [0]]) + +# Simulate initial response +response = ctrl.forced_response(active_suspension, T=t, X0=x0) + + +# Plot state variables x1 and x2 +plt.plot(t, response.states[0], 'k', label='x1') +plt.plot(t, response.states[1], 'k-.', label='x2') +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.grid(True) +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 response of the system +# Simulate the response of the system with the provided input signal +response = ctrl.forced_response(active_suspension, T=t, U=u) + +import matplotlib.pyplot as plt + +# Plot state variables x1 and x2 +plt.plot(t, response.states[0], 'k', label='x1') +plt.plot(t, response.states[1], 'k-.', label='x2') +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.grid(True) +plt.legend() +plt.show() diff --git a/Chapter7/pythonch7/CL_DCmotor_solver.py b/Chapter7/pythonch7/CL_DCmotor_solver.py new file mode 100644 index 0000000..ec5bdc1 --- /dev/null +++ b/Chapter7/pythonch7/CL_DCmotor_solver.py @@ -0,0 +1,131 @@ +# import numpy as np +# import matplotlib.pyplot as plt + +# # Clear all variables (not usually necessary in Python) +# # globals().clear() + +# # Clear the current figure +# plt.clf() + +# # Define the time span +# tspan = [0, 3] + +# # Define the initial conditions +# x0 = np.array([0, 0, 0]) + +# # If you want to mimic echoing of commands, use print statements (optional) +# print("tspan =", tspan) +# print("x0 =", x0) + + +# from scipy.integrate import solve_ivp +# import numpy as np +# import matplotlib.pyplot as plt + +# # Define the DC_motor function (placeholder function; needs to be defined) +# def DC_motor(t, x): +# # Placeholder for the differential equations +# # Replace this with the actual equations +# dxdt = [0, 0, 0] +# return dxdt + +# # Define the time span and initial conditions +# tspan = [0, 3] +# x0 = np.array([0, 0, 0]) + +# # Define options for the solver +# options = { +# 'max_step': 1e-2 +# } + +# # Solve the ODE +# solution = solve_ivp(DC_motor, tspan, x0, **options) + +# # Extract the results +# t = solution.t +# x = solution.y.T + +# # Plotting function (to mimic odeplot) +# def odeplot(t, x): +# plt.plot(t, x) +# plt.xlabel('Time') +# plt.ylabel('State Variables') +# plt.show() + +# # Call the plotting function +# odeplot(t, x) + + +# import numpy as np +# import matplotlib.pyplot as plt + +# # Assume t and x are already defined from the previous solve_ivp result +# # Example values for demonstration: +# t = np.linspace(0, 3, 100) +# x = np.zeros((100, 3)) # Replace this with the actual solution data +# x[:, 0] = np.sin(t) # Example data for plotting + +# # Convert angular displacement from radians to degrees +# angular_displacement = x[:, 0] * 180 / np.pi + +# # Plot the data +# plt.plot(t, angular_displacement, 'k') +# plt.grid(True) +# plt.xlabel('Time (sec)') +# plt.ylabel('Angular displacement θ (degrees)') + +# # Uncomment the legend line if you want to include a legend +# # plt.legend(['θ (degrees)']) + +# # Set the line width for all lines in the figure +# lines = plt.gca().get_lines() +# for line in lines: +# line.set_linewidth(2) + +# # Display the plot +# plt.show() +############################# + +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import solve_ivp + +# Define the DC_motor function (replace this with your actual function) +def DC_motor(t, x): + # Example placeholder for the differential equations + # Ensure this matches the MATLAB function exactly + dxdt = [x[1], -0.1 * x[0], 0.1] # Adjust as per your actual equations + return dxdt + +# Define the time span and initial conditions +tspan = [0, 3] +x0 = [0, 0, 0] + +# Define options for the solver +options = {'max_step': 1e-2} + +# Solve the ODE +solution = solve_ivp(DC_motor, tspan, x0, method='RK45', max_step=options['max_step']) + +# Extract the results +t = solution.t +x = solution.y.T + +# Plot the data +plt.figure() +plt.plot(t, x[:, 0] * 180 / np.pi, 'k') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Angular displacement θ (degrees)') + +# Uncomment the legend line if you want to include a legend +# plt.legend(['θ (degrees)']) + +# Set the line width for all lines in the figure +lines = plt.gca().get_lines() +for line in lines: + line.set_linewidth(2) + +# Display the plot +plt.show() + diff --git a/Chapter7/pythonch7/CL_DCmotor_w_solver.m b/Chapter7/pythonch7/CL_DCmotor_w_solver.m new file mode 100644 index 0000000..02d8099 --- /dev/null +++ b/Chapter7/pythonch7/CL_DCmotor_w_solver.m @@ -0,0 +1,16 @@ +% +% Simulation of closed-loop DC motor +% Copyright Hamid D. Taghirad 2013 +% +clf, echo on +tspan=[0 3]; +x0=[0; 0; 0; 0.01]; + +[t,x] = ode45(@DC_motor_w,tspan, x0, ... + odeset('OutputFcn','odeplot','MaxStep',1e-2)); + +plot(t,x(:,1)*180/pi,'k'),grid +xlabel('Time (sec)') +ylabel('Angular displacement (degrees)') +%legend('\theta (degrees)') +set(findall(figure(1),'type','line'),'linewidth',2) diff --git a/Chapter7/pythonch7/CL_Invpend_solver.m b/Chapter7/pythonch7/CL_Invpend_solver.m new file mode 100644 index 0000000..131861c --- /dev/null +++ b/Chapter7/pythonch7/CL_Invpend_solver.m @@ -0,0 +1,17 @@ +% +% Simulation of closed-loop Inverted Pendulum +% Copyright Hamid D. Taghirad 2013 +% +clear all +clf, echo on +tspan=[0 4]; +x0=[0; 0; 0.26; 0]; + +[t,x] = ode45(@inverted_pendulum_k1,tspan, x0, ... + odeset('OutputFcn','odeplot','MaxStep',1e-2)); + +plot(t,x(:,1),'k',t,x(:,3)*180/pi,'-.k'),grid +xlabel('Time (sec)') +ylabel('State Variables') +legend('x (m)', '\theta (degrees)') +set(findall(figure(1),'type','line'),'linewidth',2) diff --git a/Chapter7/pythonch7/DC_motor.m b/Chapter7/pythonch7/DC_motor.m new file mode 100644 index 0000000..47e5c50 --- /dev/null +++ b/Chapter7/pythonch7/DC_motor.m @@ -0,0 +1,17 @@ +% +% Model of DC Motor for ode +% Copyright Hamid D. Taghirad 2013 +% +function xp=DC_motor(t,x) +% State variable x=[\theta; \omega; i] +A=[0 1 0; 0 0 4.438; 0 -12 -24]; +B=[0 0; 0 -7.396; 20 0]; +theta_d=0; % Desired angular position +Tl=0.01; % step disturbance +v=2.255*Tl -3.0*(x(1)-theta_d) -0.879*x(2) -0.1529*x(3); +u=[v;Tl]; +xp=A*x+B*u; +end + + + \ No newline at end of file diff --git a/Chapter7/pythonch7/DC_motor_w.m b/Chapter7/pythonch7/DC_motor_w.m new file mode 100644 index 0000000..0c69922 --- /dev/null +++ b/Chapter7/pythonch7/DC_motor_w.m @@ -0,0 +1,19 @@ +% +% Model of DC Motor for ode +% Copyright Hamid D. Taghirad 2013 +% +function xp=DC_motor_w(t,x) +% State variable x=[\theta; \omega; i; Tl] +A=[0 1 0 0; 0 0 4.438 -7.396; 0 -12 -24 0; 0 0 0 -1]; +B=[0 0; 0 -7.396; 20 0; 0 0]; +k =[3.0000 0.8796 0.1529 -1.8190]; +theta_d=0; % Desired angular position +Tl=0.01; % step disturbance +v1=2.255*Tl-k(1)*(x(1)-theta_d)-k(2)*x(2)-k(3)*x(3); +v2=2.255*Tl-k*x; +u=[v1;Tl]; +xp=A*x+B*u; +end + + + \ No newline at end of file diff --git a/Chapter7/pythonch7/Invpend_solver2.py b/Chapter7/pythonch7/Invpend_solver2.py new file mode 100644 index 0000000..b4273e4 --- /dev/null +++ b/Chapter7/pythonch7/Invpend_solver2.py @@ -0,0 +1,38 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import odeint + +def inverted_pendulum_k2(x, t): + # System parameters + g = 9.81 # gravitational acceleration (m/s^2) + L = 1.0 # length of the pendulum (m) + m = 1.0 # mass of the pendulum (kg) + b = 0.1 # damping coefficient + + # Extract state variables + x1, x2, x3, x4 = x # x1 = x, x2 = x_dot, x3 = theta, x4 = theta_dot + + # Equations of motion + dx1 = x2 + dx2 = (m * g * np.sin(x3) - b * x2) / (m * L**2) + dx3 = x4 + dx4 = (-m * g * np.cos(x3) * np.sin(x3) - b * x4) / (m * L**2) + + return [dx1, dx2, dx3, dx4] + +# Initial conditions and time span +t_span = np.linspace(0, 3, 300) # from 0 to 3 seconds +x0 = [0, 0, 0.6, 0] # initial conditions [x, x_dot, theta, theta_dot] + +# Solve the differential equation +x = odeint(inverted_pendulum_k2, x0, t_span) + +# Plot results +plt.figure(figsize=(10, 6)) +plt.plot(t_span, x[:, 0], 'k', label='x (m)') +plt.plot(t_span, x[:, 2], '-.k', label='theta (rad)') +plt.xlabel('Time (sec)') +plt.ylabel('State Variables') +plt.grid(True) +plt.legend() +plt.show() diff --git a/Chapter7/pythonch7/ReadMe.md b/Chapter7/pythonch7/ReadMe.md new file mode 100644 index 0000000..a30d744 --- /dev/null +++ b/Chapter7/pythonch7/ReadMe.md @@ -0,0 +1,107 @@ +# Simulation Models in MATLAB + +This repository contains MATLAB scripts for simulating various mechanical and control systems. Detailed descriptions for each script are provided to help users understand their functions and applications. + +## File Descriptions + +### 📄 [Active_s1.m](./Active_s1.m) +- **Purpose**: Simulates an active suspension system with methods for initial response and input generation. + +### 📄 [actives_fb.m](./actives_fb.m) +- **Purpose**: Simulation and pole placement for an active suspension system, specifically Example 6-18 from the textbook. + +### 📄 [CL_DCmotor_solver.m](./CL_DCmotor_solver.m) +- **Purpose**: Simulates a closed-loop DC motor system, focusing on control and stability under various conditions. + +### 📄 [CL_DCmotor_w_solver.m](./CL_DCmotor_w_solver.m) +- **Purpose**: Another variation of the closed-loop DC motor simulation, possibly incorporating additional complexities or variations in control strategies. + +### 📄 [CL_Invpend_solver.m](./CL_Invpend_solver.m) +- **Purpose**: Simulates a closed-loop inverted pendulum, a key study in control system education for stability and control response analysis. + +### 📄 [DC_motor.m](./DC_motor.m) +- **Purpose**: Models a DC motor for use with ordinary differential equation solvers, focusing on fundamental motor dynamics including angular position, velocity, and current. + +### 📄 [DC_motor_w.m](./DC_motor_w.m) +- **Purpose**: Extends the basic DC motor model to include load torque, enhancing the model's applicability to real-world scenarios where external forces impact motor performance. + +### 📄 [ex6_2.m](./ex6_2.m) +- **Purpose**: Provides a MATLAB implementation for Example 6-2 from the "Modern Book", focusing on specific control strategies or scenarios. + +### 📄 [ex6_3.m](./ex6_3.m) +- **Purpose**: Implements MATLAB code for Example 6-14 from the "Modern Book", offering insights into advanced control topics. + +### 📄 [ex6_4.m](./ex6_4.m) +- **Purpose**: MATLAB script for Example 6-4 from the "Modern Book", focusing on detailed examples of control system analysis and application. + +### 📄 [ex6_5.m](./ex6_5.m) +- **Purpose**: MATLAB script for Example 6-5 from the "Modern Book", focusing on specific control techniques and their application. + +### 📄 [ex6_6.m](./ex6_6.m) +- **Purpose**: Implements Example 6-6 from the "Modern Book", offering insights into control system design and analysis. + +### 📄 [ex6_9.m](./ex6_9.m) +- **Purpose**: Provides MATLAB implementation for Example 6-9 of the "Modern Book", designed to showcase advanced control system strategies. + +### 📄 [ex6_10.m](./ex6_10.m) +- **Purpose**: MATLAB script for Example 6-10 from the "Modern Book", focusing on detailed examples of control system design and stability analysis. + +### 📄 [ex6_13.m](./ex6_13.m) +- **Purpose**: Implements MATLAB code for Example 6-13 from the "Modern Book", discussing specific scenarios in advanced control systems. + +### 📄 [ex6_14.m](./ex6_14.m) +- **Purpose**: MATLAB implementation for Example 6-14 from the "Modern Book", which focuses on the theoretical and practical aspects of control systems. + +### 📄 [ex6_15.m](./ex6_15.m) +- **Purpose**: Provides a MATLAB script for Example 6-16 of the "Modern Book", demonstrating complex control system techniques and their implementation. + +### 📄 [fig6_5.m](./fig6_5.m) +- **Purpose**: Generates Figure 6-5 as described in the "Modern Book", useful for visualizing and understanding specific control system concepts. + +### 📄 [inverted_pendulum_k1.m](./inverted_pendulum_k1.m) +- **Purpose**: Models an inverted pendulum with specific state variables, providing a detailed basis for simulation and control analysis. + +### 📄 [inverted_pendulum_k2.m](./inverted_pendulum_k2.m) +- **Purpose**: Another model of the inverted pendulum similar to `inverted_pendulum_k1.m`, potentially with variations in parameters or control strategies. + + +### 📄 [Invpend_solver2.m](./Invpend_solver2.m) +- **Purpose**: Provides another simulation script for the inverted pendulum model, emphasizing system initialization and clearing variables for fresh simulation runs. + +### 📄 [train_fb.m](./train_fb.m) +- **Purpose**: Models a feedback-controlled train system, defining state variables and interactions among multiple train cars. + +### 📄 [train_fb_solver.m](./train_fb_solver.m) +- **Purpose**: Simulates the behavior of a feedback-controlled train system under various scenarios and control inputs. + +### 📄 [train_fb1.m](./train_fb1.m) +- **Purpose**: An extension of the train model focusing on detailed dynamic equations for feedback control scenarios. + +### 📄 [train_fb2.m](./train_fb2.m) +- **Purpose**: Another train model variant designed for feedback control studies, specifying comprehensive state variables for simulation. + +### 📄 [train_lqr.m](./train_lqr.m) +- **Purpose**: Implements an LQR (Linear Quadratic Regulator) control strategy for the train model, focusing on optimizing the train's response according to predefined performance criteria. + +### 📄 [train_model.m](./train_model.m) +- **Purpose**: Provides a detailed model of a train system including constant inputs and state variables, serving as a foundation for further control design and simulation. + +### 📄 [train_model1.m](./train_model1.m) +- **Purpose**: Similar to `train_model.m`, this script models a train system with defined state variables for dynamic analysis. + +### 📄 [train_solver1.m](./train_solver1.m) +- **Purpose**: Simulates the dynamic response of the train model, allowing for analysis of system behavior under various operational conditions. + +## Contribution +Fork this repository to contribute and propose changes or enhancements via pull requests. For major changes, please open an issue first to discuss your proposals. + +## License +Distributed under the MIT License. See `LICENSE` for more information. + +## Contact +- Prof. Hamid D. Taghirad – [Visit Website](https://aras.kntu.ac.ir/taghirad/) +- Project Link: [https://github.com/aras-labs/Modern_Control](https://github.com/aras-labs/Modern_Control) + +![GitHub forks](https://img.shields.io/github/forks/aras-labs/Modern_Control?style=social) +![GitHub stars](https://img.shields.io/github/stars/aras-labs/Modern_Control?style=social) +![GitHub watchers](https://img.shields.io/github/watchers/aras-labs/Modern_Control?style=social) diff --git a/Chapter7/pythonch7/actives_fb.py b/Chapter7/pythonch7/actives_fb.py new file mode 100644 index 0000000..69e7fe5 --- /dev/null +++ b/Chapter7/pythonch7/actives_fb.py @@ -0,0 +1,46 @@ +import matplotlib.pyplot as plt +import numpy as np +from scipy.signal import StateSpace, impulse +from control import place # Importing pole placement function from control module + +# 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]).reshape(-1, 1) # Reshape to column vector +b2 = np.array([0, -1, 0, 0, 0]).reshape(-1, 1) # Reshape to column vector +pd = np.array([-5, -25+25j, -25-25j, -3+3j, -3-3j]) + +# Compute gain matrix k using pole placement (equivalent to place in MATLAB) +k = place(A, b1, pd) + +# Closed loop system +Acl = A - np.dot(b1, k) +Bcl = 0.1 * b2 +C = np.array([1, 0, 0, 0, 0]) +D = np.array([0]) +ld = 0.1 + +# Define state-space system +active_fb = StateSpace(Acl, Bcl, C, D) + +# Compute impulse response +t, y = impulse(active_fb) + +# Plotting +plt.figure(figsize=(10, 6)) + +# Plot l1 (y + 0.1) in solid black line +plt.plot(t, y + 0.1, 'k', label='l1') + +# Plot x (y - 0.574 * 0.1) in dashed black line +plt.plot(t, y - 0.574 * 0.1, 'k-.', label='x') + +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.legend() +plt.show() + diff --git a/Chapter7/pythonch7/ex6_10.py b/Chapter7/pythonch7/ex6_10.py new file mode 100644 index 0000000..b79b824 --- /dev/null +++ b/Chapter7/pythonch7/ex6_10.py @@ -0,0 +1,20 @@ +import numpy as np + +# Define matrices A, B, and f +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]]) + +# Compute b +b = B @ f + +# Compute eigenvalues and eigenvectors of A +eigenvalues, eigenvectors = np.linalg.eig(A) + +# Compute inverse of eigenvectors matrix +v = np.linalg.inv(eigenvectors) + +# Compute p +p = v[:2, :] @ b + +print("p =\n", p) diff --git a/Chapter7/pythonch7/ex6_13.py b/Chapter7/pythonch7/ex6_13.py new file mode 100644 index 0000000..d3d8883 --- /dev/null +++ b/Chapter7/pythonch7/ex6_13.py @@ -0,0 +1,134 @@ + +import numpy as np +import matplotlib.pyplot as plt +from scipy.linalg import solve_continuous_are +from scipy.signal import StateSpace +from scipy.integrate import solve_ivp + +# Define the system matrices +A = np.array([[0, 1, 0], + [0, 0, 4.438], + [0, -12, -24]]) + +b = np.array([[0], + [0], + [20]]) + +R = np.array([[1]]) # Define R as a 2D array (scalar case) + +# Define different values of Q +Q1 = np.diag([4, 0, 0]) +Q2 = np.diag([9, 0, 0]) +Q3 = np.diag([20, 0, 0]) +Q4 = np.diag([9, 3, 0]) # Added Q4 + +# Function to compute LQR gain matrix K +def compute_lqr_gain(A, b, Q, R): + P = solve_continuous_are(A, b, Q, R) + K = np.linalg.inv(R) @ b.T @ P + return K + +# Compute the LQR gains +k1 = compute_lqr_gain(A, b, Q1, R) +k2 = compute_lqr_gain(A, b, Q2, R) +k3 = compute_lqr_gain(A, b, Q3, R) +k4 = compute_lqr_gain(A, b, Q4, R) # Compute k4 + +# Define C, D, and x0 +C = np.array([[1, 0, 0]]) +D = np.array([[0]]) +x0 = np.array([-1, 0, 0]) + +# Closed-loop systems +Acl1 = A - b @ k1 +CL_sys1 = StateSpace(Acl1, b, C, D) + +Acl2 = A - b @ k2 +CL_sys2 = StateSpace(Acl2, b, C, D) + +Acl3 = A - b @ k3 +CL_sys3 = StateSpace(Acl3, b, C, D) + +Acl4 = A - b @ k4 # Acl4 +CL_sys4 = StateSpace(Acl4, b, C, D) # CL_sys4 + +# Function to simulate initial response +def simulate_initial_response(sys, x0, t_end): + def system_eq(t, x): + return np.dot(sys.A, x) + np.dot(sys.B, -np.dot(k1, x)) + + sol = solve_ivp(system_eq, [0, t_end], x0, t_eval=np.linspace(0, t_end, 100)) + return sol.y, sol.t, sol + +# Simulate initial responses for CL_sys1, CL_sys2, CL_sys3 +y1, t1, sol1 = simulate_initial_response(CL_sys1, x0, 2) +u1 = -np.dot(k1, sol1.y) + +y2, t2, sol2 = simulate_initial_response(CL_sys2, x0, 2) +u2 = -np.dot(k2, sol2.y) + +y3, t3, sol3 = simulate_initial_response(CL_sys3, x0, 2) +u3 = -np.dot(k3, sol3.y) + +# Simulate initial response for CL_sys4 (Q4) +y4, t4, sol4 = simulate_initial_response(CL_sys4, x0, 2) +u4 = -np.dot(k4, sol4.y) + +# Plotting +plt.figure(1, figsize=(12, 6)) + +# Plot for angular error +plt.subplot(121) +plt.plot(t1, y1[0], 'k-.', label='Q_{11}=4', linewidth=2) +plt.plot(t2, y2[0], 'k', label='Q_{11}=9', linewidth=2) +plt.plot(t3, y3[0], 'k--', label='Q_{11}=20', linewidth=2) +plt.grid(True) +plt.axis([0, 2, -1, 0.2]) +plt.xlabel('Time (sec)') +plt.ylabel('Angular Error (rad)') +plt.legend(loc='best') + +# Plot for motor voltage +plt.subplot(122) +plt.plot(t1, u1[0], 'k-.', label='Q_{11}=4', linewidth=2) +plt.plot(t2, u2[0], 'k', label='Q_{11}=9', linewidth=2) +plt.plot(t3, u3[0], 'k--', label='Q_{11}=20', linewidth=2) +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Motor Voltage (V)') +plt.legend(loc='best') + +# Adjust layout and display plot +plt.tight_layout() +plt.show() + +# Figure 2 for Q4 +plt.figure(2, figsize=(12, 6)) + +# Plot for angular error with Q22=0 and Q22=3 +plt.subplot(121) +plt.plot(t2, y2[0], 'k', label='Q_{22}=0', linewidth=2) +plt.plot(t4, y4[0], 'k-.', label='Q_{22}=3', linewidth=2) +plt.grid(True) +plt.axis([0, 2, -1, 0.2]) +plt.xlabel('Time (sec)') +plt.ylabel('Angular Error (rad)') +plt.legend(loc='best') + +# Plot for angular velocity with Q22=0 and Q22=3 +plt.subplot(122) +plt.plot(t2, sol2.y[1], 'k', label='Q_{22}=0', linewidth=2) +plt.plot(t4, sol4.y[1], 'k-.', label='Q_{22}=3', linewidth=2) +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Angular Velocity (rad/sec)') +plt.legend(loc='best') + +# Adjust layout and display plot +plt.tight_layout() +plt.show() + +print("k1=", k1) +print("k2=",k2) +print("k3=",k3) +print("k4=",k4) \ No newline at end of file diff --git a/Chapter7/pythonch7/ex6_14.py b/Chapter7/pythonch7/ex6_14.py new file mode 100644 index 0000000..55eeeeb --- /dev/null +++ b/Chapter7/pythonch7/ex6_14.py @@ -0,0 +1,25 @@ +import numpy as np +from scipy.linalg import solve_continuous_are + +# Define the 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]]) + +Q = np.diag([4, 0, 8.16, 0]) + +# Define R as a 2D array +R = np.array([[1 / 400]]) + +# Compute the LQR gain matrix K +P = solve_continuous_are(A, b, Q, R) +K = np.linalg.inv(R) @ b.T @ P + +print("LQR gain matrix K:") +print(K) diff --git a/Chapter7/pythonch7/ex6_15.py b/Chapter7/pythonch7/ex6_15.py new file mode 100644 index 0000000..8830a0b --- /dev/null +++ b/Chapter7/pythonch7/ex6_15.py @@ -0,0 +1,23 @@ +import numpy as np +from scipy.linalg import solve_continuous_are + +# Define the 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]]) + +Q1 = np.diag([9, 0, 0, 0]) +R = np.array([[1]]) # Define R as a 2D array (scalar case) + +# Compute the LQR gain matrix K +P = solve_continuous_are(A, b, Q1, R) +K = np.linalg.inv(R) @ b.T @ P + +print("LQR gain matrix K:") +print(K) diff --git a/Chapter7/pythonch7/ex6_2.py b/Chapter7/pythonch7/ex6_2.py new file mode 100644 index 0000000..a50400a --- /dev/null +++ b/Chapter7/pythonch7/ex6_2.py @@ -0,0 +1,18 @@ +import numpy as np +from scipy.signal import place_poles + +# Define A, b, and pd +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]) + +# Place poles using scipy.signal.place_poles +k = place_poles(A, b, pd) + +print("Gain matrix k:", k.gain_matrix) diff --git a/Chapter7/pythonch7/ex6_3.py b/Chapter7/pythonch7/ex6_3.py new file mode 100644 index 0000000..421459f --- /dev/null +++ b/Chapter7/pythonch7/ex6_3.py @@ -0,0 +1,19 @@ +import numpy as np +import control + +# Define A, b, Q, R as previously defined +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 + +# Compute optimal gain matrix k using LQR +k, _, _ = control.lqr(A, b[:, np.newaxis], Q, R) + +print("Optimal gain matrix k:") +print(k) diff --git a/Chapter7/pythonch7/ex6_4.py b/Chapter7/pythonch7/ex6_4.py new file mode 100644 index 0000000..32c867e --- /dev/null +++ b/Chapter7/pythonch7/ex6_4.py @@ -0,0 +1,28 @@ +import numpy as np + +# Define A, b as given +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]]) # Note: b should be a column vector + +# Calculate controllability matrix C +C = np.hstack([np.linalg.matrix_power(A, i) @ b for i in range(4)]) + +# Define vectors a and alpha +a = np.array([0, -19.6, 0, 0]) +alpha = np.array([12.86, 63.065, 149.38, 157.0]) + +# Define 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 k +k = (alpha - a) @ np.linalg.inv(C @ Psi) + +print("Optimal gain matrix k:") +print(k) diff --git a/Chapter7/pythonch7/ex6_5.py b/Chapter7/pythonch7/ex6_5.py new file mode 100644 index 0000000..a81ce58 --- /dev/null +++ b/Chapter7/pythonch7/ex6_5.py @@ -0,0 +1,28 @@ +import numpy as np + +# Define A, b as given +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]]) # Note: b should be a column vector + +# Calculate controllability matrix C +C = np.hstack([np.linalg.matrix_power(A, i) @ b for i in range(4)]) + +# Define vectors a and alpha +a = np.array([0, -19.6, 0, 0]) +alpha = np.array([12.86, 63.065, 149.38, 157.0]) + +# Define 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 k +k = (alpha - a) @ Psi_1 @ np.linalg.inv(C) + +print("Optimal gain matrix k:") +print(k) diff --git a/Chapter7/pythonch7/ex6_6.py b/Chapter7/pythonch7/ex6_6.py new file mode 100644 index 0000000..84d291d --- /dev/null +++ b/Chapter7/pythonch7/ex6_6.py @@ -0,0 +1,24 @@ +import numpy as np +from scipy.linalg import eig +from control import acker + +# Define A, b as given +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]]) # Ensure b is a column vector + +# Calculate eigenvalues of A +e, _ = eig(A) +print("Eigenvalues of A:") +print(e) + +# Desired poles (desired eigenvalues) +pd = np.array([-4.43, -4.43, -2-2j, -2+2j]) + +# Compute gain matrix k using acker +k = acker(A, b, pd) +print("\nOptimal gain matrix k:") +print(k) diff --git a/Chapter7/pythonch7/ex6_9.py b/Chapter7/pythonch7/ex6_9.py new file mode 100644 index 0000000..eb9f59c --- /dev/null +++ b/Chapter7/pythonch7/ex6_9.py @@ -0,0 +1,50 @@ + +import control +import numpy as np + +# Define the matrices and vectors +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]]) + +# Compute b +b = B @ f + +# Compute the controllability matrix C +def ctrb(A, b): + n = A.shape[0] + C = b + for i in range(1, n): + C = np.hstack((C, np.linalg.matrix_power(A, i) @ b)) + return C + +C = ctrb(A, b) + +# Define Psi and delta +Psi = np.array([[1, 2, -1], [0, 1, 2], [0, 0, 1]]) +delta = np.array([4, 13, 10]).reshape(1, -1) # reshape delta to 1x3 + +# Compute M +M = delta @ np.linalg.inv(C @ Psi) + +print(M) +# Compute K1 +K1 = f @ M +print("K1 =\n", K1) + +pd = [-2, -2, -2] +# Compute K using acker +K = control.acker(A, b, pd) +print("K =\n", K) + +# Compute K2 +K2 = f * K +print("K2 =\n", K2) + +Ac = A - B @ K1 +print("Ac =\n", Ac) + +eigenvalues = np.linalg.eigvals(Ac) +print("Eigenvalues of Ac =\n", eigenvalues) + + diff --git a/Chapter7/pythonch7/fig6_5.py b/Chapter7/pythonch7/fig6_5.py new file mode 100644 index 0000000..b195255 --- /dev/null +++ b/Chapter7/pythonch7/fig6_5.py @@ -0,0 +1,49 @@ +import numpy as np +import matplotlib.pyplot as plt + +# Parameters +k = np.arange(0.02, 5.02, 0.02) +k = k.reshape(-1, 1) +x = np.array([[1], [0]]) + +# Initialize arrays to store results +J = np.zeros((len(k), 4)) + +# Compute J values for different r values +for idx, r in enumerate([0, 1, 2]): + p11 = 0.5 / k + 0.5 + (r + 1) * k / 2 + r * k**2 / 2 + p12 = 0.0 / (5 * k) + r * k / 2 + p22 = 0.5 / k + 0.5 + r * k / 2 + J[:, idx] = p11.flatten() * x[0]**2 + 2 * p12.flatten() * x[0] * x[1] + p22.flatten() * x[1]**2 + +# Plotting +plt.figure(figsize=(10, 5)) + +# Plot J vs k for r = 0, 1, 2 +plt.subplot(1, 2, 1) +plt.plot(k, J[:, 0], 'k', label='r=0', linewidth=2) +plt.plot(k, J[:, 1], '-.', label='r=1', linewidth=2) +plt.plot(k, J[:, 2], '--', label='r=2', linewidth=2) +plt.xlabel('k') +plt.ylabel('J') +plt.grid(True) +plt.legend() + +# Compute J for x = [0, 1] and r = 2 +x = np.array([[0], [1]]) +p11 = 0.5 / k + 0.5 + (2 + 1) * k / 2 + 2 * k**2 / 2 +p12 = 0.0 / (5 * k) + 2 * k / 2 +p22 = 0.5 / k + 0.5 + 2 * k / 2 +J2 = p11.flatten() * x[0]**2 + 2 * p12.flatten() * x[0] * x[1] + p22.flatten() * x[1]**2 + +# Plot J vs k for r = 2 and x = [0, 1] +plt.subplot(1, 2, 2) +plt.plot(k, J[:, 2], 'k', label='r=2', linewidth=2) +plt.plot(k, J2, '-.', label='x=[0, 1]', linewidth=2) +plt.xlabel('k') +plt.ylabel('J') +plt.grid(True) +plt.legend() + +plt.tight_layout() +plt.show() diff --git a/Chapter7/pythonch7/inverted_pendulum_k1.m b/Chapter7/pythonch7/inverted_pendulum_k1.m new file mode 100644 index 0000000..ec48418 --- /dev/null +++ b/Chapter7/pythonch7/inverted_pendulum_k1.m @@ -0,0 +1,24 @@ +% +% Model of Inverted Pendulum +% Copyright Hamid D. Taghirad 2014 +% +function xp=inverted_pendulum_k1(t,x) +% State variable x=[x; v; \theta; \omega] +g=9.8,l=1,m=1;M=1; +d1=M+m*(1-cos(x(3))^2); +d2=l*d1; +k =[-16.0203 -15.2428 -98.6852 -28.1028]; + +F=-k*x % State feedback + +xp=[ + x(2); + (F+m*l*x(4)^2*sin(x(3))-m*g*sin(x(3))*cos(x(3)))/d1; + x(4); + (-F*cos(x(3))-m*l*x(4)^2*sin(x(3))*cos(x(3))+... + (M+m)*g*sin(x(3)))/d2; + ]; +end + + + \ No newline at end of file diff --git a/Chapter7/pythonch7/inverted_pendulum_k2.m b/Chapter7/pythonch7/inverted_pendulum_k2.m new file mode 100644 index 0000000..a3bf96e --- /dev/null +++ b/Chapter7/pythonch7/inverted_pendulum_k2.m @@ -0,0 +1,24 @@ +% +% Model of Inverted Pendulum +% Copyright Hamid D. Taghirad 2014 +% +function xp=inverted_pendulum_k2(t,x); +% State variable x=[x; v; \theta; \omega] +g=9.8,l=1,m=1;M=1; +d1=M+m*(1-cos(x(3))^2); +d2=l*d1; +k =[-40.0000 -37.3693 -190.6669 -54.7283]; + +F=-k*x; % State feedback + +xp=[ + x(2); + (F+m*l*x(4)^2*sin(x(3))-m*g*sin(x(3))*cos(x(3)))/d1; + x(4); + (-F*cos(x(3))-m*l*x(4)^2*sin(x(3))*cos(x(3))+... + (M+m)*g*sin(x(3)))/d2; + ]; +end + + + \ No newline at end of file diff --git a/Chapter7/pythonch7/train_fb.m b/Chapter7/pythonch7/train_fb.m new file mode 100644 index 0000000..62e2229 --- /dev/null +++ b/Chapter7/pythonch7/train_fb.m @@ -0,0 +1,38 @@ + +% +% Model of the train +% Copyright Hamid D. Taghirad 2013 +% +function xp=train_fb(t,x) +% State variable x=[x1 x2 x3 x4 x5 v1 v2 v3 v4 v5]; +A=[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=[0; 0; 0; 0; 0; 0.005; 0; 0; 0; 0]; % Force input +b2=[0; 0; 0; 0; 0; 250; 0; 0; 0; -1250]; % constant input + +vd=25*(1-exp(-t/40)); +% First design R=1/120^2 +% k=[54.5333, 16.2848, -1.3027, -4.3607, 191.7414, ... +% -40.4841, -34.2067, -29.7070, -27.3437, 52.0886]; +% Second Design R=35/120^2 +k=[0.4559, 0.3331, 0.2170, 0.1069, 11.5387, -0.2622, ... + -0.3371, -0.3865, -0.4110, 5.3731]; +dx=[x(2)-20; x(3)-20; x(4)-20; x(5)-20]; +dv=[x(6)-vd; x(7)-vd; x(8)-vd; x(9)-vd; x(10)-vd]; +z=x(6)-vd; +X=[dx;dv;z]; +u=-k*X; +xp=A*x+b1*u+b2; +end + + + \ No newline at end of file diff --git a/Chapter7/pythonch7/train_fb1.m b/Chapter7/pythonch7/train_fb1.m new file mode 100644 index 0000000..0c761c3 --- /dev/null +++ b/Chapter7/pythonch7/train_fb1.m @@ -0,0 +1,34 @@ +% +% Model of the train +% Copyright Hamid D. Taghirad 2013 +% +function xp=train_fb(t,x) +% x= +% [dx2 dx3 dx4 dx5 dv1 dv2 dv3 dv4 dv5 z]; +A=[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 0 + 0 0 0 0 0 0 0 0 0 -1/40 + ]; +B=[0; 0; 0; 0; 0.005; 0; 0; 0; 0; 0]; % Force input +b1=[0; 0; 0; 0; 0.005; 0; 0; 0; 0; 0]; % Force input +b2=[0; 0; 0; 0; 250; 0; 0; 0; 0; -1250]; % constant input +vd=25*(1-exp(-t/40)); +k=[54.5333, 16.2848, -1.3027, -4.3607, 191.7414, ... + -40.4841, -34.2067, -29.7070, -27.3437, 52.0886]; +dx=[x(2)-20; x(3)-20; x(4)-20; x(5)-20]; +dv=[x(6)-vd; x(7)-vd; x(8)-vd; x(9)-vd; x(10)-vd]; +z=x(6)-vd; +X=[dx;dv;z]; +u=k*X; +xp=A*x+b1*u+b2; +end + + + \ No newline at end of file diff --git a/Chapter7/pythonch7/train_fb2.m b/Chapter7/pythonch7/train_fb2.m new file mode 100644 index 0000000..c92d26d --- /dev/null +++ b/Chapter7/pythonch7/train_fb2.m @@ -0,0 +1,33 @@ +% +% Model of the train +% Copyright Hamid D. Taghirad 2013 +% +function xp=train_fb2(t,x) +% State variable x= +% [x1 x2 x3 x4 x5 v1 v2 v3 v4 v5]; +A=[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=[0; 0; 0; 0; 0; 0.005; 0; 0; 0; 0]; % Force input +b2=[0; 0; 0; 0; 0; 250; 0; 0; 0; -1250]; % constant input +vd=25*(1-exp(-t/40)); +k=[54.5333, 16.2848, -1.3027, -4.3607, 191.7414, ... + -40.4841, -34.2067, -29.7070, -27.3437, 52.0886]; +dx=[x(2)-20; x(3)-20; x(4)-20; x(5)-20]; +dv=[x(6)-vd; x(7)-vd; x(8)-vd; x(9)-vd; x(10)-vd]; +z=x(6)-vd; +X=[dx;dv;z]; +u=k*X; +xp=A*x+b1*u+b2; +end + + + \ No newline at end of file diff --git a/Chapter7/pythonch7/train_fb_solver.py b/Chapter7/pythonch7/train_fb_solver.py new file mode 100644 index 0000000..565ee0c --- /dev/null +++ b/Chapter7/pythonch7/train_fb_solver.py @@ -0,0 +1,97 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import odeint + +# Define system parameters +k = np.array([54.5333, 16.2848, -1.3027, -4.3607, 191.7414, + -40.4841, -34.2067, -29.7070, -27.3437]) # Design parameters + +Ks = 2.5e3 # Spring coefficient (KN/m) +Ds = 1.5e2 # Damper coefficient (KN/m/s) + +def train_fb(x, t): + # Define system parameters + vd = 25 * (1 - np.exp(-t / 40)) # Desired velocity function + + # Extract state variables + dx = x[1:5] - 20 # x2-x11 - 20 + dv = x[6:10] - vd # x12-x16 - vd + + # Calculate input force + z = x[6] - vd + X = np.concatenate((dx, dv, [z])) + F = -np.dot(k, X) + Fs = np.array([Ks * dx[0], Ks * dx[3]]) # Spring forces 1 and 4 + + # Calculate damping forces + D1 = Ds * (x[6] - x[7]) + D4 = Ds * (x[8] - x[9]) + + # Return derivatives + return [x[1], F, Fs[0], x[3], Fs[1], x[5], x[7], x[9], D1, D4] + +# Initial conditions and time span +t_span = np.linspace(0, 300, 1000) # from 0 to 300 seconds +x0 = np.array([0, 20, 20, 20, 20, 0, 0, 0, 0, 0]) + +# Solve the differential equation +x = odeint(train_fb, x0, t_span) + +# Extract results for plotting +t = t_span +vd = 25 * (1 - np.exp(-t / 40)) +F = np.zeros_like(t) +Fs1 = np.zeros_like(t) +Fs4 = np.zeros_like(t) +D1 = np.zeros_like(t) +D4 = np.zeros_like(t) + +for i in range(len(t)): + dx = x[i, 1:5] - 20 + dv = x[i, 6:10] - vd[i] + z = x[i, 6] - vd[i] + X = np.concatenate((dx, dv, [z])) + F[i] = -np.dot(k, X) + Fs1[i] = Ks * dx[0] + Fs4[i] = Ks * dx[3] + D1[i] = Ds * (x[i, 6] - x[i, 7]) + D4[i] = Ds * (x[i, 8] - x[i, 9]) + +# Plotting +plt.figure(figsize=(12, 10)) + +# Train position and velocity +plt.subplot(321) +plt.plot(t, x[:, 0] / 1000, 'k') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Train Position (Km)') + +plt.subplot(322) +plt.plot(t, vd, 'k', label='Desired Velocity') +plt.plot(t, x[:, 5], '-.k', label='Real Velocity') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Train Velocity (m)') +plt.legend() + +# Forces +plt.subplot(323) +plt.plot(t, F, 'k', label='Input Force') +plt.plot(t, Fs1, '-.k', label='Spring Force 1') +plt.plot(t, Fs4, '--k', label='Spring Force 4') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Force (KN)') +plt.legend() + +plt.subplot(324) +plt.plot(t, D1, 'k', label='Damping Force 1') +plt.plot(t, D4, '-.k', label='Damping Force 4') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Force (KN)') +plt.legend() + +plt.tight_layout() +plt.show() diff --git a/Chapter7/pythonch7/train_lqr.py b/Chapter7/pythonch7/train_lqr.py new file mode 100644 index 0000000..0a38c21 --- /dev/null +++ b/Chapter7/pythonch7/train_lqr.py @@ -0,0 +1,50 @@ +import numpy as np +from scipy.linalg import solve_continuous_are + +# Define the system matrices +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, 0, 0, 0, 3.75, -3.75, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 0, -1/40]]) + +B = np.array([[0], [0], [0], [0], [0.005], [0], [0], [0], [0], [0]]) + +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 = np.array([[1/120**2]]) + +# Solve the continuous-time Algebraic Riccati equation +P = solve_continuous_are(A, B, Q, R) + +# Compute the control gain K +K = np.linalg.inv(R) @ B.T @ P + +# Print the control gains K +print(K.flatten()) + +# Adjust R1 and solve again +R1 = 35 * R +P1 = solve_continuous_are(A, B, Q, R1) +K1 = np.linalg.inv(R1) @ B.T @ P1 + +# Print the adjusted control gains K1 +print(K1.flatten()) diff --git a/Chapter7/pythonch7/train_model.m b/Chapter7/pythonch7/train_model.m new file mode 100644 index 0000000..42812bb --- /dev/null +++ b/Chapter7/pythonch7/train_model.m @@ -0,0 +1,29 @@ + +% +% Model of the train +% Copyright Hamid D. Taghirad 2013 +% +function xp=train_model(t,x) +% State variable x=[x1 x2 x3 x4 x5 v1 v2 v3 v4 v5]; +A=[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=[0; 0; 0; 0; 0; 0.005; 0; 0; 0; 0]; % Force input +b2=[0; 0; 0; 0; 0; 250; 0; 0; 0; -1250]; % constant input +C=[1 0 0 0 0 0 0 0 0 0]; +D=0; +u=750*exp(-t/10) % exponentially decteasing input +%u=750; % Constant input +xp=A*x+b1*u+b2; +end + + + \ No newline at end of file diff --git a/Chapter7/pythonch7/train_model1.m b/Chapter7/pythonch7/train_model1.m new file mode 100644 index 0000000..e26c894 --- /dev/null +++ b/Chapter7/pythonch7/train_model1.m @@ -0,0 +1,25 @@ +% +% Model of the train +% Copyright Hamid D. Taghirad 2013 +% +function xp=train_model1(t,x) +% State variable x=[x1 x2 x3 x4 x5 v1 v2 v3 v4 v5]; +A=[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=[0; 0; 0; 0; 0.005; 0; 0; 0; 0; 0]; % Force input +b2=[0; 0; 0; 0; 250; 0; 0; 0; 0; -1250]; % constant input +u=750; % Constant input +xp=A*x+b1*u+b2; +end + + + \ No newline at end of file diff --git a/Chapter7/pythonch7/train_solver1.py b/Chapter7/pythonch7/train_solver1.py new file mode 100644 index 0000000..1c8eaed --- /dev/null +++ b/Chapter7/pythonch7/train_solver1.py @@ -0,0 +1,45 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import solve_ivp + +# Define the function train_model1 representing the system of differential equations +def train_model1(t, x): + # Define the parameters and constants used in the system + A = np.array([[0, 1, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 1, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 1, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 1, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]) + + B = np.array([[0], [0], [0], [0], [0], [0], [0], [0], [0], [0]]) + + # Define the differential equations (modify based on your actual equations) + dxdt = A @ x + B.flatten() # Ensure B is flattened to match the shape of x + + return dxdt + +# Define the time span and initial conditions +t_span = [0, 10] +x0 = np.array([0, 20, 20, 20, 20, 0, 0, 0, 0, 0]) + +# Integrate the differential equations using solve_ivp +sol = solve_ivp(train_model1, t_span, x0, method='RK45', dense_output=True) + +# Extract the solution time points and state variables +t = sol.t +x = sol.y + +# Plotting the results +plt.figure(figsize=(10, 6)) +plt.plot(t, x[1, :], 'k', label='x_2') +plt.plot(t, x[4, :], 'k-.', label='x_5') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.legend() +plt.show() From 3491d6ca16c640768b55c44c3c8a5842caa660d9 Mon Sep 17 00:00:00 2001 From: Elnaz MohseniNezhad Date: Sun, 7 Jul 2024 09:59:10 +0330 Subject: [PATCH 4/4] the last chapter --- Chapter8/ch8py/CL_DCmotor_LTR_solver.py | 96 ++++++++++ Chapter8/ch8py/DC_motor_LTR.py | 31 ++++ Chapter8/ch8py/DCmotor_Obs_solver.py | 96 ++++++++++ Chapter8/ch8py/Invpend_Luen_solver.py | 74 ++++++++ Chapter8/ch8py/ReadMe.md | 55 ++++++ Chapter8/ch8py/ex7_1.py | 25 +++ Chapter8/ch8py/train_lqe.py | 37 ++++ Chapter8/ch8py/train_obs_solver1.py | 234 ++++++++++++++++++++++++ Chapter8/ch8py/train_solver.py | 58 ++++++ 9 files changed, 706 insertions(+) create mode 100644 Chapter8/ch8py/CL_DCmotor_LTR_solver.py create mode 100644 Chapter8/ch8py/DC_motor_LTR.py create mode 100644 Chapter8/ch8py/DCmotor_Obs_solver.py create mode 100644 Chapter8/ch8py/Invpend_Luen_solver.py create mode 100644 Chapter8/ch8py/ReadMe.md create mode 100644 Chapter8/ch8py/ex7_1.py create mode 100644 Chapter8/ch8py/train_lqe.py create mode 100644 Chapter8/ch8py/train_obs_solver1.py create mode 100644 Chapter8/ch8py/train_solver.py diff --git a/Chapter8/ch8py/CL_DCmotor_LTR_solver.py b/Chapter8/ch8py/CL_DCmotor_LTR_solver.py new file mode 100644 index 0000000..4275de8 --- /dev/null +++ b/Chapter8/ch8py/CL_DCmotor_LTR_solver.py @@ -0,0 +1,96 @@ +import numpy as np +from scipy.integrate import solve_ivp +import matplotlib.pyplot as plt + +# Define the global parameters +class Parameters: + Tl = 0.01 + +Par = Parameters() + +# Define the DC motor model and observer function +def DC_motor_LTR1(t, X): + global Par + # Model of The Real System + # State variable x=[\theta; \omega; i] + x = X[:3] + A = np.array([[0, 1, 0], [0, 0, 4.438], [0, -12, -24]]) + B = np.array([[0], [0], [20]]) + Bd = np.array([[0], [-7.396], [0]]) + C = np.array([1, 0, 0]) + y = np.dot(C, x) + + # Model of the observer with disturbance Tl=0.01*exp(-t) + # State variable xh=[\theta_h; \omega_h; i_h; Tl_h] + xh = X[3:] + 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]).reshape((4, 1)) + Ch = np.array([1, 0, 0, 0]) + + # State feedback and state observer gains + k = np.array([3.0000, 0.8796, 0.1529, -1.8190]) + G = np.array([-1.0000, 235.7440, -978.1707, -20.4870]).reshape((4, 1)) + + # Final Equations + theta_d = 0 # Desired angular position + Tl = Par.Tl * np.exp(-t) # exponential disturbance + v = -np.dot(k, xh) + u = v + Tl + + xhp = np.dot(Ah, xh) + Bh.flatten() * v + G.flatten() * (y - np.dot(Ch, xh)) + xp = np.dot(A, x) + B.flatten() * v + Bd.flatten() * Tl + return np.concatenate((xp, xhp)) + +# Initial conditions +x0 = [0, 0, 0, 0, 0, 0, 0] +tspan = [0, 5] + +# Solve the differential equations +sol = solve_ivp(DC_motor_LTR1, tspan, x0, max_step=0.01, t_eval=np.arange(0, 5, 0.01)) + +# Extract the solution +t = sol.t +x = sol.y + +# Plot the results +plt.figure(figsize=(12, 8)) + +plt.subplot(221) +plt.plot(t, x[0], 'k', label='θ') +plt.plot(t, x[3], '--k', label='θ_h') +plt.grid() +plt.xlabel('Time (sec)') +plt.ylabel('Angular displacement (rad)') +plt.legend() + +plt.subplot(222) +plt.plot(t, x[1], 'k', label='ω') +plt.plot(t, x[4], '--k', label='ω_h') +plt.grid() +plt.xlabel('Time (sec)') +plt.ylabel('Angular velocity (rad/sec)') +plt.legend() + +plt.subplot(223) +plt.plot(t, x[2], 'k', label='i') +plt.plot(t, x[5], '--k', label='i_h') +plt.grid() +plt.xlabel('Time (sec)') +plt.ylabel('Motor Current (Amp)') +plt.legend() + +Tl = Par.Tl * np.exp(-t) +plt.subplot(224) +plt.plot(t, Tl, 'k', label='Tl') +plt.plot(t, x[6], '--k', label='Tl_h') +plt.grid() +plt.xlabel('Time (sec)') +plt.ylabel('Disturbance torque (N.m)') +plt.legend() + +# Set linewidth of all lines +for line in plt.gcf().findobj(lambda x: isinstance(x, plt.Line2D)): + line.set_linewidth(2) + +plt.tight_layout() +plt.show() diff --git a/Chapter8/ch8py/DC_motor_LTR.py b/Chapter8/ch8py/DC_motor_LTR.py new file mode 100644 index 0000000..f1b2d96 --- /dev/null +++ b/Chapter8/ch8py/DC_motor_LTR.py @@ -0,0 +1,31 @@ +import numpy as np +from scipy.linalg import solve_continuous_are, inv, eig +import control + +# Define the 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]).reshape(-1, 1) +c = np.array([1, 0, 0, 0]).reshape(1, -1) + +# State feedback design +R = np.array([[1]]) +Q1 = np.diag([9, 0, 0, 0]) + +# Calculate the LQR gain +k, _, _ = control.lqr(A, b, Q1, R) + +# State observer design +pd = [-5-5j, -5+5j, -7+7j, -7-7j] + +# Calculate the observer gain +G = control.place(A.T, c.T, pd).T + +print("k =") +print(k) + +print("\nG =") +print(G) diff --git a/Chapter8/ch8py/DCmotor_Obs_solver.py b/Chapter8/ch8py/DCmotor_Obs_solver.py new file mode 100644 index 0000000..4b8f98d --- /dev/null +++ b/Chapter8/ch8py/DCmotor_Obs_solver.py @@ -0,0 +1,96 @@ +import numpy as np +from scipy.integrate import solve_ivp +import matplotlib.pyplot as plt + +# Define the global parameters +class Parameters: + Tl = 1.0 + +Par = Parameters() + +# Define the DC motor model and observer function +def DC_motor_Obs(t, X): + global Par + # Model of The Real System + # State variable x=[\theta; \omega; i] + x = X[:3] + 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]) + Tl = Par.Tl # step disturbance + v = 0 + u = np.array([v, Tl]) + xp = np.dot(A, x) + np.dot(B, u) + y = np.dot(C, x) + + # Model of The observer + # State variable x=[\theta_hat; \omega_hat; i_hat, Tl_hat] + xh = X[3:] + 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]) + G = np.array([0, 234.7440, -936.9136, -27.6050]) + xhp = np.dot(Ah, xh) + Bh * v + np.dot(G, (y - np.dot(Ch, xh))) + + # Augment the real and estimated states + return np.concatenate((xp, xhp)) + +# Initial conditions +x0 = [1, 0, 0, 0, 0, 0, Par.Tl] +tspan = [0, 2] + +# Solve the differential equations +sol = solve_ivp(DC_motor_Obs, tspan, x0, method='RK45', t_eval=np.linspace(0, 2, 100)) + +# Extract the solution +t = sol.t +x = sol.y + +# Plot the results +plt.figure(figsize=(12, 8)) + +plt.subplot(221) +plt.plot(t, x[0], 'k', label='θ') +plt.plot(t, x[3], '--k', label='θ_hat') +plt.grid() +plt.xlabel('Time (sec)') +plt.ylabel('Angular displacement (rad)') +plt.legend() + +plt.subplot(222) +plt.plot(t, x[1], 'k', label='ω') +plt.plot(t, x[4], '--k', label='ω_hat') +plt.grid() +plt.xlabel('Time (sec)') +plt.ylabel('Angular velocity (rad/sec)') +plt.legend() + +plt.subplot(223) +plt.plot(t, x[2], 'k', label='i') +plt.plot(t, x[5], '--k', label='i_hat') +plt.grid() +plt.xlabel('Time (sec)') +plt.ylabel('Motor Current (Amp)') +plt.legend() + +plt.subplot(224) +plt.plot(t, Par.Tl + np.zeros_like(t), 'k', label='Tl') +plt.plot(t, x[6], '--k', label='Tl_hat') +plt.grid() +plt.xlabel('Time (sec)') +plt.ylabel('Disturbance torque (N.m)') +plt.legend() + +# Set linewidth of all lines +for line in plt.gcf().findobj(lambda x: isinstance(x, plt.Line2D)): + line.set_linewidth(2) + +plt.tight_layout() +plt.show() diff --git a/Chapter8/ch8py/Invpend_Luen_solver.py b/Chapter8/ch8py/Invpend_Luen_solver.py new file mode 100644 index 0000000..5efa648 --- /dev/null +++ b/Chapter8/ch8py/Invpend_Luen_solver.py @@ -0,0 +1,74 @@ +import numpy as np +from scipy.integrate import solve_ivp +import matplotlib.pyplot as plt + +# Define the function for the inverted pendulum with Luenberger observer +def inverted_pendulum_luenburger(t, X): + # State variables: x=[x; v; theta; omega], psi + x = X[0:4] + psi = X[4] + + g = 9.8 + l = 1 + m = 1 + M = 1 + + # Constants for dynamics + d1 = M + m * (1 - np.cos(x[2])**2) + d2 = l * d1 + + # State feedback gain + k = [-40.0, -37.3693, -190.6669, -54.7283] + + # Luenberger observer dynamics + dpsi = -40.0 * x[0] - 37.37 * x[1] - 405.9 * x[2] - 58.73 * psi + omega_h = psi + 4 * x[2] + + # Estimated state vector + xh = np.concatenate((x[0:3], [omega_h])) + + # State feedback control law + F = -np.dot(k, x) + # Use the Luenberger observer feedback instead + # F = -np.dot(k, xh) + + # Dynamics of the inverted pendulum + 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 the derivative of the state vector + return np.concatenate((xp, [dpsi])) + +# Initial conditions and time span +x0 = np.array([0, 0, 0.26, 0]) +psi0 = 0 # Initial value of psi + +# Combine initial conditions +X0 = np.concatenate((x0, [psi0])) + +t_span = [0, 3] + +# Solve the ODE system using solve_ivp +sol = solve_ivp(inverted_pendulum_luenburger, t_span, X0, method='RK45', t_eval=np.linspace(t_span[0], t_span[1], 100)) + +# Extract results +t = sol.t +x = sol.y[0:4, :] +psi = sol.y[4, :] +omega = x[3, :] +omega_h = psi + 4 * x[2, :] + +# Plotting results +plt.figure() +plt.plot(t, omega, 'k', label='omega') +plt.plot(t, omega_h, '-.k', label='omega_h') +plt.xlabel('Time (sec)') +plt.ylabel('Angular velocity (rad/sec)') +plt.legend() +plt.grid(True) +plt.show() diff --git a/Chapter8/ch8py/ReadMe.md b/Chapter8/ch8py/ReadMe.md new file mode 100644 index 0000000..73f609f --- /dev/null +++ b/Chapter8/ch8py/ReadMe.md @@ -0,0 +1,55 @@ +# Simulation Models in MATLAB + +This repository contains MATLAB scripts for simulating various mechanical and control systems. Detailed descriptions for each script are provided to help users understand their functions and applications. + +## File Descriptions + +### 📄 [CL_DCmotor_LTR_solver.m](./CL_DCmotor_LTR_solver.m) +- **Purpose**: Simulates a closed-loop DC motor system incorporating an observer, focusing on robust control and state estimation. + +### 📄 [DC_motor_LTR.m](./DC_motor_LTR.m) +- **Purpose**: Provides a model for DC Motor control using LQR-LQE design, aiming at optimal control and state estimation. + +### 📄 [DC_motor_LTR1.m](./DC_motor_LTR1.m) +- **Purpose**: Models a DC motor using ordinary differential equations, focusing on real system dynamics with state variables. + +### 📄 [DC_motor_Obs.m](./DC_motor_Obs.m) +- **Purpose**: Combines a DC motor model with its observer, simulating the system alongside its state estimator. + +### 📄 [DCmotor_Obs_solver.m](./DCmotor_Obs_solver.m) +- **Purpose**: Simulates a DC motor with an observer, focusing on the estimation of internal states that are not directly measurable. + +### 📄 [ex7_1.m](./ex7_1.m) +- **Purpose**: Implements MATLAB code for Example 7-1 from the "Modern Book", likely involving advanced control theory and applications. + +### 📄 [inverted_pendulum_Luenburger.m](./inverted_pendulum_Luenburger.m) +- **Purpose**: Models an inverted pendulum using the Luenburger observer for state estimation, enhancing control accuracy. + +### 📄 [Invpend_Luen_solver.m](./Invpend_Luen_solver.m) +- **Purpose**: Simulates an inverted pendulum with a Luenburger observer, highlighting observer design for improved state estimation. + +### 📄 [train_lqe.m](./train_lqe.m) +- **Purpose**: Designs a Linear Quadratic Estimator (LQE) for a train model, optimizing observer design for accurate state estimation under dynamic conditions. + +### 📄 [train_model_Obs.m](./train_model_Obs.m) +- **Purpose**: Provides a comprehensive model of a train system with an embedded observer, detailing both real system dynamics and observer design. + +### 📄 [train_obs_solver1.m](./train_obs_solver1.m) +- **Purpose**: Simulates the train system using an observer, focusing on state estimation and dynamic response analysis. + +### 📄 [train_solver.m](./train_solver.m) +- **Purpose**: Provides a simulation script for the train model, demonstrating the system's response under various operational scenarios and control inputs. + +## Contribution +Fork this repository to contribute and propose changes or enhancements via pull requests. For major changes, please open an issue first to discuss your proposals. + +## License +Distributed under the MIT License. See `LICENSE` for more information. + +## Contact +- Prof. Hamid D. Taghirad – [Visit Website](https://aras.kntu.ac.ir/taghirad/) +- Project Link: [https://github.com/aras-labs/Modern_Control](https://github.com/aras-labs/Modern_Control) + +![GitHub forks](https://img.shields.io/github/forks/aras-labs/Modern_Control?style=social) +![GitHub stars](https://img.shields.io/github/stars/aras-labs/Modern_Control?style=social) +![GitHub watchers](https://img.shields.io/github/watchers/aras-labs/Modern_Control?style=social) diff --git a/Chapter8/ch8py/ex7_1.py b/Chapter8/ch8py/ex7_1.py new file mode 100644 index 0000000..2d76d76 --- /dev/null +++ b/Chapter8/ch8py/ex7_1.py @@ -0,0 +1,25 @@ + + +import numpy as np +import control + +# Define the system matrices +A = np.array([[0, 1, 0, 0], + [0, 0, 4.438, -7.396], + [0, -12, -24, 0], + [0, 0, 0, 0]]) + +B = np.array([[0], + [0], + [20], + [0]]) + +c = np.array([[1, 0, 0, 0]]) # Ensure c is 2D +pd = np.array([-5+5j, -5-5j, -7+7j, -7-7j]) + +# Calculate the observer gain using pole placement from control library +G = control.place(A.T, c.T, pd) + +print("G =") +print(G) + diff --git a/Chapter8/ch8py/train_lqe.py b/Chapter8/ch8py/train_lqe.py new file mode 100644 index 0000000..2582088 --- /dev/null +++ b/Chapter8/ch8py/train_lqe.py @@ -0,0 +1,37 @@ +import numpy as np +import control as ctrl +import matplotlib.pyplot as plt + +# State variable +# x = [dx2 dx3 dx4 dx5 dv1 dv2 dv3 dv4 dv5]; +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] +]) + +# Observability check +O = np.linalg.matrix_rank(ctrl.obsv(A, C)) +print("Rank of observability matrix:", O) + +# Weight matrices for LQR +W = np.diag([0, 0, 0, 0, 9, 0, 0, 0, 0]) +V = np.diag([1e-2, 1]) + +# LQR design +K, _, _ = ctrl.lqr(A.T, C.T, W, V) + +# Display the result +print("G =") +print(K.T) diff --git a/Chapter8/ch8py/train_obs_solver1.py b/Chapter8/ch8py/train_obs_solver1.py new file mode 100644 index 0000000..9d2b4e2 --- /dev/null +++ b/Chapter8/ch8py/train_obs_solver1.py @@ -0,0 +1,234 @@ +# import numpy as np +# import matplotlib.pyplot as plt +# from scipy.integrate import odeint + +# # Define global parameters +# class Par: +# F = 1000 + +# # Define the model function +# def train_model1(X, t): +# global Par +# # Model 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]) # Force input +# b2 = np.array([0, 0, 0, 0, 0, 250, 0, 0, 0, -1250]) # constant input + +# if t < 10: +# u = Par.F # Constant Force +# uh = 0.5 * u +# else: +# u = 0 +# uh = u + +# xp = np.dot(A, X[:10]) + b1 * u + b2 + +# 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[:10]) +# dy = np.array([y[0] - 20, y[1]]) + +# # Observer model +# xh = X[10:19] +# Ah = 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, -62.5, 0, 0, 0, 3.75] +# ]) +# 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] +# ]) + +# yh = np.dot(Ch, xh) +# 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] +# ]) + +# xhp = np.dot(Ah, xh) + Bh * uh + np.dot(G, (dy - yh)) + +# Xp = np.concatenate((xp, xhp)) + +# return Xp + +# # Initial conditions and time span +# x0 = np.array([0, 20, 20, 20, 20, 0, 0, 0, 0, 0, +# 0, 0, 0, 0, 0, 0, 0, 0, 0]) +# tspan = np.linspace(0, 20, 100) # 100 points from 0 to 20 seconds + +# # Solve the ODE +# sol = odeint(train_model1, x0, tspan) + +# # Extract results for plotting +# x = sol[:, :10] +# xh = sol[:, 10:19] + +# # Plotting +# plt.figure(figsize=(10, 8)) + +# plt.subplot(211) +# plt.plot(tspan, x[:, 1] - 20, 'k', label='Real x_2') +# plt.plot(tspan, xh[:, 0], 'k-.', label='Est x_2') +# plt.grid(True) +# plt.xlabel('Time (sec)') +# plt.ylabel('State variables') +# plt.legend() + +# plt.subplot(212) +# plt.plot(tspan, x[:, 5], 'k', label='Real v_1') +# plt.plot(tspan, xh[:, 4], 'k-.', label='Est v_1') +# plt.grid(True) +# plt.xlabel('Time (sec)') +# plt.ylabel('State variables') +# plt.legend() + +# plt.tight_layout() +# plt.show() +########################### + +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import odeint + +# Define global parameters +class Par: + F = 1000 + +# Define the model function +def train_model1(X, t): + global Par + # Model 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]) # Force input + b2 = np.array([0, 0, 0, 0, 0, 250, 0, 0, 0, -1250]) # constant input + + if t < 10: + u = Par.F # Constant Force + uh = 0.5 * u + else: + u = 0 + uh = u + + xp = np.dot(A, X[:10]) + b1 * u + b2 + + 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[:10]) + dy = np.array([y[0] - 20, y[1]]) + + # Observer model + xh = X[10:19] + Ah = 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, -62.5, 0, 0, 0, 3.75] + ]) + 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] + ]) + + yh = np.dot(Ch, xh) + 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] + ]) + + xhp = np.dot(Ah, xh) + Bh * uh + np.dot(G, (dy - yh)) + + Xp = np.concatenate((xp, xhp)) + + return Xp + +# Initial conditions and time span +x0 = np.array([0, 20, 20, 20, 20, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0]) +tspan = np.linspace(0, 20, 100) # 100 points from 0 to 20 seconds + +# Solve the ODE +sol = odeint(train_model1, x0, tspan) + +# Extract results for plotting +x = sol[:, :10] +xh = sol[:, 10:19] + +# Plotting +plt.figure(figsize=(10, 8)) + +plt.subplot(211) +plt.plot(tspan, x[:, 1] - 20, 'k', label='Real x_2') +plt.plot(tspan, xh[:, 0], 'k-.', label='Est x_2') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.legend() + +plt.subplot(212) +plt.plot(tspan, x[:, 5], 'k', label='Real v_1') +plt.plot(tspan, xh[:, 4], 'k-.', label='Est v_1') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('State variables') +plt.legend() + +plt.tight_layout() +plt.show() diff --git a/Chapter8/ch8py/train_solver.py b/Chapter8/ch8py/train_solver.py new file mode 100644 index 0000000..afdae00 --- /dev/null +++ b/Chapter8/ch8py/train_solver.py @@ -0,0 +1,58 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import odeint + +# Define the model function +def train_model(x, t): + # Model 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]) # Force input + b2 = np.array([0, 0, 0, 0, 0, 250, 0, 0, 0, -1250]) # constant input + + u = 1000 # Constant Force + xp = np.dot(A, x) + b1 * u + b2 + + return xp + +# Initial conditions and time span +x0 = np.array([0, 20, 20, 20, 20, 0, 0, 0, 0, 0]) +tspan = np.linspace(0, 100, 1000) # 1000 points from 0 to 100 seconds + +# Solve the ODE +sol = odeint(train_model, x0, tspan) + +# Extract results for plotting +t = tspan +x = sol[:, :] + +# Plotting +plt.figure(figsize=(10, 8)) + +plt.subplot(211) +plt.plot(t, x[:, 0], 'k') +plt.grid(True) +plt.xlabel('Time (sec)') +plt.ylabel('Locomotive Position (m)') +plt.legend(['x_1']) + +plt.subplot(212) +plt.plot(t, x[:, 1], 'k', label='x_2') +plt.plot(t, 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()