卡尔曼家族从零解剖-(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 数据。

相关推荐
小白狮ww1 小时前
RStudio 教程:以抑郁量表测评数据分析为例
人工智能·算法·机器学习
空白到白5 小时前
决策树-面试题
算法·决策树·机器学习
java1234_小锋5 小时前
Scikit-learn Python机器学习 - 特征预处理 - 归一化 (Normalization):MinMaxScaler
python·机器学习·scikit-learn
西猫雷婶5 小时前
scikit-learn/sklearn学习|广义线性回归损失函数的基本表达式
深度学习·神经网络·学习·机器学习·线性回归·scikit-learn·概率论
nju_spy6 小时前
李沐深度学习论文精读(二)Transformer + GAN
人工智能·深度学习·机器学习·transformer·gan·注意力机制·南京大学
空白到白7 小时前
机器学习-集成学习
人工智能·机器学习·集成学习
水凌风里7 小时前
4.4 机器学习 - 集成学习
人工智能·机器学习·集成学习
雲_kumo7 小时前
集成学习:从理论到实践的全面解析
人工智能·机器学习·集成学习
i.ajls8 小时前
无监督学习,推荐系统以及强化学习笔记
笔记·学习·机器学习
胖达不服输9 小时前
「日拱一码」081 机器学习——梯度增强特征选择GBFS
人工智能·python·算法·机器学习·梯度增强特征选择·gbfs