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