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

原创

已于 2023-01-31 12:54:29 修改 · 2.9k 阅读

25 ·

CC 4.0 BY-SA版权

文章标签：

#算法

于 2023-01-27 17:16:47 首次发布

该博客围绕Earth.h文件展开，基于Eigen库矩阵计算，涵盖WGS84确定椭球模型参数、计算重力、子午圈与卯酉圈半径等内容，还涉及不同坐标系间的转换及地球自转角速度投影等，部分代码与严老师PSINS相近，部分公式知乎有大佬总结。

Earth.h文件

基于Eigen库矩阵计算。

1、WGS84确定椭球模型参数

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; /* 第二偏心率平方 */

2、计算重力

class Earth {

public:
    /* 正常重力计算 */
    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];
    }

与严老师PSINS中的代码相近

eth.g = eth.g0*(1+5.27094e-3*eth.sl2+2.32718e-5*sl4)-3.086e-6*pos(3); % grs80

3、计算子午圈半径

 /* 计算子午圈半径和卯酉圈半径 */
    static Eigen::Vector2d meridianPrimeVerticalRadius(double lat) {
        double tmp, sqrttmp;

        tmp = sin(lat);
        tmp *= tmp;
        tmp     = 1 -