GNSS-INS组合导航：KF-GINS（三）

原创

已于 2023-01-31 12:58:35 修改 · 1.8k 阅读

8 ·

CC 4.0 BY-SA版权

文章标签：

#算法

于 2023-01-30 20:04:33 首次发布

本文详细介绍了在三维空间中各种旋转表示之间的转换方法，包括方向余弦矩阵、旋转向量、四元数和平移向量等，并提供了从一种表示形式转换到另一种形式的具体实现，如四元数转欧拉角、欧拉角转四元数等。

rotation.h文件

方向余弦（3X3）:Eigen::Matrix3d

旋转向量（3X1）:Eigen::AngleAxisd

四元数（4X1）:Eigen::Quaterniond

平移向量（3X1）:Eigen::Vector3d

变换矩阵（4X4）:Eigen::Isometry3d

方向余弦转欧拉角（前右下）

// ZYX旋转顺序, 前右下的IMU, 输出RPY
    static Vector3d matrix2euler(const Eigen::Matrix3d &dcm) {
        Vector3d euler;

        euler[1] = atan(-dcm(2, 0) / sqrt(dcm(2, 1) * dcm(2, 1) + dcm(2, 2) * dcm(2, 2)));

        if (dcm(2, 0) <= -0.999) {
            euler[0] = 0;
            euler[2] = atan2((dcm(1, 2) - dcm(0, 1)), (dcm(0, 2) + dcm(1, 1)));
            std::cout << "[WARNING] Rotation::matrix2euler: Singular Euler Angle! Set the roll angle to 0!" << std::endl;