OB_GINS程序框架

1.程序运行

cd ~/OB_GINS
#编译好的可执行文件：./bin/ob_gins， 参数文件./dataset/ob_gins.yaml
./bin/ob_gins ./dataset/ob_gins.yaml

2.参数文件详解

# 数据文件，参考数据给的时间都是GPS周内秒，并未给定具体的GPS周。
imufile: "dataset/ADIS16465.txt"#惯导数据，惯导精度为战术级别。
gnssfile: "dataset/GNSS_RTK.pos"#GNSS数据，RTK定位结果。
outputpath: "dataset/"#文件输出路径

#滑窗大小设定为30s，这个参数有一篇英文论文进行过验证30s是精度和效率的最优选择。
#但是这个参数应该与载体运动的速度、IMU级别相关。之后博客将进行详细验证。
windows: 30
starttime: 357473#数据处理起始时间
endtime: 359090#数据处理结束时间

# 迭代次数
num_iterations: 20

# 进行GNSS粗差检测
is_outlier_culling: false

# 初始化信息
initvel: [ 0, 0, 0 ]   # 北东地速度 (NED velocity), m/s
initatt: [ 0, 0, 276 ] # 横滚俯仰航向 (RPY attitude), deg

# IMU初始误差
initgb: [ 0, 0, 0 ]  # 初始陀螺零偏 (gyroscope biases), deg/hr
initab: [ 0, 0, 0 ]  # 初始加表零偏 (accelerometer biases), mGal

# IMU文件数据列数 (7-imu only, 8-with odometer)，
#测试数据8列包含里程计数据，设置为7列则为纯惯导，并且原始数据是增量的形式。
imudatalen: 8
# IMU原始数据频率, Hz
imudatarate: 200

# 考虑地球自转补偿项
# consider the Earth's rotation
isearth: true

# 安装参数
# Installation parameters
antlever: [ -0.073, 0.302, 0.087 ]  # 天线杆臂 (antenna lever), IMU前右下方向, m
odolever: [ -0.64, -0.628, 2.122 ]  # 里程计杆臂 (odometer lever), IMU前右下方向, m
bodyangle: [ 0, -0.30, -1.09 ]       # IMU到载体的旋转角 (mouting angles to construct C_b^v), deg

#里程计参数
odometer:
    isuseodo: true              # 是否使用里程计

    std: [ 0.05, 0.05, 0.05 ]   # 里程标准差  m/s
    srw: 100                    # 比例因子误差 , PPM

# IMU噪声参数
imumodel:
    arw: 0.1     # deg/sqrt(hr)
    vrw: 0.1     # m/s/sqrt(hr)
    gbstd: 25.0   # deg/hr
    abstd: 200.0  # mGal
    corrtime: 1.0 # hr

# GNSS仿真中断配置
# GNSS outage parameters
isuseoutage: true
outagetime: 357900
outagelen: 60
outageperiod: 150

# 固定阈值GNSS抗差 (m)
gnssthreshold: 0.2

3.主程序

3.1 看主程序之前首先看编译文件（cmakelist），其他的先不用看主要看可执行文件的生成以及包含的相关代码文件。

file(GLOB_RECURSE SOURCE
        src/ob_gins.cc
        src/fileio/fileloader.cc
        src/fileio/filesaver.cc
        src/preintegration/preintegration_base.cc
        src/preintegration/preintegration_normal.cc
        src/preintegration/preintegration_earth.cc
        src/preintegration/preintegration_odo.cc
        src/preintegration/preintegration_earth_odo.cc)

include_directories(${PROJECT_SOURCE_DIR})

#生成可执行文件名字：${PROJECT_NAME}在本程序中为ob_gins
#相关代码文件： ${SOURCE}为上边的file中的.cc文件，一般主程序为第一个.cc文件
add_executable(${PROJECT_NAME} ${SOURCE})

注：如果想查看具体的变量名字可以使用下述语句：
message("变量名" ${PROJECT_NAME} )

3.2主程序ob_gins.cc
OB_GINS代码结构清晰，注释详细，因此本节主要对代码整体结构进行阐述，随后在此基础讲解具体程序实现思路。

