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

INS机械编排

pvapre 上一时刻状态

pvacur 输出当前时刻状态

posUpdate 位置更新

velUpdate 速度更新

attUpdate 姿态更新

#include "insmech.h"

void INSMech::insMech(const PVA &pvapre, PVA &pvacur, const IMU &imupre, const IMU &imucur) {

    // perform velocity update, position updata and attitude update in sequence, irreversible order
    // 依次进行速度更新、位置更新、姿态更新, 不可调换顺序
    velUpdate(pvapre, pvacur, imupre, imucur);
    posUpdate(pvapre, pvacur, imupre, imucur);
    attUpdate(pvapre, pvacur, imupre, imucur);
}

位置——姿态——速度

1、计算地理参数、子午圈半径、卯酉圈半径、地球自转角速度投影到n系、n系相对于e系转动角速度，重力值

 // 计算地理参数，子午圈半径和卯酉圈半径，地球自转角速度投影到n系, n系相对于e系转动角速度投影到n系，重力值
    // calculate geographic parameters, Meridian and Mao unitary radii,
    // earth rotational angular velocity projected to n-frame,
    // rotational angular velocity of n-frame to e-frame projected to n-frame, and gravity
    Eigen::Vector2d rmrn = Earth::meridianPrimeVerticalRadius(pvapre.pos(0));
    Eigen::Vector3d wie_n, wen_n;
    wie_n << WGS84_WIE * cos(pvapre.pos[0]), 0, -WGS84_WIE * sin(pvapre.pos[0]);
    wen_n << pvapre.vel[1] / (rmrn[1] + pvapre.pos[2]), -pvapre.vel[0] / (rmrn[0] + pvapre.pos[2]),
        -pvapre.vel[1] * tan(pvapre.pos[0]) / (rmrn[1] + pvapre.pos[2]);
    double gravity = Earth::gravity(pvapre.pos)

 重力计算在earth文件中

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、旋转效应和双样子划桨效应

 // 旋转效应和双子样划桨效应
    // rotational and sculling motion
    temp1 = imucur.dtheta.cross(imucur.dvel) / 2;
    temp2 = imupre.dtheta.cross(imucur.dvel) / 12;
    temp3 = imupre.dvel.cross(imucur.dtheta) / 12;

 3、b系比力积分项

// b系比力积分项
    // velocity increment due to the specific force
    d_vfb = imucur.dvel + temp1 + temp2 + temp3;

 4、比力积分投影到n系

// 比力积分项投影到n系
    // velocity increment dut to the specfic force projected to the n-frame
    temp1 = (wie_n + wen_n) * imucur.dt / 2;
    cnn   = I33 - Rotation::skewSymmetric(temp1);
    d_vfn = cnn * pvapre.att.cbn * d_vfb;

 5、计算重力/哥式积分项

 // 计算重力/哥式积分项
    // velocity increment due to the gravity and Coriolis force
    gl << 0, 0, gravity;
    d_vgn = (gl - (2 * wie_n + wen_n).cross(pvapre.vel)) * imucur.dt;

6、计算中间时刻速度（K+K-1)/2 ：为了提高精度

 // 得到中间时刻速度
    // velocity at k-1/2
    midvel = pvapre.vel + (d_vfn + d_vgn) / 2;

 

 7、外推中间时刻位置

// 外推得到中间时刻位置
    // position extrapolation to k-1/2
    qnn = Rotation::rotvec2quaternion(temp1);
    temp2 << 0, 0, -WGS84_WIE * imucur.dt / 2;
    qee = Rotation::rotvec2quaternion(temp2);
    qne = Earth::qne(pvapre.pos);
    qne = qee * qne * qnn;
    midpos[2] = pvapre.pos[2] - midvel[2] * imucur.dt / 2;
    midpos    = Earth::blh(qne, midpos[2]);

这里需要从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};
    }

为了得到从n系到e的四元数

 其中四元数需要从旋转矢量得到

8、重新计算中间时刻的地球子午圈、卯酉圈半径、自转角速度

// 重新计算中间时刻的rmrn, wie_e, wen_n
    // recompute rmrn, wie_n, and wen_n at k-1/2
    rmrn = Earth::meridianPrimeVerticalRadius(midpos[0]);
    wie_n << WGS84_WIE * cos(midpos[0]), 0, -WGS84_WIE * sin(midpos[0]);
    wen_n << midvel[1] / (rmrn[1] + midpos[2]), -midvel[0] / (rmrn[0] + midpos[2]),
        -midvel[1] * tan(midpos[0]) / (rmrn[1] + midpos[2]);

 

 9、重新计算n系下平均比力积分项

 // 重新计算n系下平均比力积分项
    // recompute d_vfn
    temp3 = (wie_n + wen_n) * imucur.dt / 2;
    cnn   = I33 - Rotation::skewSymmetric(temp3);
    d_vfn = cnn * pvapre.att.cbn * d_vfb;

 之前得到的比力积分项式预估得到的，其中wie_n 、wen_n是k时刻并不是k-1/2时刻。

10、重新计算呢重力、哥氏积分项

// 重新计算重力、哥式积分项
    // recompute d_vgn
    gl << 0, 0, Earth::gravity(midpos);
    d_vgn = (gl - (2 * wie_n + wen_n).cross(midvel)) * imucur.dt

 

 11、速度更新完成

 // 速度更新完成
    // velocity update finish
    pvacur.vel = pvapre.vel + d_vfn + d_vgn;
}

