KF-GINS 和 OB-GINS 的 Earth类 和 Rotation 类

news2024/9/21 11:18:20

原始 Markdown文档、Visio流程图、XMind思维导图见:https://github.com/LiZhengXiao99/Navigation-Learning

文章目录

    • 一、Earth 类:地球参数和坐标转换
      • 1、gravity():正常重力计算
      • 2、meridianPrimeVerticalRadius():计算子午圈半径 RM、卯酉圈半径 RN
      • 3、RN():计算卯酉圈主半径 RN
      • 4、cne():n系(导航坐标系)到e系(地心地固坐标系)转换矩阵
      • 5、qne():计算n系(北东地)到e系(ECEF)转换四元数
      • 6、blh():从n系到e系转换四元数得到纬度和经度
      • 7、blh2ecef():大地坐标(经纬高)转地心地固坐标
      • 7、ecef2blh():地心地固坐标转大地坐标
      • 8、DRi(): 计算 n 系相对位置转大地坐标相对位置的矩阵
      • 9、DR():计算大地坐标相对位置转 n 系相对位置的矩阵
      • 10、local2global():局部坐标(在origin处展开)转大地坐标
      • 11、global2local():大地坐标转局部坐标(在origin处展开)
      • 12、iewe():地球自转角速度投影到e系
      • 13、iewn():地球自转角速度投影到n系
      • 14、enwn():n系相对于e系转动角速度投影到n系
    • 二、Rotation 类:姿态转换
      • 1、matrix2quaternion():旋转矩阵转四元数
      • 2、quaternion2matrix():四元数转旋转矩阵
      • 3、matrix2euler():旋转矩阵转欧拉角
      • 4、quaternion2euler():四元数转欧拉角
      • 5、rotvec2quaternion():等效旋转矢量转四元数
      • 6、quaternion2vector():四元数转旋转矢量
      • 7、euler2matrix():欧拉角转旋转矩阵
      • 8、euler2quaternion():欧拉角转四元数
      • 9、skewSymmetric():计算三维向量反对称阵
      • 10、quaternionleft()、quaternionright():四元数矩阵

一、Earth 类:地球参数和坐标转换

image-20230925182052936

Earth 类里都是静态函数,使用的时候直接类名::成员函数(),文件的开头定义了一些椭球参数:

/* WGS84椭球模型参数
   NOTE:如果使用其他椭球模型需要修改椭球参数 */
const double WGS84_WIE = 7.2921151467E-5;       /* 地球自转角速度*/
const double WGS84_F   = 0.0033528106647474805; /* 扁率 */
const double WGS84_RA  = 6378137.0000000000;    /* 长半轴a */
const double WGS84_RB  = 6356752.3142451793;    /* 短半轴b */
const double WGS84_GM0 = 398600441800000.00;    /* 地球引力常数 */
const double WGS84_E1  = 0.0066943799901413156; /* 第一偏心率平方 */
const double WGS84_E2  = 0.0067394967422764341; /* 第二偏心率平方 */

1、gravity():正常重力计算

重力是万有引力与离心力共同作用的结果,随纬度升高离心力增大但引力减小、随高程升高引力减小,共同作用下重力的计算公式如下:

g L = 9.7803267715 × ( 1 + 0.0052790414 × sin ⁡ 2 L − 0.0000232718 × sin ⁡ 2 2 L ) + h × ( 0.0000000043977311 × sin ⁡ 2 L − 0.0000030876910891 ) + 0.0000000000007211 × sin ⁡ 4 2 L g_{L}=9.7803267715 \times\left(1+0.0052790414 \times \sin ^{2} L-0.0000232718 \times \sin ^{2} 2 L\right) \\ +h\times(0.0000000043977311\times\sin ^{2} L-0.0000030876910891)+0.0000000000007211\times\sin ^{4} 2 L gL=9.7803267715×(1+0.0052790414×sin2L0.0000232718×sin22L)+h×(0.0000000043977311×sin2L0.0000030876910891)+0.0000000000007211×sin42L

static double gravity(const Vector3d &blh) {
    double sin2 = sin(blh[0]);
    sin2 *= sin2;
    return 9.7803267715 * (1 + 0.0052790414 * sin2 + 0.0000232718 * sin2 * sin2) +
         blh[2] * (0.0000000043977311 * sin2 - 0.0000030876910891) + 0.0000000000007211 * blh[2] * blh[2];
}

2、meridianPrimeVerticalRadius():计算子午圈半径 RM、卯酉圈半径 RN

返回值是 Vector2d,第一个是子午圈主曲率半径 RM、第二个是卯酉圈主半径 RN:
R M = R e ( 1 − e 2 ) ( 1 − e 2 sin ⁡ 2 L ) 3 / 2 、 R N = R e 1 − e 2 sin ⁡ 2 L R_{M}=\frac{R_{e}\left(1-e^{2}\right)}{\left(1-e^{2} \sin ^{2} L\right)^{3 / 2}}、R_{N}=\frac{R_{e}}{\sqrt{1-e^{2} \sin ^{2} L}} RM=(1e2sin2L)3/2Re(1e2)RN=1e2sin2L Re

