讲解关于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 数据。