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);
来看函数具体实现
整体而言,它按照以下顺序更新内部的成员变量:
- 更新位置和速度的测量值
- 更新运动模型的噪声矩阵
- 更新观测量对零偏的各雅克比矩阵
- 更新旋转部分的测量值
- 更新积分时间在这里插入代码片
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_);}
对应公式为:

相关文章:
IMU状态预积分代码实现 —— IMU状态预积分类
IMU状态预积分代码实现 —— IMU状态预积分类 实现IMU状态预积分类 实现IMU状态预积分类 首先,实现预积分自身的结构。一个预积分类应该存储一下数据: 预积分的观测量 △ R ~ i j , △ v ~ i j , △ p ~ i j \bigtriangleup \tilde{R} _{ij},\bigtrian…...
C语言编程:探索最小公倍数的奥秘
C语言编程:探索最小公倍数的奥秘 在编程的世界中,计算两个数的最小公倍数(LCM)是一个常见的数学问题。C语言作为一种基础且强大的编程语言,为我们提供了实现这一功能的工具。本文将从四个方面、五个方面、六个方面和七…...
Java设计模式-活动对象与访问者
活动对象 Java设计模式中,活动对象是指一个对象始终处于活动的状态,该对象包括一个线程安全的数据结构以及一个活跃的执行线程。 如上所示,ActiveCreature类的构造函数初始化一个线程安全的数据结构(阻塞队列)、初始化…...
用HAL库改写江科大的stm32入门-6-3 PWM驱动LED呼吸灯
接线图: 2 :实验目的: 利用pwm实现呼吸灯。 关键PWM定时器设置: 代码部分: int main(void) {/* USER CODE BEGIN 1 *//* USER CODE END 1 *//* MCU Configuration--------------------------------------------------------*…...
[数据集][目标检测]喝水检测数据集VOC+YOLO格式995张3类别
数据集格式:Pascal VOC格式YOLO格式(不包含分割路径的txt文件,仅仅包含jpg图片以及对应的VOC格式xml文件和yolo格式txt文件) 图片数量(jpg文件个数):995 标注数量(xml文件个数):995 标注数量(txt文件个数):995 标注类别…...
【C++】开源:RabbitMQ安装与配置使用(SimpleAmqpClient)
😏★,:.☆( ̄▽ ̄)/$:.★ 😏 这篇文章主要介绍。 无专精则不能成,无涉猎则不能通。——梁启超 欢迎来到我的博客,一起学习,共同进步。 喜欢的朋友可以关注一下,下次更新不迷路…...
git使用流程与规范
原文网址:git代码提交流程与规范-CSDN博客 简介 本文git提交流程与规范是宝贵靠谱的经验,它能解决如下问题: 分支差距过大,导致合代码无数的冲突合完代码后发现代码丢失分支不清晰,无法追溯问题合代码耗时很长&…...
力扣 264. 丑数 II python AC
堆 from heapq import heappop, heappushclass Solution:def nthUglyNumber(self, n):q [1]vis {1}for _ in range(n - 1):now heappop(q)for i in [2, 3, 5]:if now * i not in vis:vis.add(now * i)heappush(q, now * i)return heappop(q)...
resetlogs强制拉库失败并使用备份system文件还原数据库故障处理---惜分飞
接手一个库,在open的过程中遭遇到ORA-600 2662错误 Sun May 26 10:15:54 2024 alter database open RESETLOGS RESETLOGS is being done without consistancy checks. This may result in a corrupted database. The database should be recreated. RESETLOGS after incomplete…...
解析Java中1000个常用类:Error类,你学会了吗?
在 Java 编程中,异常处理是一个至关重要的部分。Java 提供了丰富的异常处理机制,包括 Exception 和 Error。 本文将深入探讨 Error 类的功能、用法、实际应用中的注意事项,以及如何处理和预防这些错误。 什么是 Error 类? Error 类是 Java 中 Throwable 类的一个子类,用…...
【C++】——string模拟实现
前言 string的模拟实现其实就是增删改查,只不过加入了类的概念。 为了防止与std里面的string冲突,所以这里统一用String。 目录 前言 一 初始化和销毁 1.1 构造函数 1.2 析构函数 二 迭代器实现 三 容量大小及操作 四 运算符重载 4.1 bool…...
unity2D跑酷游戏
项目成果 项目网盘 导入资源包 放入Assets文件Assets资源文件 游戏流程分析 摄像机size调小,让图片占满屏幕 人跑本质,相对运动,图片无限向右滚动 图片720,缩小100倍第二个图片x为7.2每unit px100两张图片刚好挨着连贯 空对象Bg…...
OWASP top10--SQL注入(四、sqlmap安装及使用)
目录 sqlmap工具安装: 工具说明: 主要功能特性包括: 基本使用示例: 先下载python2.7.9版本 sqlmap运行 sqlmap工具使用 -u -r –-levelLEVEL扫描深度级别 --riskRISK 执行测试的风险 -threads 线程数 -batch-smart智能…...
Java基础入门day62
day62 AJAX 概念 AJAX: Asynchronous Javascript And XML AJAX是一种无需重新加载整个网页的情况下,能够更新部分网页的技术 AJAX是一种用于创建快速动态网页的技术 通过在后台与服务器进行少量数据交换,AJAX可以使网页实现异步更新 传统…...
Oracle中两张表具有相同结构,如何将一张表内容全部插入到另一个表中
在Oracle中,如果两张表具有相同的结构,你可以使用INSERT INTO ... SELECT语句将一张表的内容插入到另一张表中。以下是一个示例: 假设有两个表:table1 和 table2,它们具有相同的列结构。要将 table1 的所有内容插入到…...
比特币的理论上限是多少个?
标签: 比特币的理论上限; 已经挖出多少个比特币; 问题:比特币的理论上限是多少个?截至2023年10月,已经挖出多少个比特币出来了? 比特币的理论上限 比特币的设计者中本聪在比特币协议中设定了比…...
LeetCode-131 分割回文串
LeetCode-131 分割回文串 题目描述解题思路C 代码 题目描述 给你一个字符串 s,请你将 s 分割成一些子串,使每个子串都是 回文串。返回 s 所有可能的分割方案。 示例 1: 输入:s “aab” 输出:[[“a”,“a”,“b”],…...
Flutter 中的 SliverPrototypeExtentList 小部件:全面指南
Flutter 中的 SliverPrototypeExtentList 小部件:全面指南 Flutter 是一个功能强大的 UI 框架,由 Google 开发,允许开发者使用 Dart 语言构建跨平台的移动、Web 和桌面应用。在 Flutter 的丰富组件库中,SliverPrototypeExtentLis…...
NeuralForecast 推理 - 数据集从文件dataset.pkl读
NeuralForecast 推理 - 数据集从文件dataset.pkl读 flyfish from ray import tune from neuralforecast.core import NeuralForecast from neuralforecast.auto import AutoMLP from neuralforecast.models import NBEATS, NHITS import torch import torch.nn as nn import …...
TS-类型转换(显式)
1.将其他类型转换为布尔类型 要将其他类型转换为布尔类型,只需要将待转换的值传入Boolean()函数 var msg: string "ok"; var msgToBollean: boolean Boolean(msg); //得到trueBoolean()函数会判断传入的值是空值还是非空值。 若表示非空值࿰…...
用Python的powerlaw库分析游戏付费数据:从‘鲸鱼玩家’到长尾分布,手把手教你做实战分析
用Python的powerlaw库解析游戏付费行为:从数据清洗到商业决策全流程 游戏行业的数据分析师们常常面临一个经典问题:如何理解玩家付费行为背后的数学规律?当我们打开一份付费数据报表,往往会发现少数"鲸鱼玩家"贡献了绝…...
Logisim音乐盒背后的数字电路:计数器、ROM与蜂鸣器如何奏出《终生误》
Logisim音乐盒背后的数字电路:计数器、ROM与蜂鸣器如何奏出《终生误》 当一段熟悉的旋律从蜂鸣器中流淌而出,很少有人会思考这背后隐藏的数字魔法。本文将带您拆解一个基于Logisim的音乐盒设计,揭示计数器如何像指挥家一样协调时序、ROM怎样扮…...
解锁Switch模拟潜能:Ryujinx架构深度解析与实战优化
解锁Switch模拟潜能:Ryujinx架构深度解析与实战优化 【免费下载链接】Ryujinx 用 C# 编写的实验性 Nintendo Switch 模拟器 项目地址: https://gitcode.com/GitHub_Trending/ry/Ryujinx Ryujinx作为一款采用C#开发的开源Nintendo Switch模拟器,通…...
Gradio项目快速公网演示:除了share=True,你还有这几种轻量级内网穿透方案
Gradio项目快速公网演示:5种轻量级内网穿透方案横向评测 当你开发了一个酷炫的机器学习模型演示,或是精心设计的数据可视化界面,最迫切的需求往往是如何快速分享给同事或客户。Gradio的shareTrue参数可能是大多数开发者首先想到的方案&#x…...
【开发工具】Trae IDE 解决 Windows 下 C 工程无法跳转定义问题
1. 概要 👋 作为 Trae IDE 使用者,在 Windows 环境打开本地 C 工程时,习惯用 Ctrl 鼠标左键 快速跳转函数 / 变量定义却失效,仅能做文本匹配,无法精准定位语义定义。核心原因是 Trae 依赖 LSP(语言服务器协…...
Burp Suite实战进阶:用LingJing内置的burp-labs靶机打通从入门到专家22关(含解题思路)
Burp Suite实战进阶:用LingJing内置的burp-labs靶机打通从入门到专家22关(含解题思路) 在网络安全领域,Burp Suite无疑是渗透测试工程师最得力的工具之一。然而,很多学习者在掌握了基础操作后,往往会陷入&q…...
当地的美国展会搭建制作公司口碑排行
随着中国企业出海参展日益频繁,选择一家可靠的美国本土搭建商成为关键决策。许多企业主发现,直接对接海外供应商时,常面临沟通不畅、报价模糊、落地效果与设计图相差甚远等问题。这背后,是原有依赖单一信息渠道或熟人推荐的模式正…...
LeetCode 2946. 循环移位后的矩阵相似检查【数学周期性+原地比较】简单
本文属于「征服LeetCode」系列文章之一,这一系列正式开始于2021/08/12。由于LeetCode上部分题目有锁,本系列将至少持续到刷完所有无锁题之日为止;由于LeetCode还在不断地创建新题,本系列的终止日期可能是永远。在这一系列刷题文章…...
jcifs-ng:Java SMB客户端库如何简化企业文件共享?
jcifs-ng:Java SMB客户端库如何简化企业文件共享? 【免费下载链接】jcifs-ng A cleaned-up and improved version of the jCIFS library 项目地址: https://gitcode.com/gh_mirrors/jc/jcifs-ng jcifs-ng是一个经过清理和改进的jCIFS库版本&#…...
别再手动同步了!利用STM32定时器主从模式与ITR触发,实现硬件级精准定时联动
嵌入式系统中的定时器协同:STM32主从模式与ITR触发的硬件级联动 在工业控制、电机驱动和精密测量等场景中,多个定时器的精确协同往往是系统可靠性的关键。想象一下,当你的电机控制PWM需要与电流采样ADC严格同步,或者多个通信接口必…...
