GNSS-INS组合导航:KF-GINS(一）

因为毕业设计需求，最近一个月学习了开源程序KF-ginss。

这几篇文章介绍松组合导航是如何实现的，总体大概分为三部分、第一部分介绍常用的坐标系转换、大地测量学基础、角度转化，第二部分介绍组合导航、状态转移矩阵如何实现、怎样进行卡尔曼滤波更新、第三部分介绍捷联惯导部分。

Gi_engine_process.cpp文件

 Cov_.resize(RANK, RANK);
    Qc_.resize(NOISERANK, NOISERANK);
    dx_.resize(RANK, 1);
    Cov_.setZero();
    Qc_.setZero();
    dx_.setZero();

1、设置协方差矩阵，系统噪声阵和系统误差状态矩阵大小

假设有离散线性系统，k 时刻的系统状态Xk受系统噪声序列wk−1 驱动；系统状态方程及量测方
程均为系统状态量的线性方程：

 wk 与vk 是互不相关的零均值白噪声序列:

Qk为状态噪声方差阵,Rk为量测噪声方差阵、

 auto imunoise                   = options_.imunoise;
    Qc_.block(ARW_ID, ARW_ID, 3, 3) = imunoise.gyr_arw.cwiseProduct(imunoise.gyr_arw).asDiagonal();
    Qc_.block(VRW_ID, VRW_ID, 3, 3) = imunoise.acc_vrw.cwiseProduct(imunoise.acc_vrw).asDiagonal();
    Qc_.block(BGSTD_ID, BGSTD_ID, 3, 3) =
        2 / imunoise.corr_time * imunoise.gyrbias_std.cwiseProduct(imunoise.gyrbias_std).asDiagonal();
    Qc_.block(BASTD_ID, BASTD_ID, 3, 3) =
        2 / imunoise.corr_time * imunoise.accbias_std.cwiseProduct(imunoise.accbias_std).asDiagonal();
    Qc_.block(SGSTD_ID, SGSTD_ID, 3, 3) =
        2 / imunoise.corr_time * imunoise.gyrscale_std.cwiseProduct(imunoise.gyrscale_std).asDiagonal();
    Qc_.block(SASTD_ID, SASTD_ID, 3, 3) =
        2 / imunoise.corr_time * imunoise.accscale_std.cwiseProduct(imunoise.accscale_std).asDiagonal();

2、初始化系统噪声阵：在噪声阵其实分为六个部分

陀螺和加速度计零偏及比例因子误差均建模为一阶高斯马尔可夫过程

 initialize(options_.initstate, options_.initstate_std);

设置系统状态(位置、速度、姿态和IMU误差)初值和初始协方差，

pvacur_.pos       = initstate.pos;
    pvacur_.vel       = initstate.vel;
    pvacur_.att.