等效旋转矢量和姿态阵转换

学习资料参考：

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

EquRotationVec.h

#pragma once
#include <Eigen/Core>
#include <Eigen/Geometry>Eigen::Matrix3d VecToSkewSymmeticMat(const Eigen::Vector3d &vec);
Eigen::Vector3d SkewSymmeticMatToVec(const Eigen::Matrix3d &mat);// 一些实验性的代码，Eigen库中有更好的实现
class EquivalentRotationVector
{public:double          mAngle;Eigen::Vector3d mVec;Eigen::Matrix3d toDCM() const;void fromDCM(const Eigen::Matrix3d &dcm);
};

EquRotationVec.cpp

// [1] 严恭敏,翁浚. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.8.#include "EquRotationVec.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;// [1] 等效旋转矢量 -> 方向余弦阵, 参考公式: 2.2.22
// NOTE! 式中 u 为单位矢量
Eigen::Matrix3d EquivalentRotationVector::toDCM() const
{Matrix3d u_mat = VecToSkewSymmeticMat(mVec) / mVec.norm();Matrix3d dcm;dcm = Matrix3d::Identity() +sin(mAngle) * u_mat +(1 - cos(mAngle)) * u_mat * u_mat;return dcm;
}// [1] 方向余弦阵 -> 等效旋转矢量，参考公式: 2.2.28
// NOTE! 式中结果 phi 不是单位矢量，这里算出来的是单位矢量
void EquivalentRotationVector::fromDCM(const Eigen::Matrix3d &dcm)
{mAngle            = acos((dcm.trace() - 1) / 2.0);Matrix3d skew_mat = 1.0 / (2 * sin(mAngle)) * (dcm - dcm.transpose());mVec              = SkewSymmeticMatToVec(skew_mat);
}Eigen::Matrix3d VecToSkewSymmeticMat(const Eigen::Vector3d &vec)
{Matrix3d skew_mat;// clang-format offskew_mat << 0,      -vec[2], vec[1],vec[2],  0,     -vec[0],-vec[1],  vec[0], 0;// clang-format onreturn skew_mat;
}Eigen::Vector3d SkewSymmeticMatToVec(const Eigen::Matrix3d &mat)
{return Eigen::Vector3d(mat(1, 1), mat(0, 2), mat(1, 0));
}

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;EquRotationVec vec1;vec1.mAngle = 30 * DEG_TO_ARC;vec1.mVec   = {2, 0, 0};auto dcm_mat0 = vec1.toDCM();cout << "旋转角度（度）：" << vec1.mAngle * ARC_TO_DEG << "\n转轴方向（单位化）：\n"<< vec1.mVec / vec1.mVec.norm() << endl;EquRotationVec vec2;vec2.fromDCM(dcm_mat0);cout << "dcm_mat0: \n"<< dcm_mat0 << endl<< "angle: " << vec2.mAngle << endl<< "axis: \n"<< vec2.mVec << endl;AngleAxisd angleAxis(vec1.mAngle, vec1.mVec / vec1.mVec.norm());   // 注意在使用AngleAxisd 时转轴必须单位化auto       dcm_mat1 = angleAxis.toRotationMatrix();AngleAxisd angleAxis1(dcm_mat1);cout << "dcm_mat1: \n"<< dcm_mat1 << endl<< "angle: " << angleAxis1.angle() << endl<< "axis: \n"<< angleAxis1.axis() << endl;return 0;}

捷联惯导算法与组合导航原理学习——等效旋转矢量和姿态阵转换（一）相关推荐

捷联惯导算法与组合导航原理学习——四元数和姿态阵转换（二）
四元数和姿态阵转换学习资料参考: [1] 严恭敏,翁浚. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.8. Quaternion.h #pragma once #in ...
捷联惯导算法（二）位置更新算法的理解
前言文中算法公式摘自<捷联惯导算法与组合导航原理>(严恭敏.翁浚编著).<惯性导航>(秦永元编著),其他理解仅代表个人观点.本文是对位置更新算法,按照自己学习的思路整理得 ...
捷联惯导算法（三）姿态角和姿态矩阵
前言文中算法公式摘自<捷联惯导算法与组合导航原理>(严恭敏.翁浚编著).<惯性导航>(秦永元编著),其他理解仅代表个人观点.本文是对姿态角和姿态矩阵之间转化的理解. 一. ...
基于matlab的捷联惯导算法设计及仿真,基于 Matlab 的捷联惯导算法设计及仿真1doc.doc...
基于 Matlab 的捷联惯导算法设计及仿真1doc 基于 Matlab 的捷联惯导算法设计及仿真1 严恭敏西北工业大学航海学院,西安 (710072) E-mail:yangongmin@163. ...
捷联惯导算法--体会与心得
本文转自:http://www.amobbs.com/thread-5492189-1-1.html,收藏学习! 1.四个概念:"地理"坐标系."机体"坐标系. ...
【算法学习笔记001】捷联惯导算法心得
1.四个概念:"地理"坐标系."机体"坐标系.他们之间换算公式.换算公式用的系数. 地理坐标系:东.北.天,以下简称地理.在这个坐标系里有重力永远是(0,0,1 ...
组合导航算法（一）之捷联惯导更新及组合模式
捷联惯导基本算法惯性导航技术于20世纪50年代最初开始投入使用,可分为物理平台与模拟平台.物理平台就是平台式惯性导航系统(PINS).模拟平台又称捷联式惯性导航系统(SINS),它以计算机为平台,随 ...
捷联惯导算法（四）姿态更新算法
前言本文是对姿态更新算法的理解. 一.姿态更新算法地心惯性坐标系(i系)绝对不动的惯性参考坐标系,与时间无关. 个人理解: 地心惯性坐标系是绝对不动的,因此机体系到导航系的姿态矩阵可以先由机体系到 ...
捷联惯导知识点之非直角坐标系到直角坐标系
1.从非直角坐标系到直角坐标系的矩阵变换 2.以正交三角分解为例: 根据矩阵的 QR 分解理论,非奇异阵 Cba 总可以分解为单位正交阵 CbB和上三角阵CBa 之乘积的形式, 即在偏差角为小量情形 ...

捷联惯导算法与组合导航原理学习——等效旋转矢量和姿态阵转换（一）

等效旋转矢量和姿态阵转换

EquRotationVec.h

EquRotationVec.cpp

main.cpp

捷联惯导算法与组合导航原理学习——等效旋转矢量和姿态阵转换（一）相关推荐

最新文章

热门文章