今天跟大家聊聊我最近在做的卡尔曼滤波，这东西听起来高大上，上手也没那么难，就是得一步一个脚印地啃。
起因：
最开始接触卡尔曼滤波，是因为我想做一个小车自动寻迹的项目，涉及到传感器数据融合，单纯的PID控制效果不太所以就想到用卡尔曼滤波来提高精度。当时在网上搜一堆资料，公式看得我头都大，什么状态方程、观测方程、协方差矩阵…一脸懵逼。
开始：
硬着头皮开始学，先是找几篇比较通俗易懂的博客，先把卡尔曼滤波的基本原理搞明白。简单来说，它就是把预测值和测量值结合起来，得到一个更靠谱的估计值。就像咱们平时猜东西，先自己估计一个，然后听听别人的意见，综合一下，往往就能猜得更准。
实践：
光看理论没用，得动手。我找一个最简单的例子：匀速直线运动。假设一个小车以恒定的速度前进，但是测量速度的传感器有误差，我用卡尔曼滤波来估计小车的真实速度和位置。
- 第一步：建立模型。
得先把小车的运动用数学公式表示出来，也就是状态方程和观测方程。状态方程描述的是小车速度和位置随时间的变化，观测方程描述的是传感器测量值和真实值之间的关系。这步很重要，模型建错，后面就白搭。
- 第二步：初始化。
给小车的初始速度和位置一个估计值，再给一个误差协方差矩阵，表示我对这个估计值的信任程度。一开始我对自己的估计不太自信，就把误差协方差设得大一点。
- 第三步：迭代。
这是卡尔曼滤波的核心，就是不断地进行预测和更新。预测就是根据状态方程，预测小车在下一个时刻的速度和位置。更新就是根据传感器的测量值，对预测值进行修正。这个过程要用到卡尔曼增益，它决定预测值和测量值哪个更可信。传感器越准，卡尔曼增益就越大，测量值就越重要。
问题：
刚开始写代码的时候，bug一堆。不是矩阵维度不对，就是卡尔曼增益算错。调试好久，才发现是自己把公式记错。以后再也不相信自己的记忆力，还是老老实实看文档。
优化：
最开始的程序跑起来，效果还不错，但是稳定性不太有时候小车会突然抖动一下，或者速度估计值会跳变。后来我发现，是我的状态方程太简单，没有考虑到小车的加速度。于是我把状态方程改一下，把加速度也加进去，效果就好多。
搞几天，总算是把卡尔曼滤波跑通。虽然只是一个简单的例子，但是让我对卡尔曼滤波有更深的理解。以后再遇到类似的问题，我就知道该怎么下手。也没就是多看资料，多动手，多调试，一步一个脚印地来。
下一步计划：
下一步我打算把卡尔曼滤波用到我的小车自动寻迹项目里，结合激光雷达和IMU的数据，让小车跑得更稳更准。
还没有评论，来说两句吧...