IMU状态预积分代码实现 —— IMU状态预积分类

IMU状态预积分代码实现 —— IMU状态预积分类

  • 实现IMU状态预积分类

实现IMU状态预积分类

首先,实现预积分自身的结构。一个预积分类应该存储一下数据:

  • 预积分的观测量 △ R ~ i j , △ v ~ i j , △ p ~ i j \bigtriangleup \tilde{R} _{ij},\bigtriangleup \tilde{v} _{ij},\bigtriangleup \tilde{p} _{ij} R~ij,v~ij,p~ij
  • 预积分开始时的IMU零偏 b g , b a b_{g},b_{a} bg,ba
  • 在积分时期内的测量噪声 Σ i , k + 1 \Sigma _{i,k+1} Σi,k+1
  • 各积分量对IMU零偏的雅克比矩阵
  • 整个积分时间 △ t i j \bigtriangleup t_{ij} tij

以上都是必要的信息。除此之外,也可以将IMU的读数记录在预积分类中(当然,也可以不记录,因为都已经积分过了)。同时,IMU的测量噪声和零偏随机游走噪声也可以作为配置参数,写在预积分类中。

声明这个类

class IMUPreintegration {

参数配置项
其中包括:

  • 陀螺仪初始零偏
  • 加速度计初始零偏
  • 陀螺噪声
  • 加计噪声
    /// 参数配置项/// 初始的零偏需要设置,其他可以不改struct Options {Options() {}Vec3d init_bg_ = Vec3d::Zero();  // 初始零偏Vec3d init_ba_ = Vec3d::Zero();  // 初始零偏double noise_gyro_ = 1e-2;       // 陀螺噪声,标准差double noise_acce_ = 1e-1;       // 加计噪声,标准差};

构造函数

IMUPreintegration(Options options = Options());

中间省略函数的声明,之后再写。

下面完成类的成员变量定义
整体预积分时间

    double dt_ = 0;                          // 整体预积分时间

噪声矩阵,累积噪声矩阵 Σ i , k + 1 \Sigma _{i,k+1} Σi,k+1 ,测量噪声矩阵 C o v ( η d , k ) Cov(\eta_{d,k} ) Cov(ηd,k)

    Mat9d cov_ = Mat9d::Zero();              // 累计噪声矩阵Mat6d noise_gyro_acce_ = Mat6d::Zero();  // 测量噪声矩阵

预积分开始时的IMU零偏 b g , b a b_{g},b_{a} bg,ba

    // 零偏Vec3d bg_ = Vec3d::Zero();Vec3d ba_ = Vec3d::Zero();

预积分的观测量 △ R ~ i j , △ v ~ i j , △ p ~ i j \bigtriangleup \tilde{R} _{ij},\bigtriangleup \tilde{v} _{ij},\bigtriangleup \tilde{p} _{ij} R~ij,v~ij,p~ij

    // 预积分观测量SO3 dR_;Vec3d dv_ = Vec3d::Zero();Vec3d dp_ = Vec3d::Zero();

各积分量对IMU零偏的雅克比矩阵

    // 雅可比矩阵Mat3d dR_dbg_ = Mat3d::Zero();Mat3d dV_dbg_ = Mat3d::Zero();Mat3d dV_dba_ = Mat3d::Zero();Mat3d dP_dbg_ = Mat3d::Zero();Mat3d dP_dba_ = Mat3d::Zero();

因为IMU零偏相关的噪声项并不直接和预积分类有关,所以将它们挪到优化类当中。这个类主要完成对IMU数据进行预积分操作,然后提供积分之后的观测量与噪声值。

下面来看单个IMU的积分函数,首先在类中进行声明。

    /*** 插入新的IMU数据* @param imu   imu 读数* @param dt    时间差*/void Integrate(const IMU &imu, double dt);

来看函数具体实现

整体而言,它按照以下顺序更新内部的成员变量:

