组合导航中的杆臂测量

杆臂：是指坐标系A相对于坐标系B的位置在坐标系C中的投影，也称为力矩臂（moment arm）。
如GNSS/INS组合导航系统：由于惯导和 GNSS 天线两个硬件无法安装在同一个点上， IMU测量中心与 GNSS 天线相位中心必定不重合， 因此二者之间存在一个杆臂值。 杆臂值不正确会严重影响数据处理结果的精度指标。
杆臂值往往代表：天线相位中心与INS导航中心的相对位置关系，来实现GNSS导航参数与INS导航参数的转换和结合。（杆臂值得作用是：为了将位置结果统一，必须将GNSS位置解算的结果与IMU得到的测量值两者的原点归算至同一点，才能实现正确的数据融合）

加速度计的零偏单位转换

1mGal= 10^-5m/s²

受到经纬度以及高程影响的正常重力位的计算公式

OB_GINS对应的源代码

static double gravity(const Vector3d &blh) {
//blh为三维double类型的列向量，同时0位置存放的是B(纬度)，1位置L(经度)，2位置(高程)
        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];
    }

大地坐标系（LBH）向空间直角坐标系（XYZ）的转换及其逆转换

    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};

导航坐标系（n系）到地心地固坐标系（e系）的转换及其逆转换

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;
    }

最终由GNSS观测到的BLH大地坐标系到当地水平坐标系（n系）的转换

实现的过程是先从用LBH表示的大地坐标系转换成用XYZ表示的地心地固坐标系（ECEF系）
再通过地心地固坐标系（ECEF）转换成当地水平坐标系（n系）

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

        Vector3d ecef1 = blh2ecef(global);//GNSS观测文件中的LBH数据

        return cn0e.transpose() * (ecef1 - ecef0);
        //其中(ecef1 - ecef0)表示的是两次GNSS观测值之间的在ECEF的变换
        //同时e系到n系的转换只需要将n系到e系的转换矩阵求转置即可

对于将欧拉角转换为四元数

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()));
    }//旋转向量（3X1）:Eigen::AngleAxisd
	//四元数（4X1）:Eigen::Quaterniond
	//Eigen::AngleAxisd(euler[2], Vector3d::UnitZ())//表示：Eigen::AngleAxisd(角度, 旋转轴)
	//这里表示 Z-Y-X 的顺序（即先绕 Z 轴旋转 euler[2] 度，然后绕 Y 轴旋转 euler[1] 度，最后绕 X 轴旋转 euler[0] 度）创建了一个旋转变换
	//Eigen::Quaterniond q2(Matrix3d(R));// 第三种方式   则是用3x3的旋转矩阵初始化四元数

也就是下图的

杆臂补偿？

 .p    = gnss.blh - Rotation::euler2quaternion(initatt) * antlever,
 //GNSS的LBH坐标-初始姿态角*杆臂补偿 = IntegrationState state_curr时刻的位置P
 //我认为这行代码是这个意思？？？https://blog.csdn.net/wuwuku123/article/details/105269413（杆臂误差）

一些C++中的关键字回顾

explicit

explicit关键字：explicit关键字用来修饰类的构造函数，被修饰的构造函数的类，不能发生相应的隐式类型转换，只能以显示的方式进行类型转换。

GnssFileLoader() = delete;

等于将GnssFileLoader类的构造函数执行了删除操作——禁止调用 GnssFileLoader 的默认构造函数，

std::make_shared()函数

是 C++11 中引入的一个函数模板，用于动态分配内存并构造一个对象，返回指向该对象的 shared_ptr 智能指针。
动态内存分配: make_shared 可以动态分配内存来存储一个对象，这样就不需要显式地使用 new 运算符来分配内存。
对象构造: make_shared 还会在分配的内存中构造一个对象。通过 make_shared 可以直接传递构造函数的参数，用于初始化对象。
返回 shared_ptr: make_shared 返回一个 shared_ptr 智能指针，该指针可以管理所分配的对象的内存。shared_ptr 具有引用计数功能，可以确保在没有指向对象的指针时自动释放对象内存，从而避免内存泄漏。
性能优化: 由于 make_shared 一次性分配内存来存储对象和引用计数，可以减少内存碎片化，并提高性能。

  auto parameters          = std::make_shared<IntegrationParameters>();
  //用make_share函数动态分配一个IntegrationParameters对象，然后返回一个 shared_ptr 智能指针 parameters，指向这个动态分配的对象。

memcpy函数

memcpy函数的功能是从源src所指的内存地址的起始位置开始拷贝n个字节到目标dest所指的内存地址的起始位置中。

