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