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

INS导航系统更新算法

原创

已于 2023-01-31 12:53:18 修改 · 2.4k 阅读

21 ·

CC 4.0 BY-SA版权

文章标签：

#c++

于 2023-01-31 12:24:43 首次发布

本文详细介绍了INS导航系统中状态更新算法的实现过程，包括速度更新、位置更新及姿态更新的具体步骤。通过复杂的数学运算与矩阵变换，确保了导航系统的精确性和稳定性。

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.dv

最低0.47元/天解锁文章