memcpy(gnss_.blh.data(), &data_[1], 3 * sizeof(double));
void *memcpy(void *dest, const void *src, size_t n);
//357473.000    30.4604325443   114.4725046685     23.000    0.008             0.011           0.036 
//此时，从源data_[1]所指的内存地址的起始位置，开始拷贝3个double类型的值，到gnss_.blh.data中（纬度  经度 高程）

std::deque

std::deque: 这表示使用了 C++ 标准库中的 std::deque，deque 是双端队列（double-ended queue）的缩写，是一种能够在两端进行高效插入和删除操作的数据结构。

std::shared_ptr

它是 C++ 标准库中的智能指针，用于管理动态分配的内存资源。shared_ptr 允许多个指针共享同一个对象，并且会在最后一个指向对象的 shared_ptr 被销毁时自动释放对象的内存

std::deque<std::shared_ptr<PreintegrationBase>> preintegrationlist;
//这段话表明有一个名为preintegrationlist的双端队列，其中存放了PreintegrationBase的类对象的智能指针
//这表明：在preintegrationlist的双端队列中，存在了多个管理和操作多个 PreintegrationBase 类型对象的shared_ptr 智能指针

constexpr函数

constexpr是c++11新添加的特征，目的是将运算尽量放在编译阶段，而不是运行阶段。这个从字面上也好理解，const是常量的意思，也就是后面不会发生改变，因此当然可以将计算的过程放在编译过程。constexpr可以修饰函数、结构体。

修饰的函数只能包括return 语句。
修饰的函数只能引用全局不变常量。
修饰的函数只能调用其他constexpr修饰的函数。
函数不能为void 类型和，并且prefix operation（v++）不允许出现。

对于多属性成员的变量初始化赋值的操作

IntegrationState state_curr = {
        .time = round(gnss.time),
        .p    = gnss.blh - Rotation::euler2quaternion(initatt) * antlever,
        .q    = Rotation::euler2quaternion(initatt),
        .v    = initvel,
        .bg   = initbg,
        .ba   = initba,
        .sodo = 0.0,
        .abv  = {bodyangle[1], bodyangle[2]},
    };

typedef struct IntegrationState {
    double time;

    Vector3d p{0, 0, 0};
    Quaterniond q{0, 0, 0, 0};
    Vector3d v{0, 0, 0};

    Vector3d bg{0, 0, 0};
    Vector3d ba{0, 0, 0};

    Vector3d s{0, 0, 0};
    double sodo{0};
    Vector2d abv{0, 0};

    Vector3d sg{0, 0, 0};
    Vector3d sa{0, 0, 0};
} IntegrationState;

static_cast 用法

static_cast < type-id > ( expression )

该运算符把expression转换为type-id类型，但没有运行时类型检查来保证转换的安全性。
用于类层次结构中基类和子类之间指针或引用的转换。进行上行转换（把子类的指针或引用转换成基类表示）是安全的；进行下行转换（把基类指针或引用转换成子类指针或引用）时，由于没有动态类型检查，所以是不安全的。

C++在函数前面加一个static的作用

在一般函数的前面加上static：表示该函数失去了全局可见性，只在该函数所在的文件作用域内可见
当函数声明为static以后,编译器在该目标编译单元内只含有该函数的入口地址,没有函数名,其它编译单元便不能通过该函数名来调用该函数。（没有名字，自然别的单元无法看到该函数，但是在本目标单元，通过提供一个函数的入口地址实现顺利进入函数执行static函数的功能）
在类的成员函数前面加上static作用是：成员函数是属于类的，而非对象的，也就是所有该类的对象共同拥有这一个成员函数，而不是普通的每个对象各自拥有一个成员函数

coeffs()

Eigen中的针对于四元数的coeffs（）函数是用于返回四元数的四个数(可修改)，可以对其进行索引[]获取值。需要注意的是返回顺序是x、y、z、w，和定义的时候是不一样的（Quaternion的构造是标准Eigen格式，特别需要注意四个数的传入顺序是w、x、y、z，对应w+xi+yj+zk）

round函数

C++中的round函数用来对浮点类型的数据进行四舍五入

double round(double d);//函数作用就是对浮点型进行四舍五入

vector中的emplace_back方法

emplace_back函数的作用是减少对象拷贝和构造次数，是C++11中的新特性，主要适用于对临时对象的赋值。
在使用push_back函数往容器中增加新元素时，必须要有一个该对象的实例才行。emplace_back可以不用，它可以直接传入对象的构造函数参数直接进行构造，减少一次拷贝和赋值操作
但是在理解上可以将emplace_back的使用意义理解成pushback的意义