主程序包含上述四个函数，具体含义如注释所示。代码的阅读直接从主函数开始，顺序阅读即可。

//main函数分为四部分，几乎主程序都可以如此划分
main（）
{
1.数据参数预处理
主要功能：读取参数文件，在OB_GINS中读取了，滑动窗口次数、处理数据时间段、优化迭代次数、初始速度、位置、IMU相关参数等。

2.程序初始化
主要作用：通过上一步读取的数据进行系统初始化。这个初始化详细讲一下
（1）设置站心坐标系（北东地）原点，更新当地重力参数。
（2）设置预积分选项：是否添加里程计，是否考虑地球自转
（3）初始状态（当前状态）设置：北东地位置、姿态、速度、加速度计陀螺仪偏置、安装角。

3.循环处理数据
（1）imu预积分
（2）利用GNSS数据进行优化
4.结果输出
}

3.3IMU预积分

//加入IMU数据并进行预积分
preintegrationlist.back()->addNewImu(imu_cur);

void PreintegrationBase::addNewImu(const IMU &imu) {
    imu_buffer_.push_back(imu);
    integrationProcess(imu_buffer_.size() - 1);
}

integrationProcess（）函数有很多个，具体选用与预积分选项设置相关，分为一下四种：

//imu预积分
 if (options == PREINTEGRATION_NORMAL) {
      preintegration = std::make_shared<PreintegrationNormal>(parameters, imu0, state);
 } 
// imu和里程计预积分
 else if (options == PREINTEGRATION_ODO) {
       preintegration = std::make_shared<PreintegrationOdo>(parameters, imu0, state);
  } 
 // 考虑地球自转的imu预积分
  else if (options == PREINTEGRATION_EARTH) {
        preintegration = std::make_shared<PreintegrationEarth>(parameters, imu0, state);
 }
 // 考虑地球自转的IMU和里程计的预积分 
 else if (options == PREINTEGRATION_EARTH_ODO) {
        preintegration = std::make_shared<PreintegrationEarthOdo>(parameters, imu0, state);
  }

不考虑地球自转的预积分

// imu预积分
void PreintegrationNormal::integrationProcess(unsigned long index) {
    IMU imu_pre = compensationBias(imu_buffer_[index - 1]);//获取零偏补偿后的速度，姿态增量
    IMU imu_cur = compensationBias(imu_buffer_[index]);

    // 预积分
    integration(imu_pre, imu_cur);

    // 更新系统状态雅克比和协方差矩阵
    updateJacobianAndCovariance(imu_pre, imu_cur);
}

// 积分
void PreintegrationBase::integration(const IMU &imu_pre, const IMU &imu_cur) {
    //！机械编排
    // 区间时间累积
    double dt = imu_cur.dt;
    delta_time_ += dt;

    end_time_           = imu_cur.time;
    current_state_.time = imu_cur.time;

    // 连续状态积分, 先位置速度再姿态

    // 速度增量考虑旋转和划桨误差
    Vector3d dvfb = imu_cur.dvel + 0.5 * imu_cur.dtheta.cross(imu_cur.dvel) +
                    1.0 / 12.0 * (imu_pre.dtheta.cross(imu_cur.dvel) + imu_pre.dvel.cross(imu_cur.dtheta));
    Vector3d dvel = current_state_.q.toRotationMatrix() * dvfb + gravity_ * dt;//速度增量

    current_state_.p += dt * current_state_.v + 0.5 * dt * dvel;
    current_state_.v += dvel;

    // 姿态圆锥误差补偿
    Vector3d dtheta = imu_cur.dtheta + 1.0 / 12.0 * imu_pre.dtheta.cross(imu_cur.dtheta);
    current_state_.q *= Rotation::rotvec2quaternion(dtheta);
    current_state_.q.normalize();

    //！ 预积分
    dvel = delta_state_.q.toRotationMatrix() * dvfb;
    
    //位置速度增量
    delta_state_.p += dt * delta_state_.v + 0.5 * dt * dvel;
    delta_state_.v += dvel;

    // 姿态增量
    delta_state_.q *= Rotation::rotvec2quaternion(dtheta);
    delta_state_.q.normalize();
}

