程序员经验分享-hwhale

OB_GINS程序框架

2023-05-16

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

机械编排：
在这里插入图片描述
预积分：
在这里插入图片描述
在这里插入图片描述

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

OB_GINS程序框架 的相关文章

  • 解决CentOS中安装Nextcloud出现“内部服务器错误

    这个问题困扰了其他人包括我很长时间了 xff1a 当我们在CentOS中安装好httpd php xff0c 将Nextcloud程序解压进网站根目录 xff0c 然后访问 内部服务器错误 服务器发生一个内部错误并且无法完成你的请求 如果多

  • 从0开始安装k8s1.25【最新k8s版本——20220904】

    文章目录 从0开始安装k8s1 25一 准备工作1 安装Vmware和虚拟机CentOS2 虚拟机CentOS环境初始化3 安装容器运行时Containerd 二 安装kubelet kubeadm kubectl1 使用阿里云镜像安装2

  • 面向对象分析

    面向对象概述 1 什么是面向对象 what 面向对象是一种编程思想 xff0c 是一种通过多个对象互相协作完成功能处理流程的编程思路 xff0c 在核心处理过程中 xff0c 又主要区分为类的声明定义 对象的创建使用 面向对象的三大特征 封

  • 8.3调试shell程序

    常见的一般错误 调试跟踪

  • JAVA知识复习

    1 JAVA中一个变量在使用之前必须被声明和初始化 xff1b 2 命名常量标识符final 3 int num 61 10 i 43 43 等价于 int num 61 10 i i 61 i 43 1 Note i 43 43 语句的优

  • 第十七届浙江大学宁波理工学院程序设计大赛 E（循环节）

    题目 xff1a 传送门 思路 43 代码 xff1a span class token comment 首先 xff0c k一定是偶数才能0和1数量相同 xff08 1 xff09 统计0 k 1个循环节位置上所包含的0或者1的数量 xf

  • 2021-07-10（浙大可视化暑期网上学校）可视化的感知与认知

    大纲 xff1a 我们在专注做某件事时 xff0c 无法感知到某些事物的变化 可视化可以作为一种辅助工具标注容易忽视的事物 我们感知时要集中精力 xff0c 可视化可以加强认知 xff0c 使我们放松 感知过程中的特征 xff1a 1 恒常

  • 2021-07-11（浙大可视化暑期网上学校）--可视化设计、编码、变换

    大纲 可视化设计 好的可视化 xff08 准则 xff09 xff1a 最少的墨水 最小的空间 传递最多的信息 1 真实度 xff1a 图形实体 xff08 眼见不一定为实 xff09 xff08 1 xff09 谎言因子 xff1a 在实

  • C++ prev()函数用法

    含义 xff1a pre具有前一个的意思 xff0c 该函数可用来获取一个距离指定迭代器 n 个元素的迭代器 具体用法如下图 xff1a include lt bits stdc 43 43 h gt using namespace std

  • js中+的特殊用法：将字符串类型转换为number类型

    当一个字符串全是数字时 xff0c 通过在它前面添加 43 使字符串类型转化为number类型 xff0c 如下图 xff1a 注意 xff1a 只有当字符串是数字时才行 xff0c 正负数都可以 xff0c 也可以带前导零 xff1b 但

  • Vue中的Vuex

    简介 1 组成部分 xff1a xff08 三个对象类型 xff09 Actions xff1a 用来接收从Vue Componts的方法和数值 xff0c 如果需要去调用数值也可以先去调用成功数值后再传给MutationMutation

  • Vue路由

    路由简介 1 简介 xff08 1 xff09 路由就是一组key value的对应关系 xff08 2 xff09 路由要经过路由器的管理 2 路由工作原理简介 xff1a xff08 1 xff09 目的 xff1a 为了实现导航区和展

  • exit的用法

    exit函数的作用 xff1a 是程序正常退出 使用方法 xff1a 1 exit 表示结束当前进程 2 exit xff08 1 xff09 结束当前进程并且返回1 xff1b 3 exit xff08 2 xff09 结束当前进程并且返

  • pat（甲级）1004（dfs）

    1004 Counting Leaves 30 xff08 30 分 xff09 A family hierarchy is usually presented by a pedigree tree Your job is to count

  • Arch Linux 安装指南（ArchISO 2014.02.01 适用）

    1 写在前 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 43 o 本指南编译自Arch Wiki 新手指南 x

  • 数字华容道有解的条件

    问题 xff1a 有n n的矩阵 xff0c 分别填入1 n m 1 xff0c 和0 xff0c 给出一个矩阵的局势 xff0c 问是否可以将这个局势变为有序的最初矩阵 解法 xff1a xff08 eg xff1a 图1的数字顺序 1

  • 一个简易的招聘网站的数据库设计过程

    1 user表的增删改查及相关功能 1 1 admin下的权限 xff1a 可以查询所有的user信息 xff08 除去密码 xff09 xff1a select user id mobile name gender birth nickn

  • Spring5课程的思维导图

    Spring5课程的思维导图 xff08 来源百知教育 的 孙哥说Spring5的笔记视频 xff0c 外加自己的一点点理解 xff09 知识点都在导图上 xff0c 图有点大 xff0c 加载有点慢 xff0c 这里截一点图放上面 xff

  • Oracle数据库安装记录--Linux系统

    环境准备 Oracle数据库版本 xff1a 19C 系统环境 xff1a Linux系统 centOS7 所需的安装包 xff08 以下安装包都需要 xff09 安装包一 xff1a oracle database preinsta 1

  • 将对象集合具有相同属性的分到一组

    工具说明 将对象集合具有相同属性的分到一组 xff0c 必须保证属性对象在每一个小集合中唯一 比如 xff1a 参数元素一 xff08 与顺序无关 xff09 xff1a 第一个 xff1a 主ID1 从ID1 元素1 第二个 xff1a

