I have created a MRAC program for a helicopter pitch dynamics system. The example I am trying to code is example 9.1 from Lavretsky's "Robust and Adaptive Control with Aerospace Applications" if that helps. When the program runs, q is displayed as horizontal line and does not match the reference function. Can someone explain where the error is?
import numpy as np
from scipy.integrate import odeint
import matplotlib.pyplot as plt
# Define the system dynamics
def q_dot(q, k, kcmd, theta, qref, t):
M_q = -0.61
M_delta = -6.65
theta_val = -0.01
f_q = theta_val * np.tanh(360 / np.pi * q)
delta = k * q + kcmd * qcmd - np.dot(theta, phi(q))
q_dot = M_q * q + M_delta * (delta + f_q)
return q_dot
def k_dot(q, k, kcmd, theta, qref, t):
gamma = 6000
qref_dot = 4 * (reference_function(t) - qref)
k_dot = gamma * q * (q - qref)
return k_dot
def kcmd_dot(q, k, kcmd, theta, qref, t):
gammacmd = 6000
qref_dot = 4 * (reference_function(t) - qref)
kcmd_dot = gammacmd * qcmd * (q - qref)
return kcmd_dot
def theta_dot(q, k, kcmd, theta, qref, t):
Gamma = 8
qref_dot = 4 * (reference_function(t) - qref)
theta_dot = -Gamma * phi(q) * (q - qref)
return theta_dot
def qref_dot(q, k, kcmd, theta, qref, t):
qref_dot = 4 * (reference_function(t) - qref)
return qref_dot
# Define the regressor phi(q)
def phi(q):
return np.tanh(360 / np.pi * q)
# Define the reference function
def reference_function(t):
return 4 * np.sin(t) * np.cos(t)**3
# Set up initial conditions and time points
initial_conditions = [0, 0, 0, 0, 0]
t = np.linspace(0, 10, 1000) # Specify the desired time range
# Set up parameters
gamma = 6000
gammacmd = 6000
Gamma = 8
qcmd = 4
# Integrate the system for each function separately
q = odeint(q_dot, initial_conditions[0], t, args=(initial_conditions[1], initial_conditions[2], initial_conditions[3], initial_conditions[4]))
k = odeint(k_dot, initial_conditions[1], t, args=(initial_conditions[0], initial_conditions[2], initial_conditions[3], initial_conditions[4]))
kcmd = odeint(kcmd_dot, initial_conditions[2], t, args=(initial_conditions[0], initial_conditions[1], initial_conditions[3], initial_conditions[4]))
theta = odeint(theta_dot, initial_conditions[3], t, args=(initial_conditions[0], initial_conditions[1], initial_conditions[2], initial_conditions[4]))
qref = odeint(qref_dot, initial_conditions[4], t, args=(initial_conditions[0], initial_conditions[1], initial_conditions[2], initial_conditions[3]))
# Get the reference values
ref_values = reference_function(t)
# Plot the components
plt.figure(figsize=(12, 8))
plt.plot(t, q, label='q')
plt.plot(t, ref_values, label='Reference')
plt.xlabel('Time')
plt.ylabel('q')
plt.grid(True)
plt.legend()
plt.show()
I think it might be in the q_dot dynamics function, but I can not figure out where. I also tried vectorizing and calculating the state derivative that way which seemed to work, but I would still try to like know the error in this code.