OB_GINS学习
组合导航中的杆臂测量
杆臂：是指坐标系A相对于坐标系B的位置在坐标系C中的投影，也称为力矩臂（moment arm）。
如GNSS/INS组合导航系统：由于惯导和 GNSS 天线两个硬件无法安装在同一个点上， IMU测量中心与 GNSS 天线相位中心必定不重合， 因此二者之间存在一个杆臂值。 杆臂值不正确会严重影响数据处理结果的精度指标。
杆臂值往往代表：天线相位中心与INS导航中心的相对位置关系，来实现GNSS导航参数与INS导航参数的转换和结合。（杆臂值得作用是：为了将位置结果统一，必须将GNSS位置解算的结果与IMU得到的测量值两者的原点归算至同一点，才能实现正确的数据融合）
加速度计的零偏单位转换
1mGal= 10-5m/s2
受到经纬度以及高程影响的正常重力位的计算公式
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);
}
