从零入门激光SLAM(二十)——IESKF代码实现

大家好呀,我是一个SLAM方向的在读博士,深知SLAM学习过程一路走来的坎坷,也十分感谢各位大佬的优质文章和源码。随着知识的越来越多,越来越细,我准备整理一个自己的激光SLAM学习笔记专栏,从0带大家快速上手激光SLAM,也方便想入门SLAM的同学和小白学习参考,相信看完会有一定的收获。如有不对的地方欢迎指出,欢迎各位大佬交流讨论,一起进步。博主创建了一个科研互助群Q:772356582,欢迎大家加入讨论。

源码是高博大佬的,网址如下 

slam_in_autonomous_driving/src/ch8 at master · gaoxiang12/slam_in_autonomous_driving · GitHub

下面对代码的逻辑和主要函数进行解析

一、IESKF结构

  • 参数配置/设置状态变量

    struct Options {Options() = default;/// IEKF配置int num_iterations_ = 3;  // 迭代次数double quit_eps_ = 1e-3;  // 终止迭代的dx大小/// IMU 测量与零偏参数double imu_dt_ = 0.01;         // IMU测量间隔double gyro_var_ = 1e-5;       // 陀螺测量标准差double acce_var_ = 1e-2;       // 加计测量标准差double bias_gyro_var_ = 1e-6;  // 陀螺零偏游走标准差double bias_acce_var_ = 1e-4;  // 加计零偏游走标准差/// RTK 观测参数double gnss_pos_noise_ = 0.1;                   // GNSS位置噪声double gnss_height_noise_ = 0.1;                // GNSS高度噪声double gnss_ang_noise_ = 1.0 * math::kDEG2RAD;  // GNSS旋转噪声/// 其他配置bool update_bias_gyro_ = true;  // 是否更新biasbool update_bias_acce_ = true;  // 是否更新bias};// nominal stateSO3 R_;VecT p_ = VecT::Zero();VecT v_ = VecT::Zero();VecT bg_ = VecT::Zero();VecT ba_ = VecT::Zero();VecT g_{0, 0, -9.8};// error stateVec18T dx_ = Vec18T::Zero();// covarianceMat18T cov_ = Mat18T::Identity();// noiseMotionNoiseT Q_ = MotionNoiseT::Zero();GnssNoiseT gnss_noise_ = GnssNoiseT::Zero();Options options_;
    
  • 设置初始条件
    void SetInitialConditions(Options options, const VecT& init_bg, const VecT& init_ba,const VecT& gravity = VecT(0, 0, -9.8)) {BuildNoise(options);options_ = options;bg_ = init_bg;ba_ = init_ba;g_ = gravity;cov_ = 1e-4 * Mat18T::Identity();cov_.template block<3, 3>(6, 6) = 0.1 * math::kDEG2RAD * Mat3T::Identity();}
    //构建噪声模型
    void BuildNoise(const Options& options) {double ev = options.acce_var_;double et = options.gyro_var_;double eg = options.bias_gyro_var_;double ea = options.bias_acce_var_;double ev2 = ev;  // * ev;double et2 = et;  // * et;double eg2 = eg;  // * eg;double ea2 = ea;  // * ea;// set QQ_.diagonal() << 0, 0, 0, ev2, ev2, ev2, et2, et2, et2, eg2, eg2, eg2, ea2, ea2, ea2, 0, 0, 0;double gp2 = options.gnss_pos_noise_ * options.gnss_pos_noise_;double gh2 = options.gnss_height_noise_ * options.gnss_height_noise_;double ga2 = options.gnss_ang_noise_ * options.gnss_ang_noise_;gnss_noise_.diagonal() << gp2, gp2, gh2, ga2, ga2, ga2;
    }
    
  • 使用IMU预测

    bool IESKF<S>::Predict(const IMU& imu) {/// Predict 部分与ESKF完全一样,不再解释assert(imu.timestamp_ >= current_time_);double dt = imu.timestamp_ - current_time_;if (dt > (5 * options_.imu_dt_) || dt < 0) {LOG(INFO) << "skip this imu because dt_ = " << dt;current_time_ = imu.timestamp_;return false;}VecT new_p = p_ + v_ * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt + 0.5 * g_ * dt * dt;VecT new_v = v_ + R_ * (imu.acce_ - ba_) * dt + g_ * dt;SO3 new_R = R_ * SO3::exp((imu.gyro_ - bg_) * dt);R_ = new_R;v_ = new_v;p_ = new_p;Mat18T F = Mat18T::Identity();F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt;F.template block<3, 3>(3, 6) = -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt;F.template block<3, 3>(3, 12) = -R_.matrix() * dt;F.template block<3, 3>(3, 15) = Mat3T::Identity() * dt;F.template block<3, 3>(6, 6) = SO3::exp(-(imu.gyro_ - bg_) * dt).matrix();F.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt;cov_ = F * cov_ * F.transpose() + Q_;current_time_ = imu.timestamp_;return true;
    }
    
  • 迭代观测模型

    using CustomObsFunc = std::function<void(const SE3& input_pose, Eigen::Matrix<S, 18, 18>& HT_Vinv_H,Eigen::Matrix<S, 18, 1>& HT_Vinv_r)>;
    // 使用自定义观测函数更新滤波器
    bool IESKF<S>::UpdateUsingCustomObserve(IESKF::CustomObsFunc obs) {// H阵由用户给定// 保存当前的旋转矩阵SO3 start_R = R_;Eigen::Matrix<S, 18, 1> HTVr;Eigen::Matrix<S, 18, 18> HTVH;Eigen::Matrix<S, 18, Eigen::Dynamic> K;Mat18T Pk, Qk;//进入残差更新循环for (int iter = 0; iter < options_.num_iterations_; ++iter) {// 调用用户提供的观测函数//GetNominalSE3()获取当前名义状态位姿
    //SE3 GetNominalSE3() const { return SE3(R_, p_); }//HTVH卡尔曼更新步骤中用于修正预测的协方差矩阵//HTVr卡尔曼更新步骤中用于修正预测的状态变量obs(GetNominalSE3(), HTVH, HTVr);// 投影协方差矩阵PMat18T J = Mat18T::Identity();J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat((R_.inverse() * start_R).log());Pk = J * cov_ * J.transpose();// 卡尔曼更新Qk = (Pk.inverse() + HTVH).inverse();   // 计算更新后的协方差矩阵Qkdx_ = Qk * HTVr; // 计算状态增量dx_// LOG(INFO) << "iter " << iter << " dx = " << dx_.transpose() << ", dxn: " << dx_.norm();//将增量dx_合并到名义变量中Update();// 检查增量的范数是否小于终止阈值if (dx_.norm() < options_.quit_eps_) {break;}}// 更新协方差矩阵update Pcov_ = (Mat18T::Identity() - Qk * HTVH) * Pk;// 再次投影协方差矩阵PMat18T J = Mat18T::Identity();Vec3d dtheta = (R_.inverse() * start_R).log();J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat(dtheta);cov_ = J * cov_ * J.inverse();// 重置状态增量dx_.setZero();return true;
    }

详情请见...

 从零入门激光SLAM(二十)——IESKF代码解释 - 古月居 (guyuehome.com)

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

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

相关文章

Ansible自动化运维中的file文件模块模块应用详解

作者主页&#xff1a;点击&#xff01; Ansible专栏&#xff1a;点击&#xff01; 创作时间&#xff1a;2024年5月21日15点21分 &#x1f4af;趣站推荐&#x1f4af; 前些天发现了一个巨牛的&#x1f916;人工智能学习网站&#xff0c;通俗易懂&#xff0c;风趣幽默&#xf…

【Java笔记】第8章:面向对象的三大特性(封装、继承、多态)

前言1. 三大特性概述2. 封装3. 继承4. 多态结语 #include<GUIQU.h> int main { 上期回顾:【Java笔记】第7章&#xff1a;面向对象 个人主页&#xff1a;C_GUIQU 归属专栏&#xff1a;【Java学习】 return 一键三连; } 前言 各位小伙伴大家好&#xff01;上期小编给大家…

后端技术常用网站

技术说明官网SpringBootMVC框架https://spring.io/projects/spring-bootSpringCloud微服务框架https://spring.io/projects/spring-cloud/MyBatis-PlusORM框架https://mp.baomidou.com/Swagger-UI文档生产工具https://github.com/swagger-api/swagger-uiKibana分析和可视化平台…

SO_REUSEPORT 之 TCP负载均衡验证

首先启动两个tcp server&#xff0c; 代码里开启 SO_REUSEPORT [my_testlocalhost test]$ ./tcp_server_reuseport & [1] 1864 [my_testlocalhost test]$ Server listening on port 8888[my_testlocalhost test]$ ./tcp_server_reuseport & [2] 1865 [my_testlocalh…

网络工程师备考1——基础学习

认识设备 1 交换机 一、什么是交换机&#xff1f; 实现不同电脑之间数据的转发 换机是一种用于电(光)信号转发的网络设备。 它可以为接入交换机的任意两个网络节点提供独享的电信号通路。最常见的交换机是以太网交换机。交换机工作于OSI参考模型的第二层&#xff0c;即数据…

使用 Supabase 的 Realtime + Storage 非常方便呢

文章目录 &#xff08;一&#xff09;Supabase&#xff08;二&#xff09;Realtime&#xff08;消息&#xff09;&#xff08;2.1&#xff09;Python 消息订阅&#xff08;2.2&#xff09;JavaScript 消息订阅 &#xff08;三&#xff09;Storage&#xff08;存储&#xff09;&…

Linux:Ubuntu修改root密码

Linux&#xff1a;Ubuntu修改root密码 修改默认grub配置文件 rootshanxin:~# vim /etc/default/grub# 主要修改内容如下&#xff1a;GRUB_DEFAULT0 #GRUB_TIMEOUT_STYLEhidden 注释这一行 GRUB_TIMEOUT5 # 将这一行的时间改为5秒进行开启启动的grub文件的复写 rootshanxin:~…

芯课堂 | UI Creator 物理键盘移植指南

LVGL提供输入设备的种类一共有5种&#xff0c;分别是&#xff1a;touchpad&#xff08;触摸板&#xff09;、mouse&#xff08;鼠标&#xff09;、keypad&#xff08;键盘&#xff09;、encoder&#xff08;编码器&#xff09;、button&#xff08;外部按键&#xff09;。而基于…

Tron 节点 性能压测

文章目录 一、代码说明1.1 主要功能1.2 代码示例1.3 代码解释1.4 执行流程 二、结果分析三、参数解释3.1 numWorkers 和 numRequests 说明3.2 使用场景 四、注意事项 最近搭建了一个TRON节点&#xff0c;同事不相信我的自建节点比官方更靠谱&#xff0c;咱们给他使用golang写一…

如何成为一个专业的AI产品经理?

可以找专业的老师带。 可以找专业的内容学。 可以多遇挫折并快速学习&#xff0c;屡败屡战&#xff0c;笔者本人从业AI十年有余&#xff0c;吃了不少苦&#xff0c;有过很多或成功或失败的经历。 成为一个专业的AI产品经理需要一系列专业知识和技能的积累&#xff0c;以及在…

axios - 简 单 的 请 求 模 板

import {message } from ant-design-vue import axios from axios import {useUserStore } from @/store/modules/user import router from @/routerconst instance = axios.create({baseURL: http://192.168.110.171:5201,timeout: 5000, })* 请求拦截器 insta

切换Ubuntu开机的主题

要将Ubuntu系统的启动画面&#xff08;即开机时的显示界面&#xff09;从Lubuntu切换回原生的Ubuntu界面&#xff0c;可以按照以下步骤操作&#xff1a; 1. 安装原生Ubuntu的plymouth主题 首先&#xff0c;确保你已经安装了原生Ubuntu的plymouth主题。打开终端并运行以下命令…

基于Matlab卷积神经网络(CNN)人脸识别系统

欢迎大家点赞、收藏、关注、评论啦 &#xff0c;由于篇幅有限&#xff0c;只展示了部分核心代码。 文章目录 一项目简介 二、功能三、系统四. 总结 一项目简介 一、项目背景与意义 人脸识别技术作为计算机视觉领域的一个重要分支&#xff0c;已经广泛应用于安全监控、身份验证…

Python基础知识归纳总结

目录 一、线性表 总结 二、栈 三、队列 四、哈希表 五、字符串 六、正则表达式 综合示例 一、线性表 线性表&#xff08;通常用列表表示&#xff09;是一种按线性顺序存储元素的数据结构。 插入元素 (append, insert) 删除元素 (remove, pop) 查找元素 (index) 更新…

FreeRTOS学习——FreeRTOS队列(下)之队列创建

本篇文章记录我学习FreeRTOS队列创建的知识。主要分享队列创建需要使用的初始化函数、队列复位函数。 需要进一步了解FreeRTOS队列的相关知识&#xff0c;读者可以参考以下文章&#xff1a; FreeRTOS学习——FreeRTOS队列&#xff08;上&#xff09;_freertos 单元素队列-CSDN博…

Spring、SpringMVC、SpringBoot 三者之间是什么关系

Spring、SpringMVC、SpringBoot 是三个独立的框架&#xff0c;它们之间的关系是: 1. Spring是一个Java的轻量级应用框架&#xff0c;提供了基于IoC和AOP的支持&#xff0c;用于构建企业级应用。Spring有多 个模块&#xff0c;包括 Spring Core、Spring Context、Spring JDBC、…

高质量油封选择的重要性

在现代工业和机械系统中&#xff0c;油封的作用至关重要。它们不仅防止润滑剂泄漏&#xff0c;还阻止污染物进入系统&#xff0c;从而保障设备的正常运行。选择高质量的油封对于保证设备性能、延长使用寿命和降低维护成本至关重要。本文将从多个角度探讨选择高质量油封的重要性…

【hackmyvm】Slowman靶机

文章目录 主机探测端口探测FTP匿名登录 目录探测hydra爆破mysql爆破zip------fcrackzip爆破密码-----john提权 主机探测 ┌──(root㉿kali)-[/home/kali] └─# fping -ag 192.168.9.1/24 2>/dev/null 192.168.9.221 主机192.168.9.224 靶机端口探测 ┌──(roo…

送变电乙级资质申请攻略:关键步骤与注意事项

关键步骤&#xff1a; 工商注册与资本核实&#xff1a; 确保企业已正式注册&#xff0c;且注册资金不少于100万元人民币&#xff0c;同时准备企业法人营业执照等相关证明文件。 人员配置与注册&#xff1a; 根据资质要求&#xff0c;配置齐全合理的专业技术人员&#xff0c;包括…

【教程】Linux 安装 kkFileView 文档在线预览项目 及优化

【教程】Linux 安装 kkFileView 文档在线预览项目 官网 kkFileView - 在线文件预览 (keking.cn) 安装包 可以直接下载成品 也可以下载source 源码 自己编译 kkFileView 发行版 - Gitee.com 打开IDEA 然后先clear 再install 然后在 file-online-preview\server\target 目录…