def update(self, delta):
x = self.intg.step(self.state.time, self.state.rigid_body, delta)
self.state.time += self.state.timestep
self.state.rigid_body = x
# update the class structure for the true state:
# [pn, pe, h, Va, alpha, beta, phi, theta, chi, p, q, r, Vg, wn, we, psi, gyro_bx, gyro_by, gyro_bz]
pdot = quat2rot(x[6:10]) @ x[3:6]
self.state.chi = np.arctan2(pdot.item(1), pdot.item(0))
def eom(self, t, x, forces_moments):
"""EOM: Evaluates f in xdot = f(t, x, forces_moments)
"""
# Get forces, moments
F_b = forces_moments[:3]
M_b = forces_moments[3:]
# Get positions, Euler angles, velocities, angular velocities
#r_i = x[ : 3] # position in i-frame
v_b = x[ 3: 6] # velocity in b-frame
q_ib = x[ 6:10] # quaternion to keep track of angle / axis
w_b = x[10: ] # angular velocity in b-frame
# Normalize quat. -> rotation
q_ib = q_ib/np.linalg.norm(q_ib) # normalize
R_ib = quat2rot(q_ib)
# Compute equations of motion
# d/dt(r_i)
rdot_i = R_ib @ v_b
# d/dt(q_ib)
wq_ib = np.zeros(4)
wq_ib[1:] = w_b.flatten()
qdot_ib = 0.5 * prod(q_ib, wq_ib)
# Compute derivatives from EOM
# d/dt(v_b)
w_bb = skew(w_b)
vdot_b = F_b/self.m - w_bb @ v_b
# d/dt(w_b)
wdot_b = self.Jinv_bb @ (M_b - w_bb @ self.J_bb @ w_b)
return np.concatenate([
rdot_i, # d/dt position in i-frame
vdot_b, # d/dt velocity in b-frame
qdot_ib, # d/dt quaternion
wdot_b # d/dt angular velocity in b-frame
])
def rot(self):
return quat2rot(self.quaternion)
def chi(self):
R = quat2rot(self.rigid_body[6:10])
Vg_inertial = R @ self.rigid_body[3:6]
return np.arctan2(Vg_inertial[1], Vg_inertial[0])