四元数导出旋转矩阵问题

以下示例代码使用Python实现将四元数转换为旋转矩阵：

import numpy as np

def quaternion_to_rotation_matrix(quaternion):
    """Convert quaternion to rotation matrix"""
    w, x, y, z = quaternion_normalized(quaternion)
    
    rotation_matrix = np.array([[1-2*(y**2)-2*(z**2), 2*x*y-2*w*z, 2*x*z+2*w*y],
                                [2*x*y+2*w*z, 1-2*(x**2)-2*(z**2), 2*y*z-2*w*x],
                                [2*x*z-2*w*y, 2*y*z+2*w*x, 1-2*(x**2)-2*(y**2)]])
    
    return rotation_matrix

def quaternion_normalized(quaternion):
    """Normalize quaternion"""
    q_norm = np.linalg.norm(quaternion)
    if q_norm == 0:
        return np.array([1, 0, 0, 0])
    return quaternion / q_norm

其中，函数quaternion_normalized用于将四元数进行归一化，确保其模值为1。函数quaternion_to_rotation_matrix用于将归一化后的四元数转换为旋转矩阵，该矩阵可以用于将向量从旋转前的坐标系转换到旋转后的坐标系。

例如，在以下代码中，定义了一个从$x$轴绕$y$轴旋转45度的四元数，并将其转换为旋转矩阵：

quaternion = np.array([np.cos(np.deg2rad(22.5)), 0, np.sin(np.deg2rad(22.5)), 0])
rotation_matrix = quaternion_to_rotation_matrix(quaternion)

此时，rotation_matrix即为所求的旋转矩阵。