def __init__(self, A, B, Q, R, method='c', x0=None, u0=None, ulb=None, uub=None):
self.A = A
self.B = B
self.Q = Q
self.R = R
if method == 'c':
X, L, G = care(A, B, Q, R)
else:
X, L, G = dare(A, B, Q, R)
if isinstance(G, np.matrix):
G = np.array(G)
# check eigenvalues of A-BK
logger.debug('Eigen values of care are {}'.format(L))
super(ABQRLQR, self).__init__(G, x0, u0, ulb, uub)
def generate_random_sys_and_save_3(m, n):
while True:
print("new")
ss = control.matlab.rss(n, m, m)
ss.D = 100 * np.asmatrix(np.ones((m, m)))
R = ss.D + ss.D.H
ss.A = 10 * ss.A
# ss.B = 1/10 * ss.B
ss.C = 1 / 10 * ss.C
sys = WeightedSystem(ss.A, ss.B, ss.C, ss.D, np.zeros((n, n)), ss.C.H,
R)
X = control.care(ss.A, ss.B, np.zeros((n, n)), R, ss.C.H,
np.identity(n))[0]
alg = get_algorithm_object(sys, 10**(-3))
if check_positivity(-X):
break
sys.save()
def __init__(self):
Td = par.Tp / par.N
# linearize model
X_lin = ca.MX.sym('X_lin', 2, 1)
U_lin = ca.MX.sym('U_lin', 1, 1)
A_c = ca.Function('A_c', \
[X_lin, U_lin], [ca.jacobian(par.ode(X_lin, U_lin), X_lin)])
A_c = A_c([0, 0], 0).full()
B_c = ca.Function('B_c', \
[X_lin, U_lin], [ca.jacobian(par.ode(X_lin, U_lin), U_lin)])
B_c = B_c([0, 0], 0).full()
# solve continuous-time Riccati equation
Qt, e, K = cn.care(A_c, B_c, par.Q, par.R)
# this is the value used in Chen1998!!
# they do not use LQR, but a modified linear controller
# Qt = np.array([[16.5926, 11.5926], [11.5926, 16.5926]])
self.integrator = Integrator(par.ode)
self.x = self.integrator.x
self.u = self.integrator.u
self.Td = Td
# start with an empty NLP
w = []
w0 = []
lbw = []
ubw = []
g = []
lbg = []
ubg = []
Xk = ca.MX.sym('X0', 2, 1)
w += [Xk]
lbw += [0, 0]
ubw += [0, 0]
w0 += [0, 0]
f = 0
# formulate the NLP
for k in range(par.N):
# new NLP variable for the control
Uk = ca.MX.sym('U_' + str(k), 1, 1)
f = f + Td*ca.mtimes(ca.mtimes(Xk.T, par.Q), Xk) \
+ Td*ca.mtimes(ca.mtimes(Uk.T, par.R), Uk)
w += [Uk]
lbw += [-par.umax]
ubw += [par.umax]
w0 += [0.0]
# integrate till the end of the interval
Xk_end = self.integrator.eval(Td, Xk, Uk)
# new NLP variable for state at end of interval
Xk = ca.MX.sym('X_' + str(k + 1), 2, 1)
w += [Xk]
lbw += [-np.inf, -np.inf]
ubw += [np.inf, np.inf]
w0 += [0, 0]
# add equality constraint
g += [Xk_end - Xk]
lbg += [0, 0]
ubg += [0, 0]
f = f + ca.mtimes(ca.mtimes(Xk_end.T, Qt), Xk_end)
g = ca.vertcat(*g)
w = ca.vertcat(*w)
# create an NLP solver
prob = {'f': f, 'x': w, 'g': g}
self.__nlp_solver = ca.nlpsol('solver', 'ipopt', prob)
self.__lbw = lbw
self.__ubw = ubw
self.__lbg = lbg
self.__ubg = ubg
self.__w0 = np.array(w0)
self.__sol_lin = np.array(w0).transpose()
self.__rti_sol = []
self.__nlp_sol = []
# create QP solver
nw = len(w0)
# define linearization point
w_lin = ca.MX.sym('w_lin', nw, 1)
w_qp = ca.MX.sym('w_qp', nw, 1)
# linearized objective = original LLS objective
f_lin = ca.substitute(
f, w, w_qp) + par.alpha * ca.dot(w_qp - w_lin, w_qp - w_lin)
nabla_g = ca.jacobian(g, w).T
g_lin = ca.substitute(g, w, w_lin) + \
ca.mtimes(ca.substitute(nabla_g, w, w_lin).T, w_qp - w_lin)
# create a QP solver
prob = {'f': f_lin, 'x': w_qp, 'g': g_lin, 'p': w_lin}
self.__rti_solver = ca.nlpsol('solver', 'ipopt', prob)
def __init__(self, x_lin=[0, 0], u_lin=0):
Td = par.Tp / par.N
# linearize model
X_lin = ca.MX.sym('X_lin', 2, 1)
U_lin = ca.MX.sym('U_lin', 1, 1)
A_c = ca.Function('A_c', \
[X_lin, U_lin], [ca.jacobian(par.ode(X_lin, U_lin), X_lin)])
A_c = A_c([0, 0], 0).full()
B_c = ca.Function('B_c', \
[X_lin, U_lin], [ca.jacobian(par.ode(X_lin, U_lin), U_lin)])
B_c = B_c([0, 0], 0).full()
# solve continuous-time Riccati equation
Qt, e, K = cn.care(A_c, B_c, par.Q, par.R)
self.integrator = Integrator(par.ode)
self.x = self.integrator.x
self.u = self.integrator.u
self.Td = Td
# start with an empty NLP
w = []
w0 = []
w_lin = []
lbw = []
ubw = []
g = []
lbg = []
ubg = []
Xk = ca.MX.sym('X0', 2, 1)
w += [Xk]
lbw += [0, 0]
ubw += [0, 0]
w0 += [0, 0]
w_lin += x_lin
f = 0
# formulate the NLP
for k in range(par.N):
# new NLP variable for the control
Uk = ca.MX.sym('U_' + str(k), 1, 1)
f = f + Td*ca.mtimes(ca.mtimes(Xk.T, par.Q), Xk) \
+ Td*ca.mtimes(ca.mtimes(Uk.T, par.R), Uk)
w += [Uk]
lbw += [-np.inf]
ubw += [np.inf]
w0 += [0.0]
w_lin += [u_lin]
# integrate till the end of the interval
Xk_end = self.integrator.eval(Td, Xk, Uk)
# new NLP variable for state at end of interval
Xk = ca.MX.sym('X_' + str(k + 1), 2, 1)
w += [Xk]
lbw += [-np.inf, -np.inf]
ubw += [np.inf, np.inf]
w0 += [0, 0]
w_lin += x_lin
# add equality constraint
g += [Xk_end - Xk]
lbg += [0, 0]
ubg += [0, 0]
f = f + ca.mtimes(ca.mtimes(Xk_end.T, Qt), Xk_end)
g = ca.vertcat(*g)
w = ca.vertcat(*w)
# create an NLP solver
self.__lbw = lbw
self.__ubw = ubw
self.__lbg = lbg
self.__ubg = ubg
self.__w0 = np.array(w0)
self.__lqr_sol = []
# create QP solver
nw = len(w0)
# define linearization point
# w_lin = ca.MX.sym('w_lin', nw, 1)
w_qp = ca.MX.sym('w_qp', nw, 1)
# linearized objective = original LLS objective
f_lin = ca.substitute(f, w, w_qp)
nabla_g = ca.jacobian(g, w).T
g_lin = ca.substitute(g, w, w_lin) + \
ca.mtimes(ca.substitute(nabla_g, w, w_lin).T, w_qp - w_lin)
# create a QP solver
prob = {'f': f_lin, 'x': w_qp, 'g': g_lin}
self.__lqr_solver = ca.nlpsol('solver', 'ipopt', prob)
Dp = np.array([[
0,
0,
], [0, 0], [0, 0], [0, 0]])
Q = np.eye(4, 4)
R = np.eye(2, 2)
#S1 = sp.linalg.solve_continuous_are(A, B, Q, R)
#K1 = np.linalg.inv(R).dot(B.T).dot(S1)
#E1 = np.linalg.eigvals(A-B.dot(K1))
#print("S1 =\n", S1)
#print("K1 =\n", K1)
#print("E1 =\n", E1)
S2, E2, K2 = ct.care(Ap, Bp, Q, R)
#print("\nS2 =\n", S2)
#print("K2 =\n", K2)
#print("E2 =\n", E2)
print "ctrb = " + str(np.linalg.matrix_rank(ct.ctrb(Ap, Bp)))
print "obsv = " + str(np.linalg.matrix_rank(ct.obsv(Ap, Cp)))
#K3, S3, E3 = ct.matlab.lqr(Ap, Bp, Q, R)
#print("\nS3 =\n", S3)
#print("K3 =\n", K3)
#print("E3 =\n", E3)
ss_P = ct.ss(Ap, Bp, Cp, Dp)
print ss_P
t = np.linspace(0, 3, 1000)
stepy = ct.step(ss_P, t)
#plt.plot(t,stepy)
def LQR(A_ss, B_Ss, Q, R):
SLG = cn.care(A_ss, B_ss, Q, R)
delta_u = -mtimes(G, X_est[0:4] - xbar_tilde)
u1 = np.add(ubar, delta_u)
return u1
delta_u_est = [0, 0]
#plotting initilization
s_plot = np.append(s_plot, X_true[0])
n_plot = np.append(n_plot, X_true[1])
alpha_plot = np.append(alpha_plot, delta_x_est[2])
velocity_plot = np.append(velocity_plot, delta_x_est[3])
s_plot_est = np.append(s_plot_est, delta_x_est[0])
n_plot_est = np.append(n_plot_est, delta_x_est[1])
alpha_plot_est = np.append(alpha_plot_est, delta_x_est[2])
velocity_plot_est = np.append(velocity_plot_est, delta_x_est[3])
A_ss = np.array(A_ss)
B_ss = np.array(B_ss)
#sys=cn.ss(A_ss,B_ss,C,D)
S, L, G = cn.care(A_ss, B_ss, Q, R)
def plant_dyn(X_true, u, A, P_model_cov_est):
P_model_cov_true=mtimes(A, mtimes(P_model_cov_est, \
transpose(A))) + W_cov
X_true = integrator_fun(X_true, u) + wk
#print(X_true)
return (X_true, P_model_cov_true)
def observer_dyn(P_model_cov_true, X_est, u):
P_model_cov_est=inv(inv(P_model_cov_true)+\
mtimes(mtimes(transpose(C),inv(V_cov)),C))
X_est = integrator_fun(X_est, u)
# coding: utf-8
import numpy as np
import scipy as sp
import control as ct
A = np.array([[0, 1], [0, -1]])
B = np.array([[0], [1]])
Q = np.diag([2, 1])
R = np.array([[1]])
S1 = sp.linalg.solve_continuous_are(A, B, Q, R)
K1 = np.linalg.inv(R).dot(B.T).dot(S1)
E1 = np.linalg.eigvals(A - B.dot(K1))
print("S1 =\n", S1)
print("K1 =\n", K1)
print("E1 =\n", E1)
S2, E2, K2 = ct.care(A, B, Q, R)
print("\nS2 =\n", S2)
print("K2 =\n", K2)
print("E2 =\n", E2)
K3, S3, E3 = ct.matlab.lqr(A, B, Q, R)
print("\nS3 =\n", S3)
print("K3 =\n", K3)
print("E3 =\n", E3)
Dp = np.array([[ 0, 0,],
[0, 0],
[0, 0],
[0, 0]])
Q = np.eye(4,4)
R = np.eye(2,2)
#S1 = sp.linalg.solve_continuous_are(A, B, Q, R)
#K1 = np.linalg.inv(R).dot(B.T).dot(S1)
#E1 = np.linalg.eigvals(A-B.dot(K1))
#print("S1 =\n", S1)
#print("K1 =\n", K1)
#print("E1 =\n", E1)
S2, E2, K2 = ct.care(Ap, Bp, Q, R)
#print("\nS2 =\n", S2)
#print("K2 =\n", K2)
#print("E2 =\n", E2)
print "ctrb = "+ str(np.linalg.matrix_rank(ct.ctrb(Ap,Bp)))
print "obsv = " + str(np.linalg.matrix_rank(ct.obsv(Ap,Cp)))
#K3, S3, E3 = ct.matlab.lqr(Ap, Bp, Q, R)
#print("\nS3 =\n", S3)
#print("K3 =\n", K3)
#print("E3 =\n", E3)
ss_P = ct.ss(Ap,Bp,Cp,Dp)
print ss_P
t = np.linspace(0, 3, 1000)
stepy = ct.step(ss_P,t)
#plt.plot(t,stepy)
def __init__(self, sysApprox, freqsReal, alpha1, alpha2, R1, R2, Q0, Q1,
Q2, ROMorder):
'''
Parameters
----------
sysApprox : LinearSystem
The Galerkin approximation (A^N,B^N,C^N,D) of the control system for the controller design.
freqsReal : (, N) array_like
The (real) frequencies (w_k)_{k=0}^q of the reference and disturbance signals.
alpha1, alpha2 : integer
Design parameters to determine the closed-loop stability margin. Required to be positive.
R1, R2, Q0, Q1, Q2 : integer/(N1, M1) array_like
All these matrices should be chosen to be invertible and positive definite.
(the dimension Q_0 can be computed with the routine IMdim(freqsReal,p)).
Q1 and Q2 correspond to the Galerkin approximations (compatible with sysApprox).
They can be chosen to correspond to the Galerkin approximations of Q_1=q_1*I and Q_2=q_2*I for some q_1,q_2>0.
ROMorder
order of the reduced-order observer in the controller.
The model reduction step can be skipped by setting 'ROMorder=None'
'''
AN = sysApprox.A
BN = sysApprox.B
CN = sysApprox.C
D = sysApprox.D
dim_X = AN.shape[0]
dim_Y = CN.shape[0]
dim_U = BN.shape[1]
# Check the consistency of the controller design parameters
alpha1_check = np.isreal(alpha1) and np.ndim(
alpha1) == 0 and alpha1 > 0
alpha2_check = np.isreal(alpha2) and np.ndim(
alpha2) == 0 and alpha2 > 0
R1_check = np.allclose(
R1,
np.conj(R1).T) and np.all(np.linalg.eigvals(R1) > 0)
R2_check = np.allclose(
R2,
np.conj(R2).T) and np.all(np.linalg.eigvals(R2) > 0)
if not alpha1_check or not alpha2_check:
raise Exception('"alpha1" and "alpha2" need to be positive!')
elif not R1_check or not R2_check:
raise Exception('"R1" and "R2" need to be positive definite')
# Construct the internal model
cG1, cG2 = construct_internal_model(freqsReal, dim_Y)
dim_Z0 = cG1.shape[0]
# If R1, R2, Q0, Q1, or Q2 have scalar values, these are interpreted as
# "scalar times identity".
if dim_Y > 1 and np.ndim(R1) == 0:
R1 = R1 * np.eye(dim_Y)
if dim_Y > 1 and np.ndim(R2) == 0:
R2 = R2 * np.eye(dim_Y)
if dim_Z0 > 1 and np.ndim(Q0) == 0:
Q0 = Q0 * np.eye(dim_Z0)
if dim_X > 1 and np.ndim(Q1) == 0:
Q1 = Q1 * np.eye(dim_X)
if dim_X > 1 and np.ndim(Q2) == 0:
Q2 = Q2 * np.eye(dim_X)
# Check the consistency of the dimensions of R1, R2, Q0, Q1, and Q2
if not R1.shape == (dim_Y, dim_Y):
raise Exception(
'Dimensions of "R1" are incorrect! (should be [dimY,dimY]).')
if not R2.shape == (dim_U, dim_U):
raise Exception(
'Dimensions of "R1" are incorrect! (should be [dimU,dimU]).')
if not Q0.shape[1] == dim_Z0:
raise Exception('Dimensions of "Q0" are incorrect!')
if not Q1.shape[0] == dim_X:
raise Exception('Dimensions of "Q1" are incorrect!')
if not Q2.shape[1] == dim_X:
raise Exception('Dimensions of "Q2" are incorrect!')
# Form the extended system (As,Bs)
As = np.bmat([[cG1, np.dot(cG2, CN)], [np.zeros((dim_X, dim_Z0)), AN]])
Bs = np.bmat([[np.dot(cG2, D)], [BN]])
Qs = sp.linalg.block_diag(Q0, Q2)
# Stabilize the pairs (CN,AN+alpha1) and (As+alpha2,Bs) using LQR/LQG design
# Stabilize the pair (CN,AN+alpha1)
t1 = time.time()
B_ext = np.bmat([[np.conj(CN).T, np.zeros((dim_X, dim_X))]])
S_ext = np.bmat([[np.zeros((dim_X, dim_Y)), Q1]])
R1_ext = scipy.linalg.block_diag(R1, -np.eye(dim_X))
# Using the control library:
q = np.zeros(((AN + alpha1 * np.eye(dim_X)).shape[0],
(AN + alpha1 * np.eye(dim_X)).shape[1]))
e = np.eye(q.shape[0], q.shape[1])
X1, evals, L_ext_adj = ctrl.care(A=(AN +
alpha1 * np.eye(dim_X)).conj().T,
B=B_ext,
Q=q,
R=R1_ext,
S=S_ext,
E=e)
L = -np.conj(L_ext_adj[0:dim_Y, :]).T
t2 = time.time()
t = t2 - t1
print('Stabilization of the pair (CN,AN+alpha1) took %f seconds' % t)
if np.amax(np.real(evals)) >= 0:
raise Exception('Stabilization of the pair (CN,AN+alpha1) failed!')
# Stabilize the pair (As+alpha2,Bs)
t3 = time.time()
Bs_ext = np.bmat([[Bs, np.zeros((dim_Z0 + dim_X, dim_Z0 + dim_X))]])
Ss_ext = np.bmat([[np.zeros((dim_Z0 + dim_X, dim_Y)), np.conj(Qs).T]])
R2_ext = scipy.linalg.block_diag(R2, -np.eye(dim_Z0 + dim_X))
# Using the control library:
qs = np.zeros(((As + alpha2 * np.eye(dim_Z0 + dim_X)).shape[0],
(As + alpha2 * np.eye(dim_Z0 + dim_X)).shape[1]))
es = np.eye(qs.shape[0], qs.shape[1])
X2, evals, K_ext = ctrl.care(A=(As + alpha2 * np.eye(dim_Z0 + dim_X)),
B=Bs_ext,
Q=qs,
R=R2_ext,
S=Ss_ext,
E=es)
K = -K_ext[0:dim_U, :]
t4 = time.time()
t = t4 - t3
print('Stabilization of the pair (As+alpha2,Bs) took %f seconds' % t)
if np.amax(np.real(evals)) >= 0:
raise Exception('Stabilization of the pair (As+alpha2,Bs) failed!')
# Decompose the control gain K into K=[K1N,K2N]
K1N = K[:, 0:dim_Z0]
K2N = K[:, dim_Z0:]
# Complete the model reduction step of the controller design. If the model
# reduction fails (for example due to too high reduction order), the
# controller design is by default completed without the model reduction.
if ROMorder is not None:
try:
t5 = time.time()
rsys = ctrl.balred(ctrl.ss(AN + np.dot(L, CN),
np.bmat([[BN + np.dot(L, D),
L]]), K2N,
np.zeros((dim_U, dim_U + dim_Y))),
ROMorder,
method='truncate')
t6 = time.time()
t = t6 - t5
print('Model reduction step took %f seconds' % t)
ALr = rsys.A
Br_full = rsys.B
BLr = Br_full[:, 0:dim_U]
Lr = Br_full[:, dim_U:]
K2r = rsys.C
except:
print(
'Model reduction step failed! Modify "ROMorder", or check that "balred" is available. Proceeding without model reduction (with option "ROMorder=None")'
)
ALr = AN + np.dot(L, CN)
BLr = BN + np.dot(L, D)
Lr = L
K2r = K2N
else:
print('Constructing the controller without model reduction.')
ALr = AN + np.dot(L, CN)
BLr = BN + np.dot(L, D)
Lr = L
K2r = K2N
# Construct the controller matrices (G1,G2,K).
G1 = np.bmat([[cG1, np.zeros((dim_Z0, ALr.shape[0]))],
[np.dot(BLr, K1N), ALr + np.dot(BLr, K2r)]])
G2 = np.bmat([[cG2], [-Lr]])
K = np.bmat([[K1N, K2r]])
Dc = np.zeros((dim_U, dim_Y))
Controller.__init__(self, G1, G2, K, Dc, 1.0)
def simulateClosedLoop(self):
n_state = 4;
n_control = 2;
theta_offset = np.deg2rad(20); # this is a manual (optional) offset in thetas
# that trims them to be in a V-shape, which is intuitively more stable in the
# degrees of freedom not modeled here. Can be set to 0 and dynamics still work.
q_abs_init = [np.deg2rad(0), np.deg2rad(0), theta_offset, theta_offset];
# State: q = [phi; phi_dot; theta1; theta2] (radians, radians/sec)
# Control: u = [theta1dot; theta2dot] (radians/sec)
# dynamics is evolved as errors off the reference (which has to be an equilbrium; namely d/dt q_ref = 0)
# (q - q_ref) = qbar
# (u - u_ref) = ubar
# i.e. d/dt (q-q_ref) = A (q-q_ref) + B (u-u_ref)
# and then at the end the states are offset by the reference values to get
# the true values.
A = np.array([[0, 1, 0, 0],
[1/self.J*self.dmz_dphi, 0, 1/self.J*self.dmz_dtheta1, 1/self.J*self.dmz_dtheta2],
[0, 0, 0, 0],
[0, 0, 0, 0]], dtype = 'float64');
B = np.array([[0, 0],
[0, 0],
[1, 0],
[0, 1]], dtype = 'float64');
# LQR weighting matrices (note the R matrix is a cost on slew rates of
# the actuators, while the actual flap angles are in the Q matrix)
Q = 1*np.diag([30, 15, 1, 1])
R = 1*np.diag([1, 1])
# solve the infinite-horizon continuous algebraic riccati eqn for X and then solve for K
(X, L, G) = control.care(A, B, Q, R);
K = np.linalg.inv(R) @ np.transpose(B) @ X
sim_time = 20; # seconds
self.dt = .1; # seconds
self.time_vector = np.linspace(0, sim_time, int(sim_time/self.dt)+1)
qbar_evolution = np.zeros((n_state, len(self.time_vector))); # radians, radians/sec
ubar_evolution = np.zeros((n_control, len(self.time_vector))); # radians
ubar_evolution[:, -1] = [np.nan, np.nan] # control array is one index smaller
self.q_evolution = np.zeros((n_state, len(self.time_vector))); # absolute state
self.u_evolution = np.zeros((n_control, len(self.time_vector))); # absolute control values
self.u_evolution[:, -1] = [np.nan, np.nan] # control array is one index smaller
self.q_evolution[:, 0] = q_abs_init;
# Set a time-varying reference
self.q_ref = np.zeros((n_state, len(self.time_vector)))
# set body roll references for n distinct periods (can make them all equal if you want)
roll_refs = np.deg2rad([0, 15, 15, 0, 0, 30, 30, 0, 0]);
roll_refs_expanded = [ref for ref in roll_refs for i in range(int(1+len(self.time_vector)/len(roll_refs)))]
self.q_ref[0, :] = roll_refs_expanded[0:len(self.time_vector)]
# body roll rate reference is always 0
self.q_ref[1, :] = 0;
# solve for u_ref (feedforward)
for i in range(len(self.time_vector)):
# solve for the least-norm flap equilibrium angles that can offset aero torque due to the
# body roll reference angle. This amounts to our feed forward command.
self.q_ref[2, i] = -self.dmz_dphi*self.dmz_dtheta1*self.q_ref[0, i]/(self.dmz_dtheta1**2 + self.dmz_dtheta2**2) + theta_offset;
self.q_ref[3, i] = -self.dmz_dphi*self.dmz_dtheta2*self.q_ref[0, i]/(self.dmz_dtheta1**2 + self.dmz_dtheta2**2) + theta_offset;
# Set the initial conditions
qbar_evolution[:, 0] = q_abs_init - np.ravel(self.q_ref[:, 0]);
# Simulate closed-loop dynamics
nonlinearAero = True; # if True, will use real-time interpolation of the CFD nonlinear functions
for i in range(len(self.time_vector) - 1):
q_curr = self.q_evolution[:, i].reshape(n_state, 1);
qbar_curr = q_curr - self.q_ref[:, i].reshape(n_state, 1);
ubar_curr = -K @ qbar_curr
u_curr = ubar_curr; # u_ref == 0 so we don't have to offset anything here
if(nonlinearAero == True):
delta = 1; # deg, for finite differencing
phi_curr_deg = np.rad2deg(q_curr[0, 0]);
theta1_curr_deg = np.rad2deg(q_curr[2, 0]);
theta2_curr_deg = np.rad2deg(q_curr[3, 0]);
dmz_dphi_local = (self.mz_interp_func([phi_curr_deg + delta, theta1_curr_deg, theta2_curr_deg]) - self.mz_interp_func([phi_curr_deg, theta1_curr_deg, theta2_curr_deg]))/delta;
dmz_dtheta1_local = (self.mz_interp_func([phi_curr_deg, theta1_curr_deg + delta, theta2_curr_deg]) - self.mz_interp_func([phi_curr_deg, theta1_curr_deg, theta2_curr_deg]))/delta;
dmz_dtheta2_local = (self.mz_interp_func([phi_curr_deg, theta1_curr_deg, theta2_curr_deg + delta]) - self.mz_interp_func([phi_curr_deg, theta1_curr_deg, theta2_curr_deg]))/delta;
avg_dtheta_local = (np.abs(dmz_dtheta1_local) + np.abs(dmz_dtheta2_local))/2;
dmz_dtheta1_local = np.sign(dmz_dtheta1_local)*avg_dtheta_local;
dmz_dtheta2_local = np.sign(dmz_dtheta2_local)*avg_dtheta_local;
dmz_dphi_local = dmz_dphi_local[0]*180/np.pi;
dmz_dtheta1_local = dmz_dtheta1_local[0]*180/np.pi;
dmz_dtheta2_local = dmz_dtheta2_local[0]*180/np.pi;
# update our locally-linear A and B matrices for simulation
A = np.array([[0, 1, 0, 0],
[1/self.J*dmz_dphi_local, 0, 1/self.J*dmz_dtheta1_local, 1/self.J*dmz_dtheta2_local],
[0, 0, 0, 0],
[0, 0, 0, 0]], dtype = 'float64');
B = np.array([[0, 0],
[0, 0],
[1, 0],
[0, 1]], dtype = 'float64');
qbar_next = qbar_curr + self.dt*(A @ qbar_curr + B @ ubar_curr)
q_next = qbar_next + self.q_ref[:, i].reshape(n_state, 1);
self.q_evolution[:, i+1] = np.ravel(q_next);
self.u_evolution[:, i] = np.ravel(u_curr);
qbar_evolution[:, i+1] = np.ravel(qbar_next);
ubar_evolution[:, i] = np.ravel(ubar_curr);
#self.q_evolution[:, -1] = np.ravel(qbar_evolution[:, -1]) + np.ravel(self.q_ref[:, -1])
fig1, ax1 = plt.subplots(1, 2, figsize=(12, 6));
ax1[0].plot(self.time_vector, np.rad2deg(self.q_evolution[0, :]), label = 'phi');
ax1[0].plot(self.time_vector, np.rad2deg(self.q_evolution[1, :]), label = 'phi_dot');
ax1[0].step(self.time_vector, np.rad2deg(self.q_ref[0, :]), 'k--', alpha = .5, label = 'phi ref');
ax1[0].set_xlabel("Time [s]");
ax1[0].set_ylabel("Body Roll, Roll Rate [deg, deg/s]");
ax1[0].set_title("State Variables");
ax1[0].legend(loc = 'best')
ax1[1].plot(self.time_vector, np.rad2deg(self.q_evolution[2, :]), label = "theta1");
ax1[1].plot(self.time_vector, np.rad2deg(self.q_evolution[3, :]), label = "theta2");
ax1[1].set_xlabel("Time [s]");
ax1[1].set_ylabel("Flap Angle [deg]");
ax1[1].set_title("Control Variables");
ax1[1].legend(loc = 'best')
fig1.suptitle("Attitude Reference Tracking, Air Speed = 10 m/s (+y)")
