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.mzph.cn/pingmian/19921.shtml

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

相关文章

2020 6.s081——Lab2:system calls

左岸的一座白色环形阶梯 浪人正在用和弦练习忧郁 晨曦下的少女听着吉他旋律 在许愿池边巴洛克式的叹息 ——许愿池的希腊少女 完整代码见&#xff1a;SnowLegend-star/6.s081 at syscall (github.com) System call tracing (moderate) 这个实验要求我们跟踪系统调用。 感觉实…

平衡二叉树的应用举例

AVL 是一种自平衡二叉搜索树&#xff0c;其中任何节点的左右子树的高度之差不能超过 1。 AVL树的特点&#xff1a; 1、它遵循二叉搜索树的一般属性。 2、树的每个子树都是平衡的&#xff0c;即左右子树的高度之差最多为1。 3、当插入新节点时&#xff0c;树会自我平衡。因此…

R语言绘图 --- 饼状图(Biorplot 开发日志 --- 2)

「写在前面」 在科研数据分析中我们会重复地绘制一些图形&#xff0c;如果代码管理不当经常就会忘记之前绘图的代码。于是我计划开发一个 R 包&#xff08;Biorplot&#xff09;&#xff0c;用来管理自己 R 语言绘图的代码。本系列文章用于记录 Biorplot 包开发日志。 相关链接…

JDBC入门基础

目录 JDBC的基本概念 快速入门&#xff08;基本步骤&#xff09; 创建数据库 注册驱动&#xff08;可以省略不写&#xff09; 获取连接对象 获取执行SQL语句的对象 编写SQL语句&#xff0c;并执行&#xff0c;以及接收返回的结果 处理结果&#xff0c;遍历结果集和 释放资源&…

数据流通与智能家居的未来

在科技飞速发展的今天&#xff0c;智能家居逐渐融入我们的日常生活&#xff0c;改变了传统的居住方式。智能生态网络&#xff08;IEN&#xff09;作为智能家居的核心&#xff0c;集成了家庭内的各种智能设备和传感器&#xff0c;实现了对家庭环境的智能化管理。而数据要素流通则…

ESP32入门:1、VSCode+PlatformIO环境搭建(离线快速安装)

文章目录 背景安装vscode安装配置中文 安装Platform IO安装PIO 新建ESP32工程参考 背景 对于刚接触单片机的同学&#xff0c;使用vscodeplatformIO来学习ESP32是最方便快捷的&#xff0c;比IDF框架简单&#xff0c;且比arduino文件管理性能更好。但是platformIO安装较为麻烦&a…

电子阅览室能给孩子做什么

电子阅览室为孩子提供了很多活动和资源&#xff0c;可以为他们提供以下服务&#xff1a; 1. 提供电子书籍和儿童读物&#xff1a;电子阅览室通常提供大量的电子书籍和儿童读物&#xff0c;供孩子选择阅读。 2. 提供儿童学习资源&#xff1a;专久智能电子阅览室可以提供各种学习…

CraftCMS ConditionsController.php 代码执行漏洞(CVE-2023-41892)

0x01 产品简介 Craft CMS是一个开源的内容管理系统,它专注于用户友好的内容创建过程,逻辑清晰明了,是一个高度自由,高度自定义设计的平台吗,可以用来创建个人或企业网站也可以搭建企业级电子商务系统。 0x02 漏洞概述 Craft CMS在4.4.15版本之前存在远程代码执行漏洞,…

[论文笔记]MemGPT: Towards LLMs as Operating Systems

引言 今天介绍一篇论文MemGPT: Towards LLMs as Operating Systems。翻过过来就是把LLM看成操作系统。 大语言模型已经在人工智能领域引起了革命性的变革&#xff0c;但受到有限上下文窗口的限制&#xff0c;在扩展对话和文档分析等任务中的效用受到了阻碍。为了能够利用超出…

4月啤酒品类线上销售数据分析

近期&#xff0c;中国啤酒行业正处于一个重要的转型期。首先&#xff0c;消费者对高品质啤酒的需求不断增加&#xff0c;这推动了行业向高端化、场景化和社交化的方向发展。精酿啤酒作为这一趋势的代表&#xff0c;其发展势头强劲&#xff0c;不仅满足了消费者对品质化、个性化…

