需要根据加速度计算航向然后相应地旋转加速度

Question

我目前正在为模型火箭构建一个简单的遥测计算机。我正在使用标准的 6 轴 IMU，它可以提供角速率和线性加速度。我希望，在启动时。仅通过测量的线性加速度检测初始航向，然后从该点开始旋转加速度以与地球的 X、Y 和 Z 对齐。在初始化过程中，IMU 应该经历的唯一加速度来自重力。

出于实验目的，我用python编写了代码，但最终算法将转换为Rust。

以下代码基于here找到的矩阵数学。

import math

start_x = -0.43557498
start_y = 0.5819724
start_z = -9.900554

# Keep in mind we are using X, Y, and Z rotation and not Pitch, Roll, and Yaw
α = math.atan2(-start_y, -start_z);
β = math.atan2(start_x, -start_z);
γ = math.atan2(-start_x, -start_y);

sin_α = math.sin(-α)
sin_β = math.sin(-β)
sin_γ = math.sin(-γ)

cos_α = math.cos(-α)
cos_β = math.cos(-β)
cos_γ = math.cos(-γ)

end_x = (start_x * cos_β * cos_γ) + \
        (start_y * (sin_α * sin_β * cos_γ - cos_α * sin_γ)) + \
        (start_z * (cos_α * sin_β * cos_γ + sin_α * sin_γ))

end_y = (start_x * cos_β * sin_γ) + \
        (start_y * (sin_α * sin_β * sin_γ + cos_α * cos_γ)) + \
        (start_z * (cos_α * sin_β * sin_γ - sin_α * cos_γ))

end_z = (start_x * -sin_β) + \
        (start_y * sin_α * cos_β) + \
        (start_z * cos_α * cos_β)

print(str(α) + ", " + str(β) + ", " + str(γ))
print(str(end_x) + ", " + str(end_y) + ", " + str(end_z))

我期望接近以下输出：

-0.05871423859483104, -0.04396665869464738, 2.4990867751753854
0, 0, -10

但是我得到：

-0.05871423859483104, -0.04396665869464738, 2.4990867751753854
1.3912018089280702, -0.4101017035926812, -9.20680275012796

我相当确定问题出在旋转数学上，因为航向计算相当容易理解和直观。

非常感谢任何帮助！