def update(self, rocket, angular_rates, delta_time) -> np.array([]):
"""
Calculates the orientation quaternion of the Rocket object based on
fixed angular rates.
Parameters
----------
rocket: A rocket object
An rocket instance. Calculations will be done according to this rocket's state
angular_rates: numpy.array
The angular (pitch, yaw, and roll) rates of the Rocket object.
delta_time: float
The change in time since the last update of the Rocket flight in
seconds.
Returns
-------
numpy.array
Numpy array (containing data with float type) representing the
orientation of the Rocket object.
"""
orientation_quaternion = Quaternion(rocket.orientation)
orientation_quaternion.integrate(angular_rates * self.calibration,
delta_time)
return orientation_quaternion.elements
def imu_integration(imu_data, x_0_v, track_progress=True):
# TODO: get a better comparison
samples = len(imu_data)
out = np.zeros((samples, 10))
imu_v, t_diff_v = imu_data[:, :, :6], imu_data[:, :, 6:]
# Convert time diff to seconds
t_diff_v = np.squeeze(np.stack(t_diff_v / 1000), axis=2)
bar = Progbar(samples)
for sample in range(samples):
if track_progress:
bar.update(sample)
t_diff = t_diff_v[sample, :]
# Get initial states (world frame)
x_i = x_0_v[sample, :3]
v_i = x_0_v[sample, 3:6]
q_i = Quaternion(x_0_v[sample, 6:]).unit
for i in range(len(t_diff)):
dt = t_diff[i]
# Rotation body -> world
w_R_b = q_i.inverse
# Rotate angular velocity to world frame
w_w = w_R_b.rotate(imu_v[sample, i, :3])
# Rotate acceleration to world frame
w_a = w_R_b.rotate(imu_v[sample, i, 3:]) + [0, 0, 9.81]
# Integrate attitude (world frame)
q_i.integrate(w_w, dt)
# Integrate velocity
v_i += w_a * dt
# Integrate position
x_i += v_i * dt + 1 / 2 * w_a * (dt**2)
out[sample, 0:3] = x_i
out[sample, 3:6] = v_i
out[sample, 6:] = q_i.elements
out[:, 6:] = correct_quaternion_flip(out[:, 6:])
return out
def test_integration(self):
rotation_rate = [0, 0, 2*pi] # one rev per sec around z
v = [1, 0, 0] # test vector
for dt in [0, 0.25, 0.5, 0.75, 1, 2, 10, 1e-10, random()*10]: # time step in seconds
qt = Quaternion() # no rotation
qt.integrate(rotation_rate, dt)
q_truth = Quaternion(axis=[0,0,1], angle=dt*2*pi)
a = qt.rotate(v)
b = q_truth.rotate(v)
np.testing.assert_almost_equal(a, b, decimal=ALMOST_EQUAL_TOLERANCE)
self.assertTrue(qt.is_unit())
# Check integrate() is norm-preserving over many calls
q = Quaternion()
for i in range(1000):
q.integrate([pi, 0, 0], 0.001)
self.assertTrue(q.is_unit())
def make_rig_RA_RU(VX, VY, VZ, RVX, RVY, RVZ, delta_t, originCoordsG):
n_t = VX.shape[0] - 1
npoin = originCoordsG.shape[1]
# 原点情報
UX = np.zeros((n_t + 1, 2), dtype=np.float64)
UY = np.zeros((n_t + 1, 2), dtype=np.float64)
UZ = np.zeros((n_t + 1, 2), dtype=np.float64)
AX = np.zeros((n_t + 1, 2), dtype=np.float64)
AY = np.zeros((n_t + 1, 2), dtype=np.float64)
AZ = np.zeros((n_t + 1, 2), dtype=np.float64)
RAX = np.zeros((n_t + 1, 2), dtype=np.float64)
RAY = np.zeros((n_t + 1, 2), dtype=np.float64)
RAZ = np.zeros((n_t + 1, 2), dtype=np.float64)
for i in range(1, n_t + 1):
# 時間設定
UX[i, 0] = VX[i, 0]
AX[i, 0] = VX[i, 0]
UY[i, 0] = VY[i, 0]
AY[i, 0] = VY[i, 0]
UZ[i, 0] = VZ[i, 0]
AZ[i, 0] = VZ[i, 0]
RAX[i, 0] = RVX[i, 0]
RAY[i, 0] = RVY[i, 0]
RAZ[i, 0] = RVZ[i, 0]
if i > 1:
UX[i, 1] = (VX[i, 1] + VX[i - 1, 1]) / 2 * delta_t + UX[i - 1, 1]
UY[i, 1] = (VY[i, 1] + VY[i - 1, 1]) / 2 * delta_t + UY[i - 1, 1]
UZ[i, 1] = (VZ[i, 1] + VZ[i - 1, 1]) / 2 * delta_t + UZ[i - 1, 1]
if i < n_t:
AX[i, 1] = (VX[i + 1, 1] - VX[i - 1, 1]) / 2 / delta_t
AY[i, 1] = (VY[i + 1, 1] - VY[i - 1, 1]) / 2 / delta_t
AZ[i, 1] = (VZ[i + 1, 1] - VZ[i - 1, 1]) / 2 / delta_t
RAX[i, 1] = (RVX[i + 1, 1] - RVX[i - 1, 1]) / 2 / delta_t
RAY[i, 1] = (RVY[i + 1, 1] - RVY[i - 1, 1]) / 2 / delta_t
RAZ[i, 1] = (RVZ[i + 1, 1] - RVZ[i - 1, 1]) / 2 / delta_t
# 初期設定
AX[1, 1] = AX[2, 1]
AY[1, 1] = AY[2, 1]
AZ[1, 1] = AZ[2, 1]
RAX[1, 1] = RAX[2, 1]
RAY[1, 1] = RAY[2, 1]
RAZ[1, 1] = RAZ[2, 1]
tmp = np.array([
originCoordsG[0, npoin - 1], originCoordsG[1, npoin - 1],
originCoordsG[2, npoin - 1]
],
dtype=np.float64)
originE = (1 / np.sum(np.sqrt(tmp**2))) * tmp
q0 = Quaternion(np.array([1, 0, 0, 0]))
qs = [q0]
total_qs = [q0]
for step in range(1, n_t + 1): # step-1からstepを作る
wx = RVX[step, 1]
wy = RVY[step, 1]
wz = RVZ[step, 1]
q_prev = total_qs[step - 1]
q_next = Quaternion()
q_next.integrate(np.array([wx, wy, wz], dtype=np.float64), delta_t)
qs.append(q_next)
total_qs.append(q_next * q_prev)
return UX, UY, UZ, AX, AY, AZ, RAX, RAY, RAZ, qs, total_qs
def integrate_orientation(q1, angular_velocity, frequency):
q2 = Quaternion(q1)
q2.integrate(angular_velocity, 1 / frequency)
return q2
