捷联惯导算法与组合导航原理学习——四元数和姿态阵转换（二）

捷联惯导算法与组合导航原理学习——四元数和姿态阵转换（二） 四元数和姿态阵转换

学习资料参考：

[1] 严恭敏,翁浚. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.8.

Quaternion.h

#pragma once
#include 
#include 

namespace my {

class Quaternion
{
public:
    Quaternion() = default;
    Quaternion(double r, double i, double j, double k);

    double r() const { return mR; }
    double i() const { return mI; }
    double j() const { return mJ; }
    double k() const { return mK; }

    Eigen::Matrix3d toDCM() const;

    void fromDCM(const Eigen::Matrix3d &dcm);

    double     norm() const;   // 四元数的模值（2-范数）
    Quaternion normalized() const; // 四元数规范化
    Quaternion conjugate() const; // 四元数的共轭
    Quaternion inverse() const; // 四元数的逆

private:
    double mR;
    double mI;
    double mJ;
    double mK;
};
Quaternion operator+(const Quaternion &left, const Quaternion &right);
Quaternion operator-(const Quaternion &left, const Quaternion &right);
Quaternion operator*(const Quaternion &left, const Quaternion &right);
Quaternion operator/(const Quaternion &left, double right);

}   // namespace my

Quaternion.cpp

// [1] 严恭敏,翁浚. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.8.

#include "Quaternion.h"

#include 
#include 
#include 

using namespace Eigen;
using namespace std;

const double ARC_TO_DEG = 57.29577951308238;
const double DEG_TO_ARC = 0.0174532925199433;

my::Quaternion::Quaternion(double r, double i, double j, double k)
    : mR(r)
    , mI(i)
    , mJ(j)
    , mK(k)
{
}

// [1] eq: 2.4.23, 四元数 -> 方向余弦阵
Eigen::Matrix3d my::Quaternion::toDCM() const
{
    Vector3d q_v(mI, mJ, mK);
    auto     skew_mat = VecToSkewSymmeticMat(q_v);
    Matrix3d mat      = Matrix3d::Identity() + 2 * mR * skew_mat + 2 * skew_mat * skew_mat;
    return mat;
}

// [1] eq: B.11, 方向余弦阵 -> 四元数
void my::Quaternion::fromDCM(const Eigen::Matrix3d &dcm)
{
    double r, i, j, k;
    if (dcm(0, 0) >= (dcm(1, 1) + dcm(2, 2)))
    {
        i = 0.5 * sqrt(1 + dcm(0, 0) - dcm(1, 1) - dcm(2, 2));
        r = (dcm(2, 1) - dcm(1, 2)) / (4 * i);
        j = (dcm(0, 1) + dcm(1, 0)) / (4 * i);
        k = (dcm(0, 2) + dcm(2, 0)) / (4 * i);
    }
    else if (dcm(1, 1) >= (dcm(0, 0) + dcm(2, 2)))
    {
        j = 0.5 * sqrt(1 - dcm(0, 0) + dcm(1, 1) - dcm(2, 2));
        r = (dcm(0, 2) - dcm(2, 0)) / (4 * j);
        i = (dcm(0, 1) + dcm(1, 0)) / (4 * j);
        k = (dcm(1, 2) + dcm(2, 1)) / (4 * j);
    }
    else if (dcm(2, 2) >= (dcm(0, 0) + dcm(1, 1)))
    {
        k = 0.5 * sqrt(1 - dcm(0, 0) - dcm(1, 1) + dcm(2, 2));
        r = (dcm(1, 0) - dcm(0, 1)) / (4 * k);
        i = (dcm(0, 2) + dcm(2, 0)) / (4 * k);
        j = (dcm(1, 2) + dcm(2, 1)) / (4 * k);
    }
    else
    {
        r = 0.5 * sqrt(1 + dcm(0, 0) + dcm(1, 1) + dcm(2, 2));
        i = (dcm(2, 1) - dcm(1, 2)) / (4 * r);
        j = (dcm(0, 2) - dcm(2, 0)) / (4 * r);
        k = (dcm(1, 0) - dcm(0, 1)) / (4 * r);
    }
    mR = r;
    mI = i;
    mJ = j;
    mK = k;
}

