卡尔曼家族从零解剖-(06)一维卡尔曼滤波编程实践

讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解的 卡尔曼家族从零解剖 链接 :卡尔曼家族从零解剖-(00)目录最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/133846882
 
文末正下方中心提供了本人 联系方式, 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式,点击本人照片即可显示WX官方认证
 

一、前言

通过五个章节的分析,目前对于一维卡尔曼滤波有了一定层次的理解,这里先给出上篇博客推导出来的结论(卡尔曼五大公式):
①: x ˇ k = f x ^ k − 1 ②: σ X k − = f 2 σ X k − 1 + + σ Q k − 1 (01) \color{red} ①:\tag{01}\check x_{k}= f\hat x_{k-1}~~~~~~~~~~~~~~~②:\sigma^{-}_{X_{k}}=f^2\sigma_{X_{k-1}}^{+}+\sigma_{Q_{k-1}} xˇk=fx^k1               σXk=f2σXk1++σQk1(01) ③: k k = h σ X k − h 2 σ X k − + σ R k (02) \color{red} \tag{02}③:k_k=\frac{h \sigma_{X_k}^{-} }{h^{2} \sigma_{X_k}^{-} +\sigma_{R_k}} kk=h2σXk+σRkhσXk(02) ④: x ^ k = k k ( y k − h x ˇ ) + x ˇ ⑤: σ X k + = ( 1 − k k ) σ X k − (03) \color{red} \tag{03} ④:\hat x_{k}=k_k(y_k-h\check x)+\check x~~~~~~~~~~~~~~~~~~⑤:\sigma^+_{X_{k}}=(1-k_k) \sigma_{X_k}^{-} x^k=kk(ykhxˇ)+xˇ                  σXk+=(1kk)σXk(03)
上面的五个式子很明显是递推的若假设已知 x ^ 0 \hat x_0 x^0 σ X 0 + \sigma_{X_{0}}^+ σX0+、以及各个时刻观测 y k y_k yk,则可推导出出 x ^ k \hat x_k x^k σ X k + \sigma_{X_{k}}^+ σXk+,如下:
【 x ^ 0 , σ X 0 + , y 1 】 → 【 x ^ 1 , σ X 1 + , y 2 】 → ⋯ → 【 x ^ k , σ X k + 】 (04) \color{Green} \tag{04}【\hat x_0,\sigma_{X_{0}}^+,y_1】→【\hat x_1,\sigma_{X_{1}}^+,y_2】→\cdots→【\hat x_k,\sigma_{X_{k}}^+】 x^0,σX0+,y1x^1,σX1+,y2x^k,σXk+(04)该篇本博客主要是进行编程实践,为了公式与源码更好的对应起来,对上述公式公式进行改写,因为编程中通常需要进行模块下,所以代码中会实现一个函数,该函数只完成一次递推,故上5式符号简写为:
①: x m i n u s = f x p l u s ②: σ m i n u s = f 2 σ p l u s + q (05) \color{red} ①:\tag{05} x_ {minus}= f x_{plus}~~~~~~~~~~~~~~~②:\sigma_{minus}=f^2\sigma_{plus}+q xminus=fxplus               σminus=f2σplus+q(05) ③: k = h σ m i n u s h 2 σ m i n u s + r (06) \color{red} \tag{06}③:k=\frac{h \sigma_{minus} }{h^{2} \sigma_{minus} +r} k=h2σminus+rhσminus(06) ④: x p l u s = k ( y − h x m i n u s ) + x m i n u s ⑤: σ p l u s = ( 1 − k ) σ m i n u s (07) \color{red} \tag{07} ④: x_{plus}=k(y-h x_{minus})+x_{minus}~~~~~~~~~~~~~~~~~~⑤:\sigma_{plus}=(1-k) \sigma_{minus} xplus=k(yhxminus)+xminus                  σplus=(1k)σminus(07)上式中的 r = σ R k r=\sigma_{R_k} r=σRk(预测过程标准差,主要影响收敛速度), q = σ R k q=\sigma_{R_k} q=σRk(观测过程标准差,理解为传感器精度,可以通过实验获得),这两个值都是固定值,迭代过程中通常不会改变。由于是编程,(05) 式中的 x m i n u s x_ {minus} xminus 最终会被 (07) 式中的 x p l u s x_{plus} xplus 覆盖,同理 σ p l u s \sigma_{plus} σplus 也会被覆盖。每次计算出来的 x p l u s x_{plus} xplus σ p l u s \sigma_{plus} σplus 又会作为下一次的 x m i n u s x_{minus} xminus σ m i n u s \sigma_{minus} σminus 进行输入。