随机推荐

  • 基于Verilog键盘的实现

    请实现对4 4矩阵式健盘的按键识别参考书籍 Verilog HDL高级数字设计 由图可知 xff0c 键盘的每一列通过一个上拉电阻 xff0c 接到VCC 在检测有没有按键按下时 xff0c 我们可以 xff0c 先使行线同时为0 xff0

  • /nginx: error while loading shared libraries: libcrypto.so.1.1: cannot open shared object file: No

    在CentOS7下配置Nginx xff0c 但是一致都在报错 报错信息为 xff1a nginx error while loading shared libraries libcrypto so 1 1 cannot open shar

  • 【转载】sprintf的实现

    原文链接 我们已经知道printf 是控制台程序中最常用的函数 xff0c 作用是输入的字符 数字等信息拼成完整的句子并且输出到标准输出设备 显示器 控制台等 xff0c sprintf 函数命名与printf 函数及其相似又有什么作用呢

  • 【程序员读论文】题外篇：怎么读论文

    文章目录 1 如何高效读论文 xff1f 痛苦选择顺序笔记小结讨论 2 如何有针对地高效地阅读一篇学术论文 xff1f 3 一文教你如何快速高效阅读Paper xff08 硕士生版 xff09 前言Paper从哪来Paper怎么读Paper

  • Linux中C语言标准库glibc源码下载

    在这篇文章理清gcc libc glibc libc 43 43 libstdc 43 43 的关系 xff0c 我们大概理解了libc xff0c glibc之间的一些关系 下面我们就开了解一些Linux中C语言标准库glibc源码 在这

  • 50个常用SQL语句

    50个常用SQL语句 Student S Sname Sage Ssex 学生表 S 学号 xff0c 主键 Course C Cname T 课程表 C 课程号 xff0c 主键 SC S C score 成绩表 Teacher T Tn

  • 一文搞懂交叉编译，Windows和Linux的交叉编译

    文章目录 什么是交叉编译为什么要交叉编译工具链的种类 我们应该怎样建立交叉编译环境在Windows下交叉编译和调试树莓派软件一 Windows下编译树莓派程序二 用WSL来编译树莓派程序三 通过gdbserver远程调试 基于 MinGW

  • endnote 文献保留前三个作者

    1 问题描述 xff1a endnote使用GBT7714文献格式 xff0c 显示文献的全部作者 2 想要达到的效果 xff1a 最多显示三个作者 3 解决方法 xff1a 还不知道怎么弄 xff0c 看看以后再来补充 xff0c 心情烦

  • RTK_LIB 源码、可执行文件、rtkget、观测文件、星历文件（精密星历、广播星历）、精密钟差文件介绍

    1 RTK LIB开源程序下载 xff1a 点开rtklib链接 xff1a 下载最新版本的可执行文件和程序源码 2 GNSS数据处理需要的文件 2 1 伪距定位 xff1a spp 观测数据 xff08 0 xff09 导航星历 广播星历

  • RTKLIB ppp rtk_post

    1 实时ppp xff1a IGS MGEX数据处理中心的播发的实时轨道钟差产品 xff0c 结合广播星历 xff0c 实现实时定位 2 事后的 xff08 近似实时 xff09 xff1a 下载精密星历 钟差产品 xff0c 结合其他的精

  • vscode查看代码更新历史

    开源代码推出新版本后 xff0c 如何查看代码更改信息 1 首先打开vscode xff0c 点击左侧的插件管理器 xff0c 进入插件面板 xff0c 搜索Git Graph并安装 2 点击下图图标 xff0c 即可进入Git Graph

  • git更新代码

    一 git一般有很多分支 xff0c 我们clone到本地的时候一般都是master分支 xff0c 那么如何切换到其他分支呢 xff1f 主要命令如下 xff1a 1 查看本地分支文件信息 xff0c 确保更新时不产生冲突 span cl

  • char类型数组

    字符数组 xff08 一维 二维 xff09 字符数组是数组元素为char类型的一种数组 凡是适合数组的定义和赋值 xff0c 也都适合于字符数组 由于C语言没有提供字符串类型 xff0c 字符串一般用一维字符数组来存放 xff0c 而二维

  • ubuntu18.04 安装腾讯会议

    腾讯会议现在以及上线了Linux版本 xff0c 可以直接在腾讯会议官网下载linux 版本 xff0c 在官网点击免费下载 xff0c 可以直接下载Linux版本 腾讯会议下载链接 选择Linux版本 xff0c x86 64版本 xff

  • 2.树莓派系统备份

    树莓派使用SD卡来装载系统 xff0c 如果SD卡丢失或者损坏 xff0c 那么树莓派上的数据都会丢失 xff0c 所以一定要备份好SD卡 这篇文章可以帮你备份你的树莓派系统 主要内容为备份SD卡 xff0c 制作树莓派系统镜像以及在需要的

  • ic_gvins编译及环境配置问题解决

    RTK VIO松组合 对惯导精度要求较高 1 环境配置和编译 安装依赖项 span class token comment gcc 8 span span class token function sudo span span class

  • xrdp session: Login failed for display 0 树莓派 Linux

    可以通过其他方式例如ssh或者外接显示器来cat var log xrdp log查看错误日志 xff0c 我的错误日志显示为密码错误 xrdp的登录密码和账户就是你的系统用户名和密码 xff0c 不是另外的 这里我填的账户是pi xff0

  • EVO画图设置

    一 绘图设置 1 更改背景色和网格 span class token comment 白色网格 span evo config span class token builtin class name set span plot seabor

  • GINS_OB环境配置

    1 程序简介 武大开源GNSS INS松组合IMU预积分有考虑地球自传和不考虑两种形式可以灵活设置GNSS中断时间IMU可以和里程计进行融合 2 环境配置 span class token comment gcc 8 g 43 43 8 s

  • OB_GINS程序框架

    1 程序运行 span class token builtin class name cd span OB GINS span class token comment 编译好的可执行文件 xff1a bin ob gins xff0c 参数

热门标签