四元数和姿态阵转换
学习资料参考:
[1] 严恭敏,翁浚. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.8.
Quaternion.h
#pragma once
#include <Eigen/Core>
#include <Eigen/Geometry>
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 <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
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 <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
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;
}