def compute_trim(mav, Va, gamma):
# define initial state and input
e0 = Euler2Quaternion(0., gamma, 0.)
state0 = np.array([[], # pn
[], # pe
[], # pd
[], # u
[], # v
[], # w
[], # e0
[], # e1
[], # e2
[], # e3
[], # p
[], # q
[] # r
])
delta0 = np.array([[], # delta_e
[], # delta_a
[], # delta_r
[] # delta_t
])
x0 = np.concatenate((state0, delta0), axis=0)
# define equality constraints
cons = ({'type': 'eq',
'fun': lambda x: np.array([
x[3]**2 + x[4]**2 + x[5]**2 - Va**2, # magnitude of velocity vector is Va
x[4], # v=0, force side velocity to be zero
x[6]**2 + x[7]**2 + x[8]**2 + x[9]**2 - 1., # force quaternion to be unit length
x[7], # e1=0 - forcing e1=e3=0 ensures zero roll and zero yaw in trim
x[9], # e3=0
x[10], # p=0 - angular rates should all be zero
x[11], # q=0
x[12], # r=0
]),
'jac': lambda x: np.array([
[0., 0., 0., 2*x[3], 2*x[4], 2*x[5], 0., 0., 0., 0., 0., 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., 2*x[6], 2*x[7], 2*x[8], 2*x[9], 0., 0., 0., 0., 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., 1., 0., 0., 0., 0., 0., 0., 0.],
[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., 1., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.],
])
})
# solve the minimization problem to find the trim states and inputs
res = minimize(trim_objective_fun, x0, method='SLSQP', args=(mav, Va, gamma),
constraints=cons, options={'ftol': 1e-10, 'disp': True})
# extract trim state and input and return
trim_state = np.array([res.x[0:13]]).T
trim_input = np.array([res.x[13:17]]).T
print('trim_state=', trim_state.T)
print('trim_input=', trim_input.T)
return trim_state, trim_input
# Initial conditions for MAV pn0 = 0. # initial north position pe0 = 0. # initial east position pd0 = -100.0 # initial down position u0 = 25. # initial velocity along body x-axis v0 = 0. # initial velocity along body y-axis w0 = 0. # initial velocity along body z-axis phi0 = 0. # initial roll angle theta0 = 0. # initial pitch angle psi0 = 0.0 # initial yaw angle p0 = 0 # initial roll rate q0 = 0 # initial pitch rate r0 = 0 # initial yaw rate Va0 = np.sqrt(u0**2 + v0**2 + w0**2) # Quaternion State e = Euler2Quaternion(phi0, theta0, psi0) e0 = e.item(0) e1 = e.item(1) e2 = e.item(2) e3 = e.item(3) ###################################################################################### # Physical Parameters ###################################################################################### mass = 11. #kg Jx = 0.8244 #kg m^2 Jy = 1.135 Jz = 1.759 Jxz = 0.1204 S_wing = 0.55 b = 2.8956
import sys
import numpy as np
sys.path.append('../../')
from tools.rotations import Euler2Quaternion
u = 45
v = -2
w = 6
phi = np.radians(5)
theta = np.radians(10)
psi = np.radians(-27)
Vw = np.array([5, -1, 0])
quat = Euler2Quaternion(phi, theta, psi).squeeze()
R1 = np.array([[np.cos(psi), np.sin(psi), 0], [-np.sin(psi),
np.cos(psi), 0], [0, 0, 1]])
R2 = np.array([[np.cos(theta), 0, -np.sin(theta)], [0, 1, 0],
[np.sin(theta), 0, np.cos(theta)]])
R3 = np.array([[1, 0, 0], [0, np.cos(phi), np.sin(phi)],
[0, -np.sin(phi), np.cos(phi)]])
vb = np.array([u, v, w])
v2 = R3.T @ vb
v1 = R2.T @ v2
Vg = R1.T @ v1
Vab = Vg - Vw
Va = np.linalg.norm(Vab)
def compute_trim(mav, Va, gamma):
# define initial state and input
e = Euler2Quaternion([0, gamma, 0])
state0 = np.array([
0, # (0)
0, # (1)
-100, # (2)
Va, # (3)
0, # (4)
0, # (5)
e[0], # (6)
e[1], # (7)
e[2], # (8)
e[3], # (9)
0, # (10)
0, # (11)
0
]) # (12)
delta_e = 0
delta_a = 0
delta_r = 0
delta_t = 0.5
delta0 = np.array([delta_e, delta_a, delta_r, delta_t])
x0 = np.concatenate((state0, delta0), axis=0)
# define equality constraints
cons = ({
'type':
'eq',
'fun':
lambda x: np.array([
x[3]**2 + x[4]**2 + x[
5]**2 - Va**2, # magnitude of velocity vector is Va
x[4], # v=0, force side velocity to be zero
x[6]**2 + x[7]**2 + x[8]**2 + x[9]**2 -
1., # force quaternion to be unit length
x[
7
], # e1=0 - forcing e1=e3=0 ensures zero roll and zero yaw in trim
x[9], # e3=0
x[10], # p=0 - angular rates should all be zero
x[11], # q=0
x[12], # r=0
]),
'jac':
lambda x: np.array([
[
0., 0., 0., 2 * x[3], 2 * x[4], 2 * x[5], 0., 0., 0., 0., 0.,
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., 2 * x[6], 2 * x[7], 2 * x[8], 2 * x[9],
0., 0., 0., 0., 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., 1., 0., 0., 0., 0., 0., 0.,
0.
],
[
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., 1., 0., 0., 0., 0.,
0.
],
[
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0.,
0.
],
])
})
# solve the minimization problem to find the trim states and inputs
res = minimize(trim_objective,
x0,
method='SLSQP',
args=(mav, Va, gamma),
constraints=cons,
options={
'ftol': 1e-10,
'disp': True
})
# extract trim state and input and return
trim_state = np.array(res.x[0:13])
trim_input = np.array(res.x[13:17])
return trim_state, trim_input
# Initial conditions for MAV pn0 = 0. # initial north position pe0 = 0. # initial east position pd0 = -100.0 # initial down position u0 = 20. # initial velocity along body x-axis v0 = 0. # initial velocity along body y-axis w0 = 0. # initial velocity along body z-axis phi0 = 0. # initial roll angle theta0 = 0.0 # initial pitch angle psi0 = 0.0 # initial yaw angle p0 = 0 # initial roll rate q0 = 0 # initial pitch rate r0 = 0 # initial yaw rate Va0 = np.sqrt(u0**2 + v0**2 + w0**2) # Quaternion State e = Euler2Quaternion([phi0, theta0, psi0]) e0 = e.item(0) e1 = e.item(1) e2 = e.item(2) e3 = e.item(3) ###################################################################################### # Physical Parameters ###################################################################################### mass = 11 #kg Jx = 0.8244 #kg m^2 Jy = 1.135 Jz = 1.759 Jxz = 0.1204 S_wing = 0.55 b = 2.8956
# Initial conditions for MAV pn0 = 0. # initial north position pe0 = 0. # initial east position pd0 = -100.0 # initial down position u0 = 25. # initial velocity along body x-axis v0 = 0. # initial velocity along body y-axis w0 = 0. # initial velocity along body z-axis phi0 = 0. # initial roll angle theta0 = 0. # initial pitch angle psi0 = 0.0 # initial yaw angle p0 = 0 # initial roll rate q0 = 0 # initial pitch rate r0 = 0 # initial yaw rate Va0 = np.sqrt(u0**2 + v0**2 + w0**2) # Quaternion State e0, e1, e2, e3 = Euler2Quaternion(phi0, theta0, psi0) ###################################################################################### # Physical Parameters ###################################################################################### mass = 11.0 #kg Jx = 0.8244 #kg m^2 Jy = 1.135 Jz = 1.759 Jxz = 0.1204 S_wing = 0.55 b = 2.8956 c = 0.18994 S_prop = 0.2027 rho = 1.2682 k_motor = 80
def compute_trim(mav, Va, gamma):
# define initial state and input
e0 = Euler2Quaternion(0., gamma, 0.)
state0 = np.array([
[0.], # pn
[0.], # pe
[0.], # pd
[Va], # u
[0.], # v
[0.], # w
[e0.item(0)], # e0
[e0.item(1)], # e1
[e0.item(2)], # e2
[e0.item(3)], # e3
[0.], # p
[0.], # q
[0.]
]) # r
delta0 = MsgDelta()
x0 = np.concatenate((state0, delta0.to_array()), axis=0)
# define equality constraints
cons = ({
'type':
'eq',
'fun':
lambda x: np.array([
x[3]**2 + x[4]**2 + x[
5]**2 - Va**2, # magnitude of velocity vector is Va
x[4], # v=0, force side velocity to be zero
x[6]**2 + x[7]**2 + x[8]**2 + x[9]**2 -
1., # force quaternion to be unit length
x[
7
], # e1=0 - forcing e1=e3=0 ensures zero roll and zero yaw in trim
x[9], # e3=0
x[10], # p=0 - angular rates should all be zero
x[11], # q=0
x[12], # r=0
]),
'jac':
lambda x: np.array([
[
0., 0., 0., 2 * x[3], 2 * x[4], 2 * x[5], 0., 0., 0., 0., 0.,
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., 2 * x[6], 2 * x[7], 2 * x[8], 2 * x[9],
0., 0., 0., 0., 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., 1., 0., 0., 0., 0., 0., 0.,
0.
],
[
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., 1., 0., 0., 0., 0.,
0.
],
[
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0.,
0.
],
])
})
# solve the minimization problem to find the trim states and inputs
res = minimize(trim_objective_fun,
x0,
method='SLSQP',
args=(mav, Va, gamma),
constraints=cons,
options={
'ftol': 1e-10,
'disp': True
})
# extract trim state and input and return
trim_state = np.array([res.x[0:13]]).T
trim_input = MsgDelta(elevator=res.x.item(13),
aileron=res.x.item(14),
rudder=res.x.item(15),
throttle=res.x.item(16))
trim_input.print()
print('trim_state=', trim_state.T)
return trim_state, trim_input