static Eigen::Vector2d meridianPrimeVerticalRadius(double lat) {
    double tmp, sqrttmp;

    tmp = sin(lat);	
    tmp *= tmp;
    tmp     = 1 - WGS84_E1 * tmp;
    sqrttmp = sqrt(tmp);

    return {WGS84_RA * (1 - WGS84_E1) / (sqrttmp * tmp), WGS84_RA / sqrttmp};
}

3、RN():计算卯酉圈主半径 RN

R N = R e 1 − e 2 sin ⁡ 2 L R_{N}=\frac{R_{e}}{\sqrt{1-e^{2} \sin ^{2} L}} RN=1e2sin2L Re

static double RN(double lat) {
   double sinlat = sin(lat);
   return WGS84_RA / sqrt(1.0 - WGS84_E1 * sinlat * sinlat);
}

4、cne():n系(导航坐标系)到e系(地心地固坐标系)转换矩阵

C e n = [ − sin ⁡ φ 0 cos ⁡ φ 0 1 0 − cos ⁡ φ 0 − sin ⁡ φ ] [ cos ⁡ λ sin ⁡ λ 0 − sin ⁡ λ cos ⁡ λ 0 0 0 1 ] = [ − sin ⁡ φ cos ⁡ λ − sin ⁡ φ sin ⁡ λ cos ⁡ φ − sin ⁡ λ cos ⁡ λ 0 − cos ⁡ φ cos ⁡ λ − cos ⁡ φ sin ⁡ λ − sin ⁡ φ ] C_{e}^{n}=\left[\begin{array}{ccc}-\sin \varphi & 0 & \cos \varphi \\ 0 & 1 & 0 \\ -\cos \varphi & 0 & -\sin \varphi\end{array}\right]\left[\begin{array}{ccc}\cos \lambda & \sin \lambda & 0 \\ -\sin \lambda & \cos \lambda & 0 \\ 0 & 0 & 1\end{array}\right]=\\ \left[\begin{array}{ccc}-\sin \varphi \cos \lambda & -\sin \varphi \sin \lambda & \cos \varphi \\ -\sin \lambda & \cos \lambda & 0 \\ -\cos \varphi \cos \lambda & -\cos \varphi \sin \lambda & -\sin \varphi\end{array}\right] Cen= sinφ0cosφ010cosφ0sinφ cosλsinλ0sinλcosλ0001 = sinφcosλsinλcosφcosλsinφsinλcosλcosφsinλcosφ0sinφ

static Matrix3d cne(const Vector3d &blh) {
    double coslon, sinlon, coslat, sinlat;

    sinlat = sin(blh[0]);
    sinlon = sin(blh[1]);
    coslat = cos(blh[0]);
    coslon = cos(blh[1]);

    Matrix3d dcm;
    dcm(0, 0) = -sinlat * coslon;
    dcm(0, 1) = -sinlon;
    dcm(0, 2) = -coslat * coslon;

    dcm(1, 0) = -sinlat * sinlon;
    dcm(1, 1) = coslon;
    dcm(1, 2) = -coslat * sinlon;

    dcm(2, 0) = coslat;
    dcm(2, 1) = 0;
    dcm(2, 2) = -sinlat;

    return dcm;
}

5、qne():计算n系(北东地)到e系(ECEF)转换四元数

位置更新的时候,调用此函数根据上一时刻经纬度,得到上一时刻的 qne,然后 qee * qne * qnn 得到当前时刻的 qne,再调用下面的 blh() 得到经纬度。
q n e = [ cos ⁡ ( − π / 4 − φ / 2 ) cos ⁡ ( λ / 2 ) − sin ⁡ ( − π / 4 − φ / 2 ) sin ⁡ ( λ / 2 ) sin ⁡ ( − π / 4 − φ / 2 ) cos ⁡ ( λ / 2 ) cos ⁡ ( − π / 4 − sin ⁡ / 2 ) sin ⁡ ( λ / 2 ) ] ] \boldsymbol{q}_{n}^{e}=\left[\begin{array}{c}\cos (-\pi / 4-\varphi / 2) \cos (\lambda / 2) \\ -\sin (-\pi / 4-\varphi / 2) \sin (\lambda / 2) \\ \sin (-\pi / 4-\varphi / 2) \cos (\lambda / 2) \\ \cos (-\pi / 4-\sin / 2) \sin (\lambda / 2)]\end{array}\right] qne= cos(π/4φ/2)cos(λ/2)sin(π/4φ/2)sin(λ/2)sin(π/4φ/2)cos(λ/2)cos(π/4sin/2)sin(λ/2)]