C++中make_shared函数

make_shared函数的主要功能是在动态内存中分配一个对象并初始化它，返回指向此对象的shared_ptr;由于是通过shared_ptr管理内存，因此一种安全分配和使用动态内存的方法。
其中

1）make_shared是一个模板函数；
2）make_shared模板的使用需要以“显示模板实参”的方式使用，如上题所示make_shared(10, 9),如果不传递显示 模板实参string类型，make_shared无法从(10, ‘9’)两个模板参数中推断出其创建对象类型。
3）make_shared在传递参数格式是可变的，参数传递为生成类型的构造函数参数，因此在创建shared_ptr对象的过程中调用了类型T的某一个构造函数。(make_share依据类型，以及传入的参数，调用特定的构造函数，创建对象，并实现构造函数的初始化)

 preintegration = std::make_shared<PreintegrationEarthOdo>(parameters, imu0, state);
 

fabs函数

fabs函数是一个求绝对值的函数，求出x的绝对值

C ++ lround()函数 (C++ lround() function)

    lround(x);
    //x –是中途取整的数字，最接近零。
    //long int –返回long int类型值，该值是数字x的舍入值。

lround()函数是cmath标头的库函数，用于舍入给定值并将其转换为长整数，它接受一个数字并返回最接近该数字的整数(长int)值(有中途情况) )。
也就是感觉像四舍五入.

对于OB_GINS的主函数的理解

已经放在注释中