// [1] eq: 2.4.15 Q的2-范
double my::Quaternion::norm() const
{
    return sqrt(pow(mR, 2) + pow(mI, 2) + pow(mJ, 2) + pow(mK, 2));
}

// [1] p20, 规范化四元数
my::Quaternion my::Quaternion::normalized() const
{
    return (*this) / this->norm();
}

// [1] eq: 2.4.13 Q的共轭
my::Quaternion my::Quaternion::conjugate() const
{
    return Quaternion(mR, -mI, -mJ, -mK);
}

// [1] eq: 2.4.18 Q的逆
my::Quaternion my::Quaternion::inverse() const
{
    double root_norm = pow(norm(), 2);
    auto   conj      = conjugate();
    return conj / root_norm;
}

// [1] eq: 2.4.5a
my::Quaternion my::operator+(const Quaternion &left, const Quaternion &right)
{
    return Quaternion(left.r() + right.r(), left.i() + right.i(), left.j() + right.j(), left.k() + right.k());
}

// [1] eq: 2.4.5a
my::Quaternion my::operator-(const Quaternion &left, const Quaternion &right)
{
    return Quaternion(left.r() - right.r(), left.i() - right.i(), left.j() - right.j(), left.k() - right.k());
}

// [1] eq: 2.4.9a
my::Quaternion my::operator*(const Quaternion &left, const Quaternion &right)
{
    Eigen::Matrix4d Mp;
    Mp << left.r(), -left.i(), -left.j(), -left.k(),
        left.i(), left.r(), -left.k(), left.j(),
        left.j(), left.k(), left.r(), -left.i(),
        left.k(), -left.j(), left.i(), left.r();
    Eigen::Vector4d Vq(left.r(), left.i(), left.j(), left.k());

    auto sum = Mp * Vq;
    return Quaternion(sum[0], sum[1], sum[2], sum[3]);
}

my::Quaternion my::operator/(const Quaternion &left, double right)
{
    return Quaternion(left.r() / right, left.i() / right, left.j() / right, left.k() / right);
}

main.cpp

#include "EquRotationVec.h"

#include 
#include 
#include 

using namespace Eigen;
using namespace std;

int main(int argc, char*argv[])
{
	cout << "----- 自行实现的算法和 Eigen 中的算法比较 -----" << endl;
    cout << "----- 四元数 <-> 方向余弦阵 -----" << endl;

    Quaterniond q_0(1, 2, 3, 4);
    q_0.normalize();
    cout << "q_0: " << q_0.w() << " " << q_0.x() << " " << q_0.y() << " " << q_0.z() << endl;
    cout << "q_0: " << pow(q_0.w(), 2) + pow(q_0.x(), 2) + pow(q_0.y(), 2) + pow(q_0.z(), 2) << endl;
    auto rot_mat0 = q_0.toRotationMatrix();
    cout << "q_0: n"
         << rot_mat0 << endl;
    Quaterniond q_1(rot_mat0);
    cout << "q_1: " << q_1.w() << " " << q_1.x() << " " << q_1.y() << " " << q_1.z() << endl;

    my::Quaternion q = {1, 2, 3, 4};
    q                = q.normalized();
    auto dcm         = q.toDCM();
    cout << "q: " << q.r() << " " << q.i() << " " << q.j() << " " << q.k() << endl;
    cout << "my q -> dcm: n"
         << dcm << endl;

    my::Quaternion q1;
    q1.fromDCM(dcm);
    cout << "q1: " << q1.r() << " " << q1.i() << " " << q1.j() << " " << q1.k() << endl;

    return 0;
}