/* n系(导航坐标系)到e系(地心地固坐标系)转换四元数 */
static Quaterniond qne(const Vector3d &blh) {
    Quaterniond quat;

    double coslon, sinlon, coslat, sinlat;

    coslon = cos(blh[1] * 0.5);
    sinlon = sin(blh[1] * 0.5);
    coslat = cos(-M_PI * 0.25 - blh[0] * 0.5);
    sinlat = sin(-M_PI * 0.25 - blh[0] * 0.5);

    quat.w() = coslat * coslon;
    quat.x() = -sinlat * sinlon;
    quat.y() = sinlat * coslon;
    quat.z() = coslat * sinlon;

    return quat;
}

6、blh():从n系到e系转换四元数得到纬度和经度

位置更新的时候,通过算当前时刻 n 系到 e 系转换四元数 qne,然后调用此函数得到经纬度。

/* 从n系到e系转换四元数得到纬度和经度 */
static Vector3d blh(const Quaterniond &qne, double height) {
    return {-2 * atan(qne.y() / qne.w()) - M_PI * 0.5, 2 * atan2(qne.z(), qne.w()), height};
}

7、blh2ecef():大地坐标(经纬高)转地心地固坐标

x = ( R N + h ) cos ⁡ L cos ⁡ λ y = ( R N + h ) cos ⁡ L sin ⁡ λ z = [ R N ( 1 − e 2 ) + h ] sin ⁡ L \begin{array}{l}x=\left(R_{N}+h\right) \cos L \cos \lambda \\ y=\left(R_{N}+h\right) \cos L \sin \lambda \\ z=\left[R_{N}\left(1-e^{2}\right)+h\right] \sin L\end{array} x=(RN+h)cosLcosλy=(RN+h)cosLsinλz=[RN(1e2)+h]sinL

/* 大地坐标(纬度、经度和高程)转地心地固坐标 */
static Vector3d blh2ecef(const Vector3d &blh) {
    double coslat, sinlat, coslon, sinlon;
    double rnh, rn;

    coslat = cos(blh[0]);
    sinlat = sin(blh[0]);
    coslon = cos(blh[1]);
    sinlon = sin(blh[1]);

    rn  = RN(blh[0]);
    rnh = rn + blh[2];

    return {rnh * coslat * coslon, rnh * coslat * sinlon, (rnh - rn * WGS84_E1) * sinlat};
}

7、ecef2blh():地心地固坐标转大地坐标

B 0 = arctan ⁡ ( Z ( 1 − e 2 ) p ) N k = a 1 − e 2 sin ⁡ 2 B k − 1 H k = p cos ⁡ B k − 1 − N k B k = arctan ⁡ ( z ( 1 − e 2 N k N k )   p ) \begin{array}{c}B_{0}=\arctan \left(\frac{Z}{\left(1-e^{2}\right) p}\right) \\ N_{k}=\frac{a}{\sqrt{1-e^{2} \sin ^{2} B_{k-1}}} \\ H_{k}=\frac{p}{\cos B_{k-1}}-N_{k} \\ B_{k}=\arctan \left(\frac{z}{\left(1-\frac{e^{2} N_{k}}{N_{k}}\right)\ p }\right)\end{array} B0=arctan((1e2)pZ)Nk=1e2sin2Bk1 aHk=cosBk1pNkBk=arctan (1Nke2Nk) pz

static Vector3d ecef2blh(const Vector3d &ecef) {
    double p = sqrt(ecef[0] * ecef[0] + ecef[1] * ecef[1]);
    double rn;
    double lat, lon;
    double h = 0, h2;

    // 初始状态
    lat = atan(ecef[2] / (p * (1.0 - WGS84_E1)));
    lon = 2.0 * atan2(ecef[1], ecef[0] + p);

    do {
        h2  = h;
        rn  = RN(lat);
        h   = p / cos(lat) - rn;
        lat = atan(ecef[2] / (p * (1.0 - WGS84_E1 * rn / (rn + h))));
    } while (fabs(h - h2) > 1.0e-4);

    return {lat, lon, h};
}

8、DRi(): 计算 n 系相对位置转大地坐标相对位置的矩阵

通过算出来的矩阵实现 ENU 和 LLH 之间的转换:

  • 捷联惯导求出的速度是 n 系的,要转成经纬度增量就得用这个矩阵。
  • 杆臂误差补偿时也需要用这个函数,因为杆臂是 n 系的,算出的 IMU 坐标和给的 GNSS 解都是经纬高。
  • 存的位置是经纬高,GNSS 量测更新时候计算的是 ENU 下位置的增量,反馈的时候也需要此矩阵。