二、C++一维示例

是一个程序, 假设有这样一条曲线 y = 0.5 x 2 + 8 y=0.5x^2+8 y=0.5x2+8 ,现在以其为真值,当然实际应用中我们是不知道的,这里是为了模拟观测数据。观测数据在真值的基础上叠加一个高斯噪声 N ( 0 , 100 ) N(0,100) N(0,100),也就是公式推导中的 y k y_k yk。总之,通过这种方式,拿到了一批带有高斯噪声的 y k y_k yk 数据。

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

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

相关文章

【工具】Java计算图片相似度

【工具】Java图片相似度匹配工具 方案一 通过像素点去匹配 /*** * param file1Url 图片url* param file2Url 图片url* return*/public static double img相似度Url(String file1Url, String file2Url){InputStream inputStream1 HttpUtil.createGet(file1Url).execute().…

Unity中Shader光照探针的支持

文章目录 前言一、光照探针用在哪怎么用1、光照探针的应用场景2、我们按照以上条件,在Unity中搭建一个相同的环境3、创建光照探针 二、在我们自己的Shader中,实现支持光照探针1、使用常用的 cginc2、在 v2f 中,准备如下变量3、在顶点着色器中…

macOS文本编辑器 BBEdit 最新 for mac

BBEdit是一款功能强大的文本编辑器,适用于Mac操作系统。它由Bare Bones Software开发,旨在为开发者和写作人员提供专业级的文本编辑工具。 以下是BBEdit的一些主要特点和功能: 多语言支持:BBEdit支持多种编程语言和标记语言&…

ZYNQ PS端的Cache问题

Zynq Cache问题的解决方法 - Kevin_HeYongyuan - 博客园 (cnblogs.com) zynq双核AMP实验之cpu1唤醒代码_xil_settlbattributes-CSDN博客 内存与cache一致性问题_ddr cache一致性-CSDN博客 使用Xil_SetTlbAttributes(0xFFFF0000,0x14de2);可以禁用掉0x1F00 0000区域的Cache&…

负债1320万美元的【思宏集团/Neo-Concep】申请900万美元纳斯达克IPO上市

来源:猛兽财经 作者:猛兽财经 猛兽财经获悉,总部位于香港的思宏集团Neo-Concept International Group Holdings Limited(简称:思宏集团)近期已向美国证券交易委员会(SEC)提交招股书&#xff0c…

【解刊】IEEE(trans),中科院2区,顶刊,CCF-A类,圈外人别想投?

计算机类 • 好刊解读 今天小编带来IEEE旗下计算机领域好刊的解读,如有相关领域作者有意向投稿,可作为重点关注!后文有真实发表案例,供您投稿参考~ 01 期刊简介 IEEE Transactions on Computers ☑️出版社:IEEE …

layui 表格(table)合计 取整数

第一步 开启合计行 是否开启合计行区域 table.render({elem: #myTable, url: ../baidui/, page: true, cellMinWidth: 100,totalRow:true,cols: [[ //表头//{ type: checkbox },{ type: checkbox,totalRowText: "合计" },//合计行区域{ field: id, align: center,…

devops步骤 -- jenkins安装

安装的docker-compose ##安装步骤参考: https://editor.csdn.net/md/?articleId133070011 编写docker-compose.yml version: 3 services: # 集合docker_jenkins:user: root # 为了避免一些…

Zookeeper篇---第八篇

系列文章目录 文章目录 系列文章目录一、描述一下 ZAB 协议二、ZAB 和 Paxos 算法的联系与区别?三、ZooKeeper 宕机如何处理?一、描述一下 ZAB 协议 ZAB 协议是 ZooKeeper 自己定义的协议,全名 ZooKeeper 原子广播协议。 ZAB 协议有两种模式:Leader 节点崩溃了如何恢复和消…

稀疏数组如何帮助我们节省内存,提升性能

本文由葡萄城技术团队发布。转载请注明出处:葡萄城官网,葡萄城为开发者提供专业的开发工具、解决方案和服务,赋能开发者。 什么是稀疏矩阵 稀疏矩阵是指矩阵中大部分元素为零的矩阵。在实际应用中,很多矩阵都是稀疏的&#xff0c…

RocketMQ 如何保证消息正常【投递】和【消费】

消息整体处理过程,这里我们将消息的整体处理阶段分为3个阶段进行分析:1、Producer发送消息阶段。 2、Broker处理消息阶段。 3、Consumer消费消息阶段。一、Producer发送消息阶段 1、安全机制保障1,发送方式。 1、同步发送 2、异步发送 3、O…

JPA Buddy快速创建update、find、count、delete、exists方法

JPA Buddy快速创建update、find、count、delete、exists方法,JPA默认提供的CrudRepository\JpaRepository提供的方法比较少,一般我们会手写一些方法,这里我们选择通过JPA Buddy快速生成,之前文章中讲到了JPA Buddy原本是IDEA收费插…

《QT从基础到进阶·二十一》QGraphicsView、QGraphicsScene和QGraphicsItem坐标关系和应用

前言: 我们需要先由一个 QGraphicsView,这个是UI显示的地方,也就是装满可见原色的Scene,然后需要一个QGraphicsScene 用来管理所有可见的界面元素,要实现UI功能,我们需要用各种从QGraphicsItem拼装成UI控件…

scDrug:从scRNA-seq到药物反应预测

scRNA-seq技术允许在转录组水平上对数千个细胞进行测量。scRNA-seq正在成为研究肿瘤微环境中细胞成分及其相互作用的重要工具。scRNA-seq也被用于揭示肿瘤微环境模式与临床结果之间的关联,并在复杂组织中剖析药物治疗的细胞特异性效应。scRNA-seq的最新进展推动了疾…

广告业展示服务预约小程序的效果如何

虽然不少人不会与广告业直接接触,但各种形式的广告却是充斥在人们生活中,线下的传单展板、线上的视频、音频、图文等都是广告很好的传播通道,同时广告业能扩展的客户属性也非常广,下到超市小摊,上到企业公司都有大小相…

RHCE第三次作业

网站需求: 1.基于域名www.openlab.com可以访问网站内容为 welcome to openlab!!! [rootserver conf.d]# mkdir /www/openlab [rootserver conf.d]# echo welcome to openlab!!! > /www/openlab/index.html [rootserver openlab]# cat /etc/httpd/conf.d/myhos…

从0到0.01入门React | 006.精选 React 面试题

🤍 前端开发工程师(主业)、技术博主(副业)、已过CET6 🍨 阿珊和她的猫_CSDN个人主页 🕠 牛客高级专题作者、在牛客打造高质量专栏《前端面试必备》 🍚 蓝桥云课签约作者、已在蓝桥云课上架的前后端实战课程《Vue.js 和 Egg.js 开发企业级健康管理项目》、《带你从入…

阿里云国际站服务器如何升级内存容量?

阿里云服务器是阿里云供给的计算服务,它具有高效安稳、可扩展性强等特色,适用于各种应用环境。在运用阿里云服务器的过程中,或许会遇到内存容量缺乏的状况,这时候就需求晋级内存容量。那么,阿里云服务器怎么晋级内存容…

地面沉降监测站可以监测什么?

随着城市化的飞速发展,地面沉降问题日益凸显。为了及时掌握土地沉降情况,确保人们安全,就需要借助地面沉降监测站的力量。 一、实时监测土地沉降 地面沉降监测站的核心功能是实时监测土地沉降。通过高精度GNSS位移监测站和先进的数据分析技术…

【带头学C++】----- 四、动态内存空间申请 ---- 4.1 动态内存分配(补充面试背题)

1.new/delete和malloc、calloc、realloc/free的区别 相同点:(都是为了分配动态的申请内存空间和释放空间) 动态内存分配:malloc、calloc、realloc和new都是用于在运行时动态分配内存空间的方式。内存管理:它们都涉及…