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

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

一、前言

通过五个章节的分析，目前对于一维卡尔曼滤波有了一定层次的理解，这里先给出上篇博客推导出来的结论(卡尔曼五大公式)：
$$①：{\check{x}}_k=f{\hat{x}}_{k-1}②：{\sigma }_{X_k}^{-}=f^2{\sigma }_{X_{k-1}}^{+}+{\sigma }_{Q_{k-1}}\text{\hspace{1em}(01)}$$ $$③：k_k=\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}\text{\hspace{1em}(02)}$$$$④：{\hat{x}}_k=k_k(y_k-h\check{x})+\check{x}⑤：{\sigma }_{X_k}^{+}=(1-k_k){\sigma }_{X_k}^{-}\text{\hspace{1em}(03)}$$
上面的五个式子很明显是递推的若假设已知 ${\hat{x}}_0$、${\sigma }_{X_0}^{+}$、以及各个时刻观测 $y_k$，则可推导出出 ${\hat{x}}_k$、${\sigma }_{X_k}^{+}$，如下：
$$【{\hat{x}}_0,{\sigma }_{X_0}^{+},y_1】\to 【{\hat{x}}_1,{\sigma }_{X_1}^{+},y_2】\to \cdots \to 【{\hat{x}}_k,{\sigma }_{X_k}^{+}】\text{\hspace{1em}(04)}$$该篇本博客主要是进行编程实践，为了公式与源码更好的对应起来，对上述公式公式进行改写，因为编程中通常需要进行模块下，所以代码中会实现一个函数，该函数只完成一次递推，故上5式符号简写为：
$$①：x_{minus}=f{x}_{plus}②：{\sigma }_{minus}=f^2{\sigma }_{plus}+q\text{\hspace{1em}(05)}$$ $$③：k=\frac{h{\sigma }_{minus}}{h^2{\sigma }_{minus}+r}\text{\hspace{1em}(06)}$$$$④：x_{plus}=k(y-h{x}_{minus})+x_{minus}⑤：{\sigma }_{plus}=(1-k){\sigma }_{minus}\text{\hspace{1em}(07)}$$上式中的 $r={\sigma }_{R_k}$(预测过程标准差，主要影响收敛速度)，$q={\sigma }_{R_k}$(观测过程标准差，理解为传感器精度，可以通过实验获得)，这两个值都是固定值，迭代过程中通常不会改变。由于是编程，(05) 式中的 $x_{minus}$ 最终会被 (07) 式中的 $x_{plus}$ 覆盖，同理 ${\sigma }_{plus}$ 也会被覆盖。每次计算出来的 $x_{plus}$ 与 ${\sigma }_{plus}$ 又会作为下一次的 $x_{minus}$ 与 ${\sigma }_{minus}$ 进行输入。

二、C++一维示例

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