acceleration Example

integration/acceleration-calculation.py

+import numpy as np
+from scipy.spatial.transform import Rotation as R
+
+
+class QuaternionMotion:
+    def __init__(self, quaternions, dt, position_vector):
+        self.quaternions = quaternions
+        self.dt = dt
+        self.position_vector = np.array(position_vector)
+
+    def quaternion_derivative(self):
+        q_next = self.quaternions[1:]
+        q_prev = self.quaternions[:-1]
+        dq_dt = (q_next - q_prev) / self.dt
+        return dq_dt
+
+    def angular_velocity(self):
+        dq_dt = self.quaternion_derivative()
+        angular_velocities = []
+        for i in range(len(dq_dt)):
+            q = self.quaternions[i]
+            q_conj = np.array([q[0], -q[1], -q[2], -q[3]])  # 四元数的共轭
+
+            # 四元数微分公式来计算角速度
+            omega_quat = 2 * self.quaternion_multiply(q_conj, dq_dt[i])
+            omega = omega_quat[1:]  # 提取角速度的向量部分
+            angular_velocities.append(omega)
+        return np.array(angular_velocities)
+
+    def quaternion_multiply(self, q1, q2):
+        w1, x1, y1, z1 = q1
+        w2, x2, y2, z2 = q2
+        return np.array([
+            w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
+            w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
+            w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
+            w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
+        ])
+
+    def angular_acceleration(self):
+        omega = self.angular_velocity()
+        omega_next = omega[1:]
+        omega_prev = omega[:-1]
+        alpha = (omega_next - omega_prev) / self.dt
+        return alpha
+
+    def linear_acceleration(self):
+        alpha = self.angular_acceleration()
+        linear_accelerations = np.cross(alpha, self.position_vector)
+        return linear_accelerations
+
+
+def main():
+    quaternions = np.array([
+        [0.707, 0, 0.707, 0],
+        [0.707, 0, 0.707, 0.1],
+        [0.707, 0, 0.707, 0.2]
+    ])
+
+    dt = 0.1  # 时间间隔
+    position_vector = [1, 0, 0]  # 位置矢量
+
+    motion = QuaternionMotion(quaternions, dt, position_vector)
+    linear_acceleration = motion.linear_acceleration()
+    print("线性加速度: ", linear_acceleration)
+
+
+if __name__ == "__main__":
+    main()