卡尔曼滤波

卡尔曼滤波器原理推导

网上关于卡尔曼滤波原理的推导已经有很多了，在这里我主要推荐一位宝藏b站UP主DR_CAN的 卡尔曼滤波推导视频合集以及一片知乎的卡尔曼滤波推导文章，这篇文章里面有两个小问题(可以自己读一遍看看自己能不能找到问题)，评论区已经有人指出来了。但是我觉得这篇文章的排版不错，讲解的比较详细，给出了很多概念的详细定义，值得一读。

卡尔曼滤波的核心公式就是这五条，掌握理解以后便可以开始实践。

卡尔曼滤波实践

具体项目实践的话我很推荐自己动手实现一下卡尔曼滤波，自己实现以后会对学过的知识印象十分深刻，在这里我再推荐一个b站视频

，这个视频十分详细的讲解了卡尔曼滤波的应用过程。在这里我想说明几个细节。

1. 超参数Q与R，在设置这两个协方差矩阵的时候我们需要考虑自己对预测值以及测量值的信赖程度，如果你的状态方程建立比较完备，可以必将精确的预测下一个状态值，则将Q矩阵的该值对应的方差设置小一些，反之则设置大一点。如果你的传感器测量精度很高，可以精确的测量出某个状态值，则将该值对应的方差设置小一些，反之则设置大一点。

2. 协方差矩阵P和状态值x的初始值。协方差矩阵p的值我们一般设置小一点利于收敛，设置成单位矩阵就可以，切记不能为0。状态值x的初始化比较随意，设置成0也可以。

3. 状态值x与测量值z维度不同，例如

   $\begin{bmatrix} x_{k} \\ \dot{x}_{k} \end{bmatrix} = \begin{bmatrix} 1 & \Delta t\\ 0 & 1 \end{bmatrix}\begin{bmatrix} x_{k-1} \\ \dot{x}_{k-1} \end{bmatrix}$

   $z_{k} = \begin{bmatrix} 1 & 0 \end{bmatrix}\begin{bmatrix} x_{k} \\ \dot{x}_{k} \end{bmatrix}$

   当我们测量值只有位置信息没有速度信息时，卡尔曼滤波依旧可以正常的work，但是他的速度信息 $$\dot{x}$$ 相对于位置信息x来说会收敛的比较慢，估计效果会差一些。

我使用c++实现了一下卡尔曼滤波，矩阵运算模块借助了eigen库，这个库我直接放在了自己的省去了大家自己下载配置cmakelist的步骤。将需要的数据写入txt后可以使用excel分析数据，excel可以从文本文件导入数据。如下图。

我的示例中，观测量只有位置x与y，速度以及加速度都没有观测，所以可以看到，x与y的误差一直很小，而速度量与加速度量会有一些较大的偏差，虽然最终也会随着时间收敛到真实值。这也就是我在上面提到过的观测量不全的问题。

扩展卡尔曼滤波

卡尔曼滤波的使用前提是：

1. 系统是线性的
2. 噪声是高斯白噪声

然而实际场景中状态方程或者观测方程往往是非线性的，如果学习过现代控制理论，我们就会知道，这时候我们需要使用泰勒展开对方程进行线性化。扩展卡尔曼滤波就是为了解决非线性方程的问题，拓宽了卡尔曼滤波的适用范围。

参考文献：

https://zhuanlan.zhihu.com/p/63641680

https://zhuanlan.zhihu.com/p/50620063

小结

温故了一下卡尔曼滤波，卡尔曼滤波的数学推导假设噪声为高斯白噪声，但是在实践中需要我们调节协方差等超参数。

代码已上传github仓库：项目地址