使用matlab编写四元数的单位化

假设四元数为q，其单位化后的四元数为q_unit。

步骤如下：

1. 计算四元数的模长 q_norm = norm(q)，其中 norm 表示向量或矩阵的模长。

2. 如果四元数的模长小于等于 0，则将单位化后的四元数 q_unit 设为原始四元数 q。

3. 如果四元数的模长大于 0，则将单位化后的四元数 q_unit 计算为 q_norm 的倒数乘以原始四元数 q，即：

q_unit = q / q_norm

4. 返回单位化后的四元数 q_unit。

以下是 MATLAB 代码实现：

function q_unit = quat_unit(q)
% q: 原始四元数
% q_unit: 单位化后的四元数

q_norm = norm(q);
if q_norm <= 0
q_unit = q;
else
q_unit = q / q_norm;
end

end