12、重新计算中间时刻速度与位置

 // 重新计算中间时刻的速度和位置
    // recompute velocity and position at k-1/2
    midvel = (pvacur.vel + pvapre.vel) / 2;
    midpos = pvapre.pos + Earth::DRi(pvapre.pos) * midvel * imucur.dt / 2;

这里进行了线性假设，存在一定的误差，不过影响应该不大。

13、重新计算中间时刻地理参数

 // 重新计算中间时刻地理参数
    // recompute rmrn, wie_n, wen_n at k-1/2
    Eigen::Vector2d rmrn;
    Eigen::Vector3d wie_n, wen_n;
    rmrn = Earth::meridianPrimeVerticalRadius(midpos[0]);
    wie_n << WGS84_WIE * cos(midpos[0]), 0, -WGS84_WIE * sin(midpos[0]);
    wen_n << midvel[1] / (rmrn[1] + midpos[2]), -midvel[0] / (rmrn[0] + midpos[2]),
        -midvel[1] * tan(midpos[0]) / (rmrn[1] + midpos[2]);

与之前一样过，就不用放图了

14、旋转矢量转四元数

 // 重新计算 k时刻到k-1时刻 n系旋转矢量
    // recompute n-frame rotation vector (n(k) with respect to n(k-1)-frame)
    temp1 = (wie_n + wen_n) * imucur.dt;
    qnn   = Rotation::rotvec2quaternion(temp1);
    // e系转动等效旋转矢量 (k-1时刻k时刻，所以取负号)
    // e-frame rotation vector (e(k-1) with respect to e(k)-frame)
    temp2 << 0, 0, -WGS84_WIE * imucur.dt;
    qee = Rotation::rotvec2quaternion(temp2);

符号相反由于坐标系转换方向不同

15、位置更新完成

// 位置更新完成
    // position update finish
    qne           = Earth::qne(pvapre.pos);
    qne           = qee * qne * qnn;
    pvacur.pos[2] = pvapre.pos[2] - midvel[2] * imucur.dt;
    pvacur.pos    = Earth::blh(qne, pvacur.pos[2]);
}

两次的位置更新方法不一样：

第一次位置更新采用讲义上的方法，是通过n系下速度计算出n系下相对位置变化，进一步转到大地坐标的相对位置进行位置更新，然而两个时刻位置不一样，n系是有略微变化的，第一次重相当忽略了这个变化。

第二次位置更新采用四元数更新，考虑了n系的变化。

16、重新计算熟速度、位置、地理参数（与之前一样）

Eigen::Quaterniond qne_pre, qne_cur, qne_mid, qnn, qbb;
    Eigen::Vector3d temp1, midpos, midvel;

    // 重新计算中间时刻的速度和位置
    // recompute velocity and position at k-1/2
    midvel = (pvapre.vel + pvacur.vel) / 2;
    qne_pre   = Earth::qne(pvapre.pos);
    qne_cur   = Earth::qne(pvacur.pos);
    temp1     = Rotation::quaternion2vector(qne_cur.inverse() * qne_pre);
    qne_mid   = qne_pre * Rotation::rotvec2quaternion(temp1 / 2).inverse();
    midpos[2] = (pvacur.pos[2] + pvapre.pos[2]) / 2;
    midpos    = Earth::blh(qne_mid, midpos[2]);

    // 重新计算中间时刻地理参数
    // recompute rmrn, wie_n, wen_n at k-1/2
    Eigen::Vector2d rmrn;
    Eigen::Vector3d wie_n, wen_n;
    rmrn = Earth::meridianPrimeVerticalRadius(midpos[0]);
    wie_n << WGS84_WIE * cos(midpos[0]), 0, -WGS84_WIE * sin(midpos[0]);
    wen_n << midvel[1] / (rmrn[1] + midpos[2]), -midvel[0] / (rmrn[0] + midpos[2]),
        -midvel[1] * tan(midpos[0]) / (rmrn[1] + midpos[2]);

17、计算n系的旋转四元数 k-1时刻到k时刻变换

// 计算n系的旋转四元数 k-1时刻到k时刻变换
    // n-frame rotation vector (n(k-1) with respect to n(k)-frame)
    temp1 = -(wie_n + wen_n) * imucur.dt;
    qnn   = Rotation::rotvec2quaternion(temp1);

 18、计算b系旋转四元数 补偿二阶圆锥误差

 // 计算b系旋转四元数 补偿二阶圆锥误差
    // b-frame rotation vector (b(k) with respect to b(k-1)-frame)
    // compensate the second-order coning correction term.
    temp1 = imucur.dtheta + imupre.dtheta.cross(imucur.dtheta) / 12;
    qbb   = Rotation::rotvec2quaternion(temp1);

 19姿态更新完成

 // 姿态更新完成
    // attitude update finish
    pvacur.att.qbn   = qnn * pvapre.att.qbn * qbb;
    pvacur.att.cbn   = Rotation::quaternion2matrix(pvacur.att.qbn);
    pvacur.att.euler = Rotation::matrix2euler(pvacur.att.cbn);
}

感谢武汉大学卫星导航定位技术研究中心多源智能导航实验室(i2Nav)牛小骥教授团队开源的KF-GINS软件平台。