组合导航中的杆臂测量

杆臂：是指坐标系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 configurationYAML::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 timeint windows   = config["windows"].as<int>();int starttime = config["starttime"].as<int>();int endtime   = config["endtime"].as<int>();// 迭代次数// number of iterationsint num_iterations = config["num_iterations"].as<int>();// 进行GNSS粗差检测// Do GNSS outlier cullingbool is_outlier_culling = config["is_outlier_culling"].as<bool>();// 初始化信息// initializationvec = 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 filestd::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 rotationbool isearth = config["isearth"].as<bool>();GnssFileLoader gnssfile(gnsspath);//调用GnssFileLoader的构造函数 执行初始化column = 7ImuFileLoader 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 parametersvec = 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 parametersauto 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 / sparameters->acc_vrw      = config["imumodel"]["vrw"].as<double>() / 60.0;//速度的随机游走m/s/sqrt(hr)  ——>m / s^0.5parameters->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;//相关时间sbool isuseodo       = config["odometer"]["isuseodo"].as<bool>();//运用odometervec                 = 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-6parameters->lodo    = odolever;//b系下的里程计杆臂, mparameters->abv     = bodyangle;//IMU到载体的旋转角// GNSS仿真中断配置// GNSS outage parametersbool isuseoutage = config["isuseoutage"].as<bool>();//GNSS outage parametersint 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 alignmentIMU 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: 30std::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 preintegrationpreintegrationlist.emplace_back(//Preintegration::createPreintegration(parameters, imu_pre, state_curr, preintegration_options));//parameters为噪声的参数   imu_pre表示IMU与GNSS数据对齐前一个时刻的数据（IMU的角度增量，IMU的速度增量，相邻时间间隔对应的数据）， //state_curr当前时刻的状态// 读取下一个整秒GNSSgnss                = 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 preintegrationpreintegrationlist.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 GNSSif (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 outageif (isuseoutage) {//isuseoutage = trueif (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);}