蓝桥杯2024国赛--备赛刷题题单

1.游戏&#xff08;单调队列&#xff09; 注意如果结果是分数&#xff0c;直接设置变量为double&#xff0c;最好不要使用把int类型乘1.0变成分数来计算。 #include <iostream> #include <queue> using namespace std; const int N1e510; //滑动窗口大小为k,最大值…

2024上海中小学生古诗文大会方案已发布,家长孩子最关心10个问题

昨天&#xff08;2024年5月30日&#xff09;下午15点&#xff0c;上海中小学生古诗文大会组委会通过两个公众号发布了《2024上海中小学生古诗文大会系列活动方案出炉》的推文&#xff08;下称《方案》&#xff09;。如我之前的分析和预测&#xff0c;5月份会发布今年的中小学生…

邮件服务器部署

目录 介绍 资源列表 基础环境 关闭防火墙 关闭内核安全机制 修改主机名 一、部署DNS服务器 mail节点操作 修改DNS地址 二、部署postfix和dovecot 安装软件包 修改postfix配置文件 重启postfix服务和开机自启 修改dovecot配置文件 重启dovecot服务和开机自启 创…

微服务:一篇博客带你学会Gateway(路由、过滤、跨域问题配置)

文章目录 Gateway搭建路由断言工厂路由过滤器全局过滤器过滤器执行顺序网关的core跨域配置跨域问题配置 Gateway 网关功能&#xff1a; 身份认证、权限校验服务路由、负载均衡请求限流 搭建 gateway也算一个服务 所以创建gateway子模块 引入依赖 <dependency><gro…

AIGC智能办公实战 课程,祝你事业新高度

在数字化时代&#xff0c;人工智能&#xff08;AI&#xff09;已经渗透到我们生活的方方面面&#xff0c;从智能家居到自动驾驶&#xff0c;从医疗诊断到金融分析&#xff0c;AI助手正在改变我们的工作方式和生活质量。那么&#xff0c;你是否想过自己也能从零开始&#xff0c;…

Redis学习笔记【实战篇--短信登录】

开篇导读 实战篇有什么样的内容 短信登录 这一块我们会使用redis共享session来实现 商户查询缓存 通过本章节&#xff0c;我们会理解缓存击穿&#xff0c;缓存穿透&#xff0c;缓存雪崩等问题&#xff0c;让小伙伴的对于这些概念的理解不仅仅是停留在概念上&#xff0c;更…

音视频直播(一)

协议基础篇 直播协议基础推流与拉流推流拉流 直播传输协议RTMP传输协议 && HTTP-FLV协议为什么RTMP做推流&#xff0c;反而很少做拉流&#xff1f;HTTP-FLV协议 RTSP协议HLS协议SRT协议 WebRTC协议应用于直播 直播协议基础 从网络上搜寻到的有关推流与拉流的示意图 从…

Java项目对接redis,客户端是选Redisson、Lettuce还是Jedis?

JAVA项目对接redis&#xff0c;客户端是选Redisson、Lettuce还是Jedis&#xff1f; 一、客户端简介1. Jedis介绍2. Lettuce介绍3. Redisson介绍 二、横向对比三、选型说明 在实际的项目开发中&#xff0c;对于一个需要对接Redis的项目来说&#xff0c;就面临着选择合适的Redis客…

如何从浅入深理解transformer?

前言 在人工智能的浩瀚海洋中&#xff0c;大模型目前无疑是其中一颗璀璨的明星。从简单的图像识别到复杂的自然语言处理&#xff0c;大模型在各个领域都取得了令人瞩目的成就。而在这其中&#xff0c;Transformer模型更是成为大模型技术的核心。 一、大模型的行业发展现状如…

QT5:调用qt键盘组件实现文本框输入

目录 一、环境与目标 二、Qt VirtualKeyboard 1.勾选Qt VirtualKeyboard 2.ui设计流程 3.注意事项及问题点 三、参考代码 参考博客 一、环境与目标 qt版本&#xff1a;5.12.7 windows 11 下的 Qt Designer &#xff08;已搭建&#xff09; 目标&#xff1a;创建一个窗…