[ δ φ δ L δ H ] = [ ( R M + H ) − 1 0 0 0 ( R N + H ) − 1 0 0 0 − 1 ] [ δ p N δ p E δ p B ] \left[\begin{array}{l}\delta \varphi \\ \delta L \\ \delta H\end{array}\right]=\left[\begin{array}{ccc}\left(R_{M}+H\right)^{-1} & 0 & 0 \\ 0 & \left(R_{N}+H\right)^{-1} & 0 \\ 0 & 0 & -1 \end{array}\right]\left[\begin{array}{l}\delta \boldsymbol{p}_{N} \\ \delta \boldsymbol{p}_{E} \\ \delta \boldsymbol{p}_{B}\end{array}\right] δφδLδH = (RM+H)1000(RN+H)10001 δpNδpEδpB

/* n系相对位置转大地坐标相对位置 */
static Matrix3d DRi(const Vector3d &blh) {
    Matrix3d dri = Matrix3d::Zero();

    Eigen::Vector2d rmn = meridianPrimeVerticalRadius(blh[0]);

    dri(0, 0) = 1.0 / (rmn[0] + blh[2]);
    dri(1, 1) = 1.0 / ((rmn[1] + blh[2]) * cos(blh[0]));
    dri(2, 2) = -1;
    return dri;
}

9、DR():计算大地坐标相对位置转 n 系相对位置的矩阵

就是上面 DRI() 计算矩阵的倒数。
[ δ φ δ L δ H ] = [ ( R M + H ) 0 0 0 ( R N + H ) 0 0 0 − 1 ] [ δ p N δ p E δ p B ] \left[\begin{array}{l}\delta \varphi \\ \delta L \\ \delta H\end{array}\right]=\left[\begin{array}{ccc}\left(R_{M}+H\right) & 0 & 0 \\ 0 & \left(R_{N}+H\right) & 0 \\ 0 & 0 & -1 \end{array}\right]\left[\begin{array}{l}\delta \boldsymbol{p}_{N} \\ \delta \boldsymbol{p}_{E} \\ \delta \boldsymbol{p}_{B}\end{array}\right] δφδLδH = (RM+H)000(RN+H)0001 δpNδpEδpB

/* 大地坐标相对位置转n系相对位置 */
static Matrix3d DR(const Vector3d &blh) {
    Matrix3d dr = Matrix3d::Zero();

    Eigen::Vector2d rmn = meridianPrimeVerticalRadius(blh[0]);

    dr(0, 0) = rmn[0] + blh[2];
    dr(1, 1) = (rmn[1] + blh[2]) * cos(blh[0]);
    dr(2, 2) = -1;
    return dr;
}

10、local2global():局部坐标(在origin处展开)转大地坐标

enwn() 中被调用,为了方便能直接传入北东地(n 系)坐标计算 n 系相对于 e 系转动角速度在 n 系的投影。

static Vector3d local2global(const Vector3d &origin, const Vector3d &local) {

    Vector3d ecef0 = blh2ecef(origin);
    Matrix3d cn0e  = cne(origin);

    Vector3d ecef1 = ecef0 + cn0e * local;
    Vector3d blh1  = ecef2blh(ecef1);

    return blh1;
}

11、global2local():大地坐标转局部坐标(在origin处展开)

好像整个程序中都没用到这个函数。

static Vector3d global2local(const Vector3d &origin, const Vector3d &global) {
    Vector3d ecef0 = blh2ecef(origin);
    Matrix3d cn0e  = cne(origin);

    Vector3d ecef1 = blh2ecef(global);

    return cn0e.transpose() * (ecef1 - ecef0);
}
static Pose global2local(const Vector3d &origin, const Pose &global) {
    Pose local;

    Vector3d ecef0 = blh2ecef(origin);
    Matrix3d cn0e  = cne(origin);

    Vector3d ecef1 = blh2ecef(global.t);
    Matrix3d cn1e  = cne(global.t);

    local.t = cn0e.transpose() * (ecef1 - ecef0);
    local.R = cn0e.transpose() * cn1e * global.R;

    return local;
}

12、iewe():地球自转角速度投影到e系

ω i e e = [ 0 0 ω e ] T \boldsymbol{\omega}_{i e}^{e}=\left[\begin{array}{lll}0 & 0 & \omega_{e}\end{array}\right]^{T} ωiee=[00ωe]T

static Vector3d iewe() {
    return {0, 0, WGS84_WIE};
}

13、iewn():地球自转角速度投影到n系

ω i e n = [ ω e cos ⁡ φ 0 − ω e sin ⁡ φ ] T \boldsymbol{\omega}_{i e}^{n}=\left[\begin{array}{lll}\omega_{e} \cos \varphi & 0 & -\omega_{e} \sin \varphi\end{array}\right]^{T} ωien=[ωecosφ0ωesinφ]T

static Vector3d iewn(double lat) {
    return {WGS84_WIE * cos(lat), 0, -WGS84_WIE * sin(lat)};
}

也可以直接传入北东地(n 系)坐标计算:

static Vector3d iewn(const Vector3d &origin, const Vector3d &local) {
    Vector3d global = local2global(origin, local);
    return iewn(global[0]);
}