int main(int argc, char *argv[]) {

    if (argc != 2) {
        std::cout << "usage: ob_gins ob_gins.yaml" << std::endl;
        return -1;
    }

    std::cout << "\nOB_GINS: An Optimization-Based GNSS/INS Integrated Navigation System\n\n";

    auto ts = absl::Now();
	//参数设置
	{
    // 读取配置
    // load configuration
    YAML::Node config;
    std::vector<double> vec;
    try {
        config = YAML::LoadFile(argv[1]);
    } catch (YAML::Exception &exception) {
        std::cout << "Failed to read configuration file" << std::endl;
        return -1;
    }

    // 时间信息
    // processing time
    int windows   = config["windows"].as<int>();
    int starttime = config["starttime"].as<int>();
    int endtime   = config["endtime"].as<int>();

    // 迭代次数
    // number of iterations
    int num_iterations = config["num_iterations"].as<int>();

    // 进行GNSS粗差检测
    // Do GNSS outlier culling
    bool is_outlier_culling = config["is_outlier_culling"].as<bool>();

    // 初始化信息
    // initialization
    vec = config["initvel"].as<std::vector<double>>();
    Vector3d initvel(vec.data());
    vec = config["initatt"].as<std::vector<double>>();
    Vector3d initatt(vec.data());
    initatt *= D2R;

    vec = config["initgb"].as<std::vector<double>>();
    Vector3d initbg(vec.data());
    initbg *= D2R / 3600.0;//initgb(初始化陀螺零偏)为弧度
    vec = config["initab"].as<std::vector<double>>();
    Vector3d initba(vec.data());
    initba *= 1.0e-5;//初始化加表零偏（100mGal = 0.001m/s^(2)）——转换成m/s(2)

    // 数据文件
    // data file
    std::string gnsspath   = config["gnssfile"].as<std::string>();
    std::string imupath    = config["imufile"].as<std::string>();
    std::string outputpath = config["outputpath"].as<std::string>();
    int imudatalen         = config["imudatalen"].as<int>();
    int imudatarate        = config["imudatarate"].as<int>();

    // 是否考虑地球自转
    // consider the Earth's rotation
    bool isearth = config["isearth"].as<bool>();

    GnssFileLoader gnssfile(gnsspath);//调用GnssFileLoader的构造函数 执行初始化column = 7
    ImuFileLoader imufile(imupath, imudatalen, imudatarate);//初始化imu文件数据
    FileSaver navfile(outputpath + "/OB_GINS_TXT.nav", 11, FileSaver::TEXT);//保存文件的路径
    FileSaver errfile(outputpath + "/OB_GINS_IMU_ERR.bin", 7, FileSaver::BINARY);
    if (!imufile.isOpen() || !navfile.isOpen() || !navfile.isOpen() || !errfile.isOpen()) {
        std::cout << "Failed to open data file" << std::endl;
        return -1;
    }

    // 安装参数
    // installation parameters
    vec = config["antlever"].as<std::vector<double>>();//天线杆臂
    Vector3d antlever(vec.data());
    vec = config["odolever"].as<std::vector<double>>();//里程计杆臂
    Vector3d odolever(vec.data());
    vec = config["bodyangle"].as<std::vector<double>>();//IMU到载体的旋转角
    Vector3d bodyangle(vec.data());
    bodyangle *= D2R;//弧度制

    // IMU噪声参数
    // IMU noise parameters
    auto parameters          = std::make_shared<IntegrationParameters>();
    parameters->gyr_arw      = config["imumodel"]["arw"].as<double>() * D2R / 60.0;//转化为弧度每秒的平方（sqrt（3600）=60）
    parameters->gyr_bias_std = config["imumodel"]["gbstd"].as<double>() * D2R / 3600.0;//陀螺零偏标准差,deg/hr—/3600—> rad / s
    parameters->acc_vrw      = config["imumodel"]["vrw"].as<double>() / 60.0;//速度的随机游走m/s/sqrt(hr)  ——>m / s^0.5
    parameters->acc_bias_std = config["imumodel"]["abstd"].as<double>() * 1.0e-5;//mGal——>为m/s^-2   1mGal= 10^-5^m/s^2^ 
    parameters->corr_time    = config["imumodel"]["corrtime"].as<double>() * 3600;//相关时间s

    bool isuseodo       = config["odometer"]["isuseodo"].as<bool>();//运用odometer
    vec                 = config["odometer"]["std"].as<std::vector<double>>();
    parameters->odo_std = Vector3d(vec.data());//odo标准差
    parameters->odo_srw = config["odometer"]["srw"].as<double>() * 1e-6;//里程计比例因子随机游走, PPM / sqrt(Hz)  //表示表示每百万份之一所以是100/e-6
    parameters->lodo    = odolever;//b系下的里程计杆臂, m
    parameters->abv     = bodyangle;//IMU到载体的旋转角

    // GNSS仿真中断配置
    // GNSS outage parameters
    bool isuseoutage = config["isuseoutage"].as<bool>();//GNSS outage parameters
    int outagetime   = config["outagetime"].as<int>();//中断时间
    int outagelen    = config["outagelen"].as<int>();//中断长度
    int outageperiod = config["outageperiod"].as<int>();//中断周期

    auto gnssthreshold = config["gnssthreshold"].as<double>();//固定阈值GNSS抗差 (m)
	}
	//同样对于要处理的IMU以及GNSS的第一第一行数据进行初始化
	{
    // 数据文件调整
	//实现IMU数据与GNSS数据对齐
    // data alignment
    IMU imu_cur, imu_pre;
    do {
        imu_pre = imu_cur;
        imu_cur = imufile.next();
    } while (imu_cur.time < starttime);//获得对齐的IMU第一行数据

    GNSS gnss;
    do {
        gnss = gnssfile.next();
    } while (gnss.time < starttime);//获得对齐的GNSS第一行数据

    // 初始位置, 求相对
    Vector3d station_origin = gnss.blh;//初始站的经纬高//对齐的第一个数据的BLH对应着初始测站的大地坐标
    parameters->gravity     = Earth::gravity(gnss.blh);//计算位于LBH的当地正常重力
    gnss.blh                = Earth::global2local(station_origin, gnss.blh);//不应该输出0吗

    // 站心坐标系原点
    parameters->station = station_origin;

    std::vector<IntegrationState> statelist(windows + 1);//windows: 30
    std::vector<IntegrationStateData> statedatalist(windows + 1);//一次的状态数据列表能够存放30个
	//将30个状态变量的数据进行共同处理
    std::deque<std::shared_ptr<PreintegrationBase>> preintegrationlist;
    std::deque<GNSS> gnsslist;
    std::deque<double> timelist;

    Preintegration::PreintegrationOptions preintegration_options = Preintegration::getOptions(isuseodo, isearth); // use odometer  //考虑地球自转补偿项

    // 初始状态
    // initialization
	//系统状态初始化（数据已经对齐）
	//系统状态的初始化通过GNSS的信息来实现
    IntegrationState state_curr = {
        .time = round(gnss.time),//第一次就是对齐的GNSS数据对应的时间 
        .p    = gnss.blh - Rotation::euler2quaternion(initatt) * antlever,//initatt: [ 0, 0, 276 ] # 横滚俯仰航向 (RPY attitude), deg
       //antlever: [ -0.073, 0.302, 0.087 ]  # 天线杆臂 (antenna lever), IMU前右下方向, m
		.q    = Rotation::euler2quaternion(initatt),//初始四元数向量
        .v    = initvel,//北东地速度 
        .bg   = initbg,// 初始陀螺零偏
        .ba   = initba,// 初始加表零偏
        .sodo = 0.0,//里程计的比例因子
        .abv  = {bodyangle[1], bodyangle[2]},//IMU到载体的旋转角
		//b系与v系的安装角
		//bodyangle: [ 0, -0.30, -1.09 ]   表示IMU前右下方向，其中前向的IMU与载体对齐
    };
    std::cout << "Initilization at " << gnss.time << " s " << std::endl;

    statelist[0]     = state_curr;
    statedatalist[0] = Preintegration::stateToData(state_curr, preintegration_options);//3
	//将当前状态的数据提出，存档到statedatalist中
	使用preintegration_odo内置的stateToData函数
    gnsslist.push_back(gnss);//将GNSS当前的对齐的一行数据添加到gnsslist的双端队列的末尾中

    double sow = round(gnss.time);//对GNSS时间进行四舍五入取整
    timelist.push_back(sow);//将取整后的GNSS时间信息加入timelist的双端队列的末尾
	}
    // 初始预积分
    // Initial preintegration
    preintegrationlist.emplace_back(//
        Preintegration::createPreintegration(parameters, imu_pre, state_curr, preintegration_options));
	//parameters为噪声的参数   imu_pre表示IMU与GNSS数据对齐前一个时刻的数据（IMU的角度增量，IMU的速度增量，相邻时间间隔对应的数据）， 
	//state_curr当前时刻的状态
    // 读取下一个整秒GNSS
    gnss                = gnssfile.next();
    parameters->gravity = Earth::gravity(gnss.blh);//基于此时大地坐标系位置的重力值计算
    gnss.blh            = Earth::global2local(station_origin, gnss.blh);//BLH的变化

    // 边缘化信息
    std::shared_ptr<MarginalizationInfo> last_marginalization_info;
    std::vector<double *> last_marginalization_parameter_blocks;

    // 下一个积分节点
    sow += INTEGRATION_LENGTH;//达到下一个积分长度的时间结点
	//实现IMU预积分
    while (true) {
        if ((imu_cur.time > endtime) || imufile.isEof()) {
            break;
        }

        // 加入IMU数据
        // Add new imu data to preintegration
        preintegrationlist.back()->addNewImu(imu_cur);

        imu_pre = imu_cur;
        imu_cur = imufile.next();//下一时刻的IMU数据

        if (imu_cur.time > sow) {//也就是满足数据对齐
            // 当前IMU数据时间等于GNSS数据时间, 读取新的GNSS
            // add GNSS and read new GNSS
            if (fabs(gnss.time - sow) < MINIMUM_INTERVAL) {//可以近似堪称IMU与GNSS数据对齐
                gnsslist.push_back(gnss);//插入GNSS下一时刻数据

                gnss = gnssfile.next();
				//纬度、经度、高程的标准差大于gnssthreshold   gnssthreshold: 20.0 固定阈值GNSS抗差 (m)
                while ((gnss.std[0] > gnssthreshold) || (gnss.std[1] > gnssthreshold) ||
                       (gnss.std[2] > gnssthreshold)) {
                    gnss = gnssfile.next();//放弃当前行数据，进行下一时刻数据的预积分
                }

                // 中断配置
                // do GNSS outage
                if (isuseoutage) {//isuseoutage = true
                    if (lround(gnss.time) == outagetime) {//outagetime: 357900开始中断
                        std::cout << "GNSS outage at " << outagetime << " s" << std::endl;
                        for (int k = 0; k < outagelen; k++) {
                            gnss = gnssfile.next();
                        }
                        outagetime += outageperiod;//下一次中断的时间间隔
                    }
                }

                parameters->gravity = Earth::gravity(gnss.blh);//计算当地的重力
                gnss.blh            = Earth::global2local(station_origin, gnss.blh);
				//Vector3d station_origin = gnss.blh;计算BLH的变化量
                if (gnssfile.isEof()) {
                    gnss.time = 0;
                }
            }

            // IMU内插处理
            // IMU interpolation
			// sow = round(gnss.time);//对GNSS时间进行四舍五入取整
            int isneed = isNeedInterpolation(imu_pre, imu_cur, sow);
            if (isneed == -1) {
            } else if (isneed == 1) {//不需要插值
                preintegrationlist.back()->addNewImu(imu_cur);

                imu_pre = imu_cur;
                imu_cur = imufile.next();
            } else if (isneed == 2) {//需要插值
				//将imu_pre与imu_cur重新进行插值
                imuInterpolation(imu_cur, imu_pre, imu_cur, sow);
				//将imu_pre插值的数据插入到preintegrationlist的结尾
                preintegrationlist.back()->addNewImu(imu_pre);
            }