  1. 更新位置和速度的测量值
  2. 更新运动模型的噪声矩阵
  3. 更新观测量对零偏的各雅克比矩阵
  4. 更新旋转部分的测量值
  5. 更新积分时间在这里插入代码片
void IMUPreintegration::Integrate(const IMU &imu, double dt) {

去掉零偏的测量

    Vec3d gyr = imu.gyro_ - bg_;  // 陀螺Vec3d acc = imu.acce_ - ba_;  // 加计

更新预积分速度观测量和位置观测量

        // 更新dv, dpdp_ = dp_ + dv_ * dt + 0.5f * dR_.matrix() * acc * dt * dt;dv_ = dv_ + dR_ * acc * dt;

对应公式为
在这里插入图片描述
在这里插入图片描述
预积分旋转观测 dR先不更新,因为A, B阵还需要现在的dR

下面计算运动方程雅克比矩阵系数A、B阵,用于更新噪声项
在这里插入图片描述
在这里插入图片描述

    // 运动方程雅可比矩阵系数,A,B阵,// 另外两项在后面Eigen::Matrix<double, 9, 9> A;A.setIdentity();Eigen::Matrix<double, 9, 6> B;B.setZero();

加速度计的伴随矩阵和t的平方

    Mat3d acc_hat = SO3::hat(acc);double dt2 = dt * dt;

公式中的这个地方有用到,避免重复计算
在这里插入图片描述

    A.block<3, 3>(3, 0) = -dR_.matrix() * dt * acc_hat;A.block<3, 3>(6, 0) = -0.5f * dR_.matrix() * acc_hat * dt2;A.block<3, 3>(6, 3) = dt * Mat3d::Identity();

计算A矩阵中对应的各个块,分别对应公式如下,A矩阵中的A.block<3, 3>(0, 0)块,之后用更新完的dR 更新
在这里插入图片描述

    B.block<3, 3>(3, 3) = dR_.matrix() * dt;B.block<3, 3>(6, 3) = 0.5f * dR_.matrix() * dt2;

更新B矩阵的各块,分别对应公式如下
在这里插入图片描述

    // 更新各雅可比dP_dba_ = dP_dba_ + dV_dba_ * dt - 0.5f * dR_.matrix() * dt2;                     dP_dbg_ = dP_dbg_ + dV_dbg_ * dt - 0.5f * dR_.matrix() * dt2 * acc_hat * dR_dbg_; dV_dba_ = dV_dba_ - dR_.matrix() * dt;                                             dV_dbg_ = dV_dbg_ - dR_.matrix() * dt * acc_hat * dR_dbg_;     

更新各雅克比矩阵对应公式依次为:
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
下面更新预积分旋转部分观测量

    // 旋转部分Vec3d omega = gyr * dt;         // 转动量Mat3d rightJ = SO3::jr(omega);  // 右雅可比SO3 deltaR = SO3::exp(omega);   // exp后dR_ = dR_ * deltaR;             // 更新预积分旋转部分观测量

对应公式:
在这里插入图片描述
其中右雅克比矩阵的计算是为了更新上面的B矩阵

    A.block<3, 3>(0, 0) = deltaR.matrix().transpose();B.block<3, 3>(0, 0) = rightJ * dt;

利用更新完的dR和右雅克比矩阵更新A、B阵中对应的块
对应公式:
在这里插入图片描述

    // 更新噪声项cov_ = A * cov_ * A.transpose() + B * noise_gyro_acce_ * B.transpose();

利用填充好的A阵和B阵,来更新噪声项
对应公式如下:
在这里插入图片描述
其中 C o v ( η d , k ) Cov(\eta_{d,k} ) Cov(ηd,k)即代码中的noise_gyro_acce_的构成就是陀螺仪和加计的噪声构成的对角矩阵,在构造函数中构成的

    const float ng2 = options.noise_gyro_ * options.noise_gyro_;const float na2 = options.noise_acce_ * options.noise_acce_;noise_gyro_acce_.diagonal() << ng2, ng2, ng2, na2, na2, na2;

下则继续更新预积分旋转观测量对陀螺仪零偏的雅克比矩阵

    // 更新dR_dbgdR_dbg_ = deltaR.matrix().transpose() * dR_dbg_ - rightJ * dt;  

对应公式如下:
在这里插入图片描述

最后增加积分时间:

    // 增量积分时间dt_ += dt;

这样就完成了一次对IMU数据的操作。需要注意的是,如果不进行优化,则预积分和直接积分的效果是完全一致的,都是将IMU数据一次性地积分。在预积分之后,也可以向ESKF一样,从起始状态向最终状态进行预测。

预测函数实现如下:

    /*** 从某个起始点开始预测积分之后的状态* @param start 起始时时刻状态* @return  预测的状态*/NavStated IMUPreintegration::Predict(const sad::NavStated &start, const Vec3d &grav) const {SO3 Rj = start.R_ * dR_;Vec3d vj = start.R_ * dv_ + start.v_ + grav * dt_;Vec3d pj = start.R_ * dp_ + start.p_ + start.v_ * dt_ + 0.5f * grav * dt_ * dt_;auto state = NavStated(start.timestamp_ + dt_, Rj, pj, vj);state.bg_ = bg_;state.ba_ = ba_;return state;}

与ESKF不同的是,预积分可以对多个IMU数据进行预测,可以从任意起始时刻向后预测,而ESKF通常只在当前状态下,针对单个IMU数据,向下一时刻预测。

获取修正之后的观测量,bias可以与预积分时期的不同,会有一阶修正

// 预积分旋转零偏更新修正后测量值
SO3 IMUPreintegration::GetDeltaRotation(const Vec3d &bg) { return dR_ * SO3::exp(dR_dbg_ * (bg - bg_)); }

对应公式为:
在这里插入图片描述
预积分速度零偏更新修正后测量值

    // 预积分速度零偏更新修正后测量值Vec3d IMUPreintegration::GetDeltaVelocity(const Vec3d &bg, const Vec3d &ba) {return dv_ + dV_dbg_ * (bg - bg_) + dV_dba_ * (ba - ba_);}

对应公式为:
在这里插入图片描述
预积分位置零偏更新修正后测量值

    // 预积分位置零偏更新修正后测量值Vec3d IMUPreintegration::GetDeltaPosition(const Vec3d &bg, const Vec3d &ba) {return dp_ + dP_dbg_ * (bg - bg_) + dP_dba_ * (ba - ba_);}

对应公式为:
在这里插入图片描述

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.rhkb.cn/news/337069.html

如若内容造成侵权/违法违规/事实不符,请联系长河编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

Superset二次开发之更新 SECRET_KEY

SECRET_KEY 的作用 加密和签名:SECRET_KEY用于对敏感数据(如会话、cookie、CSRF令牌)进行加密和签名,防止数据被篡改。安全性:确保应用的安全性,防止跨站请求伪造(CSRF)攻击和会话劫持等安全问题。如何生成 SECRET_KEY openssl rand -base64 42 配置 SECRET_KEY 在sup…

git使用流程与规范

原文网址&#xff1a;git代码提交流程与规范-CSDN博客 简介 本文git提交流程与规范是宝贵靠谱的经验&#xff0c;它能解决如下问题&#xff1a; 分支差距过大&#xff0c;导致合代码无数的冲突合完代码后发现代码丢失分支不清晰&#xff0c;无法追溯问题合代码耗时很长&…

使用Spring Boot自定义注解 + AOP实现基于IP的接口限流和黑白名单

&#x1f604; 19年之后由于某些原因断更了三年&#xff0c;23年重新扬帆起航&#xff0c;推出更多优质博文&#xff0c;希望大家多多支持&#xff5e; &#x1f337; 古之立大事者&#xff0c;不惟有超世之才&#xff0c;亦必有坚忍不拔之志 &#x1f390; 个人CSND主页——Mi…

数据在内存中的存储<C语言>

导言 在计算机中不同类型的数据在计算机内部存储形式各不相同&#xff0c;弄懂各种数据在计算机内部存储形式是有必要的&#xff0c;C语言的学习不能浮于表面&#xff0c;更要锻炼我们的“内功”&#xff0c;将来在写程序的时候遇见各种稀奇古怪的bug时&#xff0c;也便能迎刃而…

应用案例|精密制造中使用复合机器人得到显著提升

精密制造行业对设备的精度、稳定性和效率要求极高&#xff0c;而复合机器人凭借其多功能性、高度灵活性和精准控制能力&#xff0c;正逐渐成为该领域的新宠。以下是一个富唯智能复合机器人在精密制造中的应用案例。 案例背景 某知名汽车零部件制造企业&#xff0c;专注于生产…

u盘文件保密的方法有哪些?关于U盘的使用你要知道这些!

U盘作为便携式的存储设备&#xff0c;被广泛应用于日常工作和生活中。 然而&#xff0c;U盘的丢失或被盗可能导致敏感数据泄露&#xff0c;因此&#xff0c;掌握U盘文件保密的方法至关重要。 本文将介绍几种有效的U盘文件保密方法&#xff0c;并分享关于U盘使用的关键知识&…

Threejs(WebGL)绘制线段优化:Shader修改gl.LINES模式为gl.LINE_STRIP

目录 背景 思路 Threejs实现 记录每条线的点数 封装原始裁剪索引数据 封装合并几何体的缓冲数据&#xff1a;由裁剪索引组成的 IntArray 守住该有的线段&#xff01; 修改顶点着色器 修改片元着色器 完整代码 WebGL实现类似功能&#xff08;简易版&#xff0c;便于测…

cdo | 常用命令

整理一下平时经常会使用的cdo命令 如何来更改netcdf数据中的变量名呢&#xff1f; 假设我现在有一个sst月平均数据,希望将里面的变量名称sst修改为sst_new netcdf oisst_monthly { dimensions:lat 180 ;lon 360 ;time UNLIMITED ; // (476 currently)nbnds 2 ; variable…

音视频开发14 FFmpeg 视频 相关格式分析 -- H264 NALU格式分析

H264简介-也叫做 AVC H.264&#xff0c;在MPEG的标准⾥是MPEG-4的⼀个组成部分–MPEG-4 Part 10&#xff0c;⼜叫Advanced Video Codec&#xff0c;因此常常称为MPEG-4 AVC或直接叫AVC。 原始数据YUV,RGB为什么要压缩-知道就行 在⾳视频传输过程中&#xff0c;视频⽂件的传输…

Element快速入门

Vue组件库Element 1 Element介绍 vue是侧重于VM开发的&#xff0c;主要用于数据绑定到视图的&#xff0c;ElementUI就是一款侧重于V开发的前端框架&#xff0c;主要用于开发美观的页面的。 Element&#xff1a;是饿了么公司前端开发团队提供的一套基于 Vue 的网站组件库&…

使用pytorch搭建textCNN、BERT、transformer进行文本分类

首先展示数据处理后的类型&#xff1a; 第一列为文本&#xff0c;第二类为标注的标签&#xff0c;数据保存在xlsx的表格中&#xff0c;分为训练集和验证集。 textCNN 直接上整个工程代码&#xff1a; import pandas as pd import numpy as np import torch from torch.util…

SAPUI5基础知识3 - 引导过程(Bootstrap)

1. 背景 在上一篇博客中&#xff0c;我们已经建立出了第一个SAPUI5项目&#xff0c;接下来&#xff0c;我们将为这个项目添加引导过程。 在动手练习之前&#xff0c;让我们先解释一下什么引导过程。 1.1 什么是引导过程&#xff1f; 在计算机科学中&#xff0c;引导过程也称…

Presto 从提交SQL到获取结果 源码详解(3)

物理执行计划 回到SqlQueryExecution.startExecution() &#xff0c;执行计划划分以后&#xff0c; // 初始化连接&#xff0c;获取Connect 元数据&#xff0c;添加会话&#xff0c;初始ConnectId metadata.beginQuery(getSession(), plan.getConnectors()); // 构建物理执行…

你真的会用收藏夹吗?可道云teamOS收藏夹,竟能缩短多层级文件夹的路径,实现快速访问

在日常工作中&#xff0c;我们时常会面临一个让人头疼的问题&#xff1a;如何在海量的文件和资料中快速找到我们需要的那一份&#xff1f; 尤其是在团队协作中&#xff0c;每个人都在不断地上传、更新文件……导致文件目录层级复杂&#xff0c;搜索也变得繁琐。 这时候&#x…

编程学习 (C规划) 6 {24_4_18} 七 ( 简单扫雷游戏)

首先我们要清楚扫雷大概是如何实现的&#xff1a; 1.布置雷 2.扫雷&#xff08;排查雷&#xff09; &#xff08;1&#xff09;如果这个位置是雷就炸了&#xff0c;游戏结束 &#xff08;2&#xff09;如果不是雷&#xff0c;就告诉周围有几个雷 3.把所有不是雷的位置都找…

一周学会Django5 Python Web开发 - Django5内置Admin系统二次开发

锋哥原创的Python Web开发 Django5视频教程&#xff1a; 2024版 Django5 Python web开发 视频教程(无废话版) 玩命更新中~_哔哩哔哩_bilibili2024版 Django5 Python web开发 视频教程(无废话版) 玩命更新中~共计56条视频&#xff0c;包括&#xff1a;2024版 Django5 Python we…

【C++】Vector的简易模拟与探索

&#x1f49e;&#x1f49e; 前言 hello hello~ &#xff0c;这里是大耳朵土土垚~&#x1f496;&#x1f496; &#xff0c;欢迎大家点赞&#x1f973;&#x1f973;关注&#x1f4a5;&#x1f4a5;收藏&#x1f339;&#x1f339;&#x1f339; &#x1f4a5;个人主页&#x…

模型 FABE(特性 优势 好处 证据)法则

说明&#xff1a;系列文章 分享 模型&#xff0c;了解更多&#x1f449; 模型_思维模型目录。特性、优势、好处、证据&#xff0c;一气呵成。 1 FABE法则的应用 1.1 FABE法则营销商用跑步机 一家高端健身器材公司的销售代表正在向一家新开的健身房推销他们的商用跑步机。以下…

Microsoft Dynamics 365 Business Central 讲解VAT RATE CHANGE TOOL(增值税税率更改工具)

学习目标&#xff1a; 如果使用VAT RATE CHANGE TOOL&#xff08;增值税税率更改工具&#xff09; 过程演示&#xff1a; 1.创建新的VAT产品过账组 2.创建新的总账科目以过账采购、销售和逆向征收增值税。 3.给新的VAT产品过账设置过账设置 4.创建一个新的一般产品过账组 5…

CUDA学习(2)

什么是CUDA CUDA&#xff08;Compute Unified Device Architecture&#xff09;&#xff0c;统一计算设备架构&#xff0c;英伟达推出的基于其GPU的通用高性能计算平台和编程模型。 借助CUDA&#xff0c;开发者可以充分利用英伟达GPU的强大计算能力加速各种计算任务。 软件生…