14、enwn():n系相对于e系转动角速度投影到n系

由载体运动线速度和地球曲率引起,与东向、北向速度有关,与天向速度无关
ω e n n = [ v E R N + h − v N R M + h − v E tan ⁡ φ R N + h ] T \boldsymbol{\omega}_{e n}^{n}=\left[\begin{array}{lll}\frac{v_{E}}{R_{N}+h} & \frac{-v_{N}}{R_{M}+h} & -\frac{v_{E} \tan \varphi}{R_{N}+h}\end{array}\right]^{T} ωenn=[RN+hvERM+hvNRN+hvEtanφ]T

static Vector3d enwn(const Eigen::Vector2d &rmn, const Vector3d &blh, const Vector3d &vel) {
    return {vel[1] / (rmn[1] + blh[2]), -vel[0] / (rmn[0] + blh[2]), -vel[1] * tan(blh[0]) / (rmn[1] + blh[2])};
}

同样也可以直接传入北东地(n 系)坐标计算:

static Vector3d enwn(const Vector3d &origin, const Vector3d &local, const Vector3d &vel) {
    Vector3d global     = local2global(origin, local);
    Eigen::Vector2d rmn = meridianPrimeVerticalRadius(global[0]);

    return enwn(rmn, global, vel);
}

二、Rotation 类:姿态转换

image-20231020105459334

1、matrix2quaternion():旋转矩阵转四元数

Eigen 中的四元数可以直接传入旋转矩阵(三维矩阵)构造:

static Quaterniond matrix2quaternion(const Matrix3d &matrix) {
    return Quaterniond(matrix);
}

2、quaternion2matrix():四元数转旋转矩阵

四元数调用 toRotationMatrix() 函数,转为旋转矩阵:

static Matrix3d quaternion2matrix(const Quaterniond &quaternion) {
    return quaternion.toRotationMatrix();
}

3、matrix2euler():旋转矩阵转欧拉角

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] = atan2(dcm(2, 1), dcm(2, 2));
        euler[2] = atan2((dcm(1, 2) - dcm(0, 1)), (dcm(0, 2) + dcm(1, 1)));
    } else if (dcm(2, 0) >= 0.999) {
        euler[0] = atan2(dcm(2, 1), dcm(2, 2));
        euler[2] = M_PI + atan2((dcm(1, 2) + dcm(0, 1)), (dcm(0, 2) - dcm(1, 1)));
    } else {
        euler[0] = atan2(dcm(2, 1), dcm(2, 2));
        euler[2] = atan2(dcm(1, 0), dcm(0, 0));
    }

    // heading 0~2PI
    if (euler[2] < 0) {
        euler[2] = M_PI * 2 + euler[2];
    }

    return euler;
}

4、quaternion2euler():四元数转欧拉角

先调用 toRotationMatrix() 转为旋转矩阵,再调用 matrix2euler() 转欧拉角:

static Vector3d quaternion2euler(const Quaterniond &quaternion) {
    return matrix2euler(quaternion.toRotationMatrix());
}

5、rotvec2quaternion():等效旋转矢量转四元数

根据传入的旋转矢量,计算向量的长度作为旋转的角度,计算向量的归一化版本作为旋转的轴,然后调用 AngleAxisd(),将角度和轴转换为四元数。

static Quaterniond rotvec2quaternion(const Vector3d &rotvec) {
    double angle = rotvec.norm();       // 计算向量的长度作为旋转的角度
    Vector3d vec = rotvec.normalized(); // 计算向量的归一化版本作为旋转的轴
    return Quaterniond(Eigen::AngleAxisd(angle, vec));  // 调用 AngleAxisd(),将角度和轴转换为四元数
}

6、quaternion2vector():四元数转旋转矢量

传入的四元数通过 Eigen::AngleAxisd 类的构造函数转换为角度轴(angle-axis)表示。角度轴是一个描述旋转的方法,其中旋转角度和旋转轴是两个独立的部分。然后,该函数返回这个角度轴表示的旋转的角度乘以旋转的轴,得到一个三维向量。这个向量的 x、y 和 z 分量分别对应于旋转轴在x、y 和 z 轴上的分量,而其长度(或者说范数)等于旋转角度。

static Vector3d quaternion2vector(const Quaterniond &quaternion) {
    Eigen::AngleAxisd axisd(quaternion);
    return axisd.angle() * axisd.axis();
}

7、euler2matrix():欧拉角转旋转矩阵

三个欧拉角分别转为 ZYX 角轴,相乘之后构造旋转矩阵

static Matrix3d euler2matrix(const Vector3d &euler) {
    return Matrix3d(Eigen::AngleAxisd(euler[2], Vector3d::UnitZ()) *
                    Eigen::AngleAxisd(euler[1], Vector3d::UnitY()) *
                    Eigen::AngleAxisd(euler[0], Vector3d::UnitX()));
}

8、euler2quaternion():欧拉角转四元数