机械编排，OB_GINS对于旋转误差和划桨误差处理方法是：“单子样+前一周期的方法”：
速度增量：



注：速度增量计算中，有害加速度可以分为两项1.载体运动和地球自转引起的哥氏加速度、载体运动造成的对地向心加速度和重力加速度。不考虑地球自转的预计积分中只考虑了重力加速度，（补充考虑地球自转的预积分中考虑了在此基础上考虑了哥氏加速度）

姿态增量：

注意这个并未考虑地球自转的影响，因此代码姿态更新时并未加入C^nn。

考虑地球自转的IMU预积分：

// 零偏补偿和积分
void PreintegrationEarth::integrationProcess(unsigned long index) {
    //零偏补偿
    IMU imu_pre = compensationBias(imu_buffer_[index - 1]);
    IMU imu_cur = compensationBias(imu_buffer_[index]);

    // 区间时间累积
    double dt = imu_cur.dt;
    delta_time_ += dt;

    end_time_           = imu_cur.time;
    current_state_.time = imu_cur.time;

    //b系速度增量=速度增量+旋转误差+划桨误差 对应公式（6）速度积分，b系速度增量detv^b_fm
    Vector3d dvfb = imu_cur.dvel + 0.5 * imu_cur.dtheta.cross(imu_cur.dvel) +
                    1.0 / 12.0 * (imu_pre.dtheta.cross(imu_cur.dvel) + imu_pre.dvel.cross(imu_cur.dtheta));
    
    // 哥氏项和重力项对应公式（6）速度积分(g^w-2w^w_ie x v^w_bm-1)dt
    Vector3d dv_cor_g = (gravity_ - 2.0 * iewn_.cross(current_state_.v)) * dt;

    // 地球自转补偿项, 省去了enwn项
    Vector3d dnn    = -iewn_ * dt;//对应公式（7）
    Quaterniond qnn = Rotation::rotvec2quaternion(dnn);
    //对应公式（6）n系速度增量
    Vector3d dvel =  0.5 * (Matrix3d::Identity() + qnn.toRotationMatrix()) * current_state_.q.toRotationMatrix() * dvfb + dv_cor_g;
        
    // 前后历元平均速度计算当前位置
    current_state_.p += dt * current_state_.v + 0.5 * dt * dvel;//相较于起点的当前位置
    current_state_.v += dvel;//当前速度

    // 缓存IMU时刻位置, 时间间隔为两个历元的间隔
    pn_.emplace_back(std::make_pair(dt, current_state_.p));

    // 姿态圆锥误差补偿
    Vector3d dtheta = imu_cur.dtheta + 1.0 / 12.0 * imu_pre.dtheta.cross(imu_cur.dtheta);
    //地球自转补偿
    current_state_.q = qnn * current_state_.q * Rotation::rotvec2quaternion(dtheta);
    current_state_.q.normalize();//当前姿态

    // 预积分
    // 起始时刻到当前时刻的地球自转等效旋转矢量
    dnn  = -(delta_time_ - 0.5 * dt) * iewn_;
    
    // 公式（11）中的R^bk-1_w，
    //q0_.inverse() * Rotation::rotvec2quaternion(dnn) * q0_地球自转补偿
    // * delta_state_.q上一时刻到当前时刻的速度增量，转换到起点坐标系。
    dvel = (q0_.inverse() * Rotation::rotvec2quaternion(dnn)  * q0_ * delta_state_.q).toRotationMatrix() 
    * dvfb;//当前b系速度增量(去除划桨误差和旋转误差的速度增量)
    
    // 到预积分起点的位置和速度增量
    delta_state_.p += dt * delta_state_.v + 0.5 * dt * dvel;
    delta_state_.v += dvel;

     // 到预积分起点的姿态增量
    delta_state_.q *= Rotation::rotvec2quaternion(dtheta);
    delta_state_.q.normalize();

    // 更新系统状态雅克比和协方差矩阵
    updateJacobianAndCovariance(imu_pre, imu_cur);
}

机械编排：

预积分：