卡尔曼家族从零解剖-(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^k−1 ②:σXk−=f2σXk−1++σQk−1(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(yk−hxˇ)+xˇ ⑤:σXk+=(1−kk)σ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+,y1】→【x^1,σX1+,y2】→⋯→【x^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(y−hxminus)+xminus ⑤:σplus=(1−k)σ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 数据。

相关推荐
罗小罗同学3 分钟前
医工交叉入门书籍分享:Transformer模型在机器学习领域的应用|个人观点·24-11-22
深度学习·机器学习·transformer
孤独且没人爱的纸鹤7 分钟前
【深度学习】:从人工神经网络的基础原理到循环神经网络的先进技术,跨越智能算法的关键发展阶段及其未来趋势,探索技术进步与应用挑战
人工智能·python·深度学习·机器学习·ai
羊小猪~~10 分钟前
tensorflow案例7--数据增强与测试集, 训练集, 验证集的构建
人工智能·python·深度学习·机器学习·cnn·tensorflow·neo4j
不去幼儿园2 小时前
【MARL】深入理解多智能体近端策略优化(MAPPO)算法与调参
人工智能·python·算法·机器学习·强化学习
无脑敲代码,bug漫天飞3 小时前
COR 损失函数
人工智能·机器学习
HPC_fac130520678164 小时前
以科学计算为切入点:剖析英伟达服务器过热难题
服务器·人工智能·深度学习·机器学习·计算机视觉·数据挖掘·gpu算力
老艾的AI世界12 小时前
AI翻唱神器,一键用你喜欢的歌手翻唱他人的曲目(附下载链接)
人工智能·深度学习·神经网络·机器学习·ai·ai翻唱·ai唱歌·ai歌曲
DK2215112 小时前
机器学习系列----关联分析
人工智能·机器学习
FreedomLeo113 小时前
Python数据分析NumPy和pandas(四十、Python 中的建模库statsmodels 和 scikit-learn)
python·机器学习·数据分析·scikit-learn·statsmodels·numpy和pandas
风间琉璃""13 小时前
二进制与网络安全的关系
安全·机器学习·网络安全·逆向·二进制