三个欧拉角分别转为 ZYX 角轴,相乘之后构造四元数

static Quaterniond euler2quaternion(const Vector3d &euler) {
    return Quaterniond(Eigen::AngleAxisd(euler[2], Vector3d::UnitZ()) *
                       Eigen::AngleAxisd(euler[1], Vector3d::UnitY()) *
                       Eigen::AngleAxisd(euler[0], Vector3d::UnitX()));
}

9、skewSymmetric():计算三维向量反对称阵

static Matrix3d skewSymmetric(const Vector3d &vector) {
    Matrix3d mat;
    mat << 0, -vector(2), vector(1), vector(2), 0, -vector(0), -vector(1), vector(0), 0;
    return mat;
}

10、quaternionleft()、quaternionright():四元数矩阵

P ∘ Q = [ p 0 − p 1 − p 2 − p 3 p 1 p 0 − p 3 p 2 p 2 p 3 p 0 − p 1 p 3 − p 2 p 1 p 0 ] [ q 0 q 1 q 2 q 3 ] = M P Q = [ q 0 − q 1 − q 2 − q 3 q 1 q 0 q 3 − q 2 q 2 − q 3 q 0 q 1 q 3 q 2 − q 1 q 0 ] [ p 0 p 1 p 2 p 3 ] = M Q ′ P \boldsymbol{P} \circ \boldsymbol{Q}=\left[\begin{array}{cccc}p_{0} & -p_{1} & -p_{2} & -p_{3} \\ p_{1} & p_{0} & -p_{3} & p_{2} \\ p_{2} & p_{3} & p_{0} & -p_{1} \\ p_{3} & -p_{2} & p_{1} & p_{0}\end{array}\right]\left[\begin{array}{l}q_{0} \\ q_{1} \\ q_{2} \\ q_{3}\end{array}\right]=\boldsymbol{M}_{P} \boldsymbol{Q}=\left[\begin{array}{cccc}q_{0} & -q_{1} & -q_{2} & -q_{3} \\ q_{1} & q_{0} & q_{3} & -q_{2} \\ q_{2} & -q_{3} & q_{0} & q_{1} \\ q_{3} & q_{2} & -q_{1} & q_{0}\end{array}\right]\left[\begin{array}{l}p_{0} \\ p_{1} \\ p_{2} \\ p_{3}\end{array}\right]=\boldsymbol{M}_{Q}^{\prime} \boldsymbol{P} PQ= p0p1p2p3p1p0p3p2p2p3p0p1p3p2p1p0 q0q1q2q3 =MPQ= q0q1q2q3q1q0q3q2q2q3q0q1q3q2q1q0 p0p1p2p3 =MQP

M P = [ p 0 − p 1 − p 2 − p 3 p 1 p 0 − p 3 p 2 p 2 p 3 p 0 − p 1 p 3 − p 2 p 1 p 0 ] = [ p 0 − p v T p v p 0 I + ( p v × ) ] \boldsymbol{M}_{P}=\left[\begin{array}{cccc}p_{0} & -p_{1} & -p_{2} & -p_{3} \\ p_{1} & p_{0} & -p_{3} & p_{2} \\ p_{2} & p_{3} & p_{0} & -p_{1} \\ p_{3} & -p_{2} & p_{1} & p_{0}\end{array}\right]=\left[\begin{array}{cc}p_{0} & -\boldsymbol{p}_{v}^{\mathrm{T}} \\ \boldsymbol{p}_{v} & p_{0} \boldsymbol{I}+\left(\boldsymbol{p}_{v} \times\right)\end{array}\right] MP= p0p1p2p3p1p0p3p2p2p3p0p1p3p2p1p0 =[p0pvpvTp0I+(pv×)]

static Eigen::Matrix4d quaternionleft(const Quaterniond &q) {
    Eigen::Matrix4d ans;
    ans(0, 0)             = q.w();
    ans.block<1, 3>(0, 1) = -q.vec().transpose();
    ans.block<3, 1>(1, 0) = q.vec();
    ans.block<3, 3>(1, 1) = q.w() * Eigen::Matrix3d::Identity() + skewSymmetric(q.vec());
    return ans;
}

M Q ′ = [ q 0 − q 1 − q 2 − q 3 q 1 q 0 q 3 − q 2 q 2 − q 3 q 0 q 1 q 3 q 2 − q 1 q 0 ] = [ q 0 − q v T q v q 0 I − ( q v × ) ] \boldsymbol{M}_{Q}^{\prime}=\left[\begin{array}{cccc}q_{0} & -q_{1} & -q_{2} & -q_{3} \\ q_{1} & q_{0} & q_{3} & -q_{2} \\ q_{2} & -q_{3} & q_{0} & q_{1} \\ q_{3} & q_{2} & -q_{1} & q_{0}\end{array}\right]=\left[\begin{array}{cc}q_{0} & -\boldsymbol{q}_{v}^{\mathrm{T}} \\ \boldsymbol{q}_{v} & q_{0} \boldsymbol{I}-\left(\boldsymbol{q}_{v} \times\right)\end{array}\right] MQ= q0q1q2q3q1q0q3q2q2q3q0q1q3q2q1q0 =[q0qvqvTq0I(qv×)]

