MRAC Program Output Not Matching Reference Function

26 Views Asked by At

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.

0

There are 0 best solutions below