static Eigen::Matrix4d quaternionright(const Quaterniond &p) {
    Eigen::Matrix4d ans;
    ans(0, 0)             = p.w();
    ans.block<1, 3>(0, 1) = -p.vec().transpose();
    ans.block<3, 1>(1, 0) = p.vec();
    ans.block<3, 3>(1, 1) = p.w() * Eigen::Matrix3d::Identity() - skewSymmetric(p.vec());
    return ans;
}

egin{array}{cc}q_{0} & -\boldsymbol{q}{v}^{\mathrm{T}} \ \boldsymbol{q}{v} & q_{0} \boldsymbol{I}-\left(\boldsymbol{q}_{v} \times\right)\end{array}\right]
$$

static Eigen::Matrix4d quaternionright(const Quaterniond &p) {
    Eigen::Matrix4d ans;
    ans(0, 0)             = p.w();
    ans.block<1, 3>(0, 1) = -p.vec().transpose();
    ans.block<3, 1>(1, 0) = p.vec();
    ans.block<3, 3>(1, 1) = p.w() * Eigen::Matrix3d::Identity() - skewSymmetric(p.vec());
    return ans;
}

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/1135349.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

04 文件管理

文件管理 文件和目录的创建 删除文件和目录 文件查找命令 文件的拷贝和移动 打包和压缩

MySQL -- 表的约束

MySQL – 表的约束 文章目录 MySQL -- 表的约束一、表的约束1.空属性2.默认值3.列描述4.zerofill5.主键6.自增长7.唯一键8.外键 一、表的约束 真正约束字段的是数据类型&#xff0c;但是数据类型约束很单一&#xff0c;需要有一些额外的约束&#xff0c;更好的保证数据的合 法…

使用Selenium和Java编写爬虫程序

以下是一个使用Selenium和Java编写的音频爬虫程序&#xff0c;该程序使用了proxy的代码。请注意&#xff0c;这个示例需要在IDE中运行&#xff0c;并且可能需要根据您的系统和需求进行调整。 import java.io.IOException; import java.util.List; import java.util.concurrent…

PYTHON快捷键合集!学会让你成为大一最靓的仔

前言 大家好&#xff0c;我是艾登&#xff0c;一个始于JAVA终于PYTHON的老程序员&#xff0c;学习代码固然重要&#xff0c;但是在职场上能够知道打代码的各种快捷键的手法能够让你事半功倍&#xff0c;现在就由我来向大家介绍一下python各种快捷键的用法。 如果觉得对你有帮助…

机器学习(python)笔记整理

目录 一、数据预处理&#xff1a; 1. 缺失值处理&#xff1a; 2. 重复值处理&#xff1a; 3. 数据类型&#xff1a; 二、特征工程: 1. 规范化&#xff1a; 2. 归一化&#xff1a; 3. 标准化(方差)&#xff1a; 三、训练模型&#xff1a; 如何计算精确度&#xff0c;召…

浅谈IIC总线通信协议

IIC IIC&#xff1a;集成电路总线(Inter-Integrated Circuit) 快速&#xff1a;400kbit/s 高速&#xff1a;3.4Mbit/s 速度由 SCL 决定&#xff0c;上升沿斜率受上拉电阻和等效电容影响。 物理层 两线式串行总线&#xff0c;可发送和接收数据。 数据线&#xff1a;SDA 时钟线…

栈和队列(2)

目录 &#x1f341;一、链表的概念 &#x1f341;二、针对本文章给出的几点注意事项&#xff1a; &#x1f341;三、队列的实现 &#x1f315;&#xff08;一&#xff09;、代码定义 注意&#xff1a; &#x1f315;&#xff08;二&#xff09;、初始化 &#x1f315;&am…

java.java.lang.NoSuchMethodError: org.bouncycastle.math.ec.ECFieldElement

目录 Java运行时异常:行时找不到指定的方法 1.前言2.原因2.1项目中的版本有冲突2.2项目中某个包缺少bouncycastle依赖 总结参考 1.前言 java.lang.NoSuchMethodError: org.bouncycastle.math.ec.ECFieldElement$Fp.(Ljava/math/BigInteger;Ljava/math/BigInteger;) java.lang…

(PC+WAP)照明科技类网站模板 LED灯具照明网站源码下载

(PCWAP)照明科技类网站模板 LED灯具照明网站源码下载 PbootCMS内核开发的网站模板&#xff0c;该模板适用于照明科技网站、灯具照明网站等企业&#xff0c;当然其他行业也可以做&#xff0c;只需要把文字图片换成其他行业的即可&#xff1b; pcwap&#xff0c;同一个后台&#…

【网安大模型专题10.19】论文6:Java漏洞自动修复+数据集 VJBench+大语言模型、APR技术+代码转换方法+LLM和DL-APR模型的挑战与机会

How Effective Are Neural Networks for Fixing Security Vulnerabilities 写在最前面摘要贡献发现 介绍背景&#xff1a;漏洞修复需求和Java漏洞修复方向动机方法贡献 数据集先前的数据集和Java漏洞Benchmark数据集扩展要求数据处理工作最终数据集 VJBenchVJBench 与 Vul4J 的…

SSO 系统设计_token 生成

SSO 系统设计_token 生成 目录概述需求&#xff1a; 设计思路实现思路分析1.增加依赖2.代码编写3.测试 参考资料和推荐阅读 Survive by day and develop by night. talk for import biz , show your perfect code,full busy&#xff0c;skip hardness,make a better result,wai…

IT行业职场走向,哪些方向更有就业前景?——IT行业的发展现状及趋势探析

文章目录 每日一句正能量前言IT技术发展背景及历程IT行业的就业方向有哪些&#xff1f;分享在IT行业的就业经历后记 每日一句正能量 如果你认为你自己无法控制自己的情绪&#xff0c;这就是一种极为严重的不良暗示。 前言 在信息量浩如烟海、星罗棋布的大数据时代&#xff0c;…

深度学习第四阶段:NLP第二章 Transformer学习笔记

引言1&#xff1a;什么是注意力机制 参考我的一篇文章&#xff1a;https://blog.csdn.net/weixin_42110638/article/details/134011134?csdn_share_tail%7B%22type%22%3A%22blog%22%2C%22rType%22%3A%22article%22%2C%22rId%22%3A%22134011134%22%2C%22source%22%3A%22weixin…

优优嗨聚集团:抖音外卖,美食与文化的完美结合

在今天的数字化时代&#xff0c;外卖行业正在迅速发展&#xff0c;而抖音外卖的出现&#xff0c;更是引领了外卖行业的新潮流。抖音外卖不仅满足了人们对美食的追求&#xff0c;还让人们在享受美食的同时&#xff0c;感受到了浓厚的文化氛围。 抖音外卖是抖音平台推出的一项全新…

RISC Zero zkVM性能指标

1. 引言 对应代码&#xff1a; https://github.com/risc0/risc0&#xff08;C和Rust&#xff09; 运行如下指令&#xff0c;进行性能评估&#xff1a; cargo run -r --example loop //CPU cargo run -r -F metal --example loop //Metal GPU cargo run -r -F cuda --exampl…

Seata入门系列【14】AT模式源码分析之二阶段全局提交和全局回滚

1 全局提交 1.1 前言 在之前我们分析了&#xff0c;开启全局事务&#xff0c;和业务执行时是如何校验全局锁和提交本地事务的&#xff0c;接下来分析下是如何进行全局提交的。 1.2 二阶段全局提交 核心代码还是在TransactionalTemplate类中&#xff0c;当TC 没有收到异常时…

2023高频前端面试题-http

1. HTTP有哪些⽅法&#xff1f; HTTP 1.0 标准中&#xff0c;定义了3种请求⽅法&#xff1a;GET、POST、HEAD HTTP 1.1 标准中&#xff0c;新增了请求⽅法&#xff1a;PUT、PATCH、DELETE、OPTIONS、TRACE、CONNECT 2. 各个HTTP方法的具体作用是什么&#xff1f; 方法功能G…

论坛议程|COSCon'23青少年开源与开源教育(E)

众多开源爱好者翘首期盼的开源盛会&#xff1a;第八届中国开源年会&#xff08;COSCon23&#xff09;将于 10月28-29日在四川成都市高新区菁蓉汇举办。本次大会的主题是&#xff1a;“开源&#xff1a;川流不息、山海相映”&#xff01;各位新老朋友们&#xff0c;欢迎到成都&a…

OceanGPT:面向海洋科学的大型语言模型初探

海洋覆盖了约 71% 的地球表面&#xff0c;对全球的气候调节、天气模式、生物多样性以及人类的经济发展都扮演着至关重要的角色。海洋科学专注于研究海洋的自然特性、其变化规律以及与海洋资源开发和利用相关的理论、方法与应用。 本文介绍一个为海洋领域打造的大型语言模型——…

分享一款低损耗 高效率高性能 低 VCE(sat) 晶体管 NSS60600MZ4T1G

关于低 VCE(sat) 晶体管&#xff1f; 是指其饱和电压 VCE(sat) 很低的晶体管。VCE(sat) 是指晶体管在饱和区时&#xff0c;集电极与发射极之间的电压降。低 VCE(sat) 晶体管的优点是在同样的电流下&#xff0c;其 VCE(sat) 更低&#xff0c;因此能够降低电路总成本&#xff0c…