卡尔曼滤波算法--核心公式推导导论 - ZZ

卡尔曼滤波算法--核心公式推导导论

 

10 个月前

写在最前面：这是我第一篇专栏文章，感谢知乎提供这么一个平台，让本身能和你们分享知识。本人会不按期的开始更新文章，文章的内容应该集中在汽车动力学控制，整车软件架构，控制器等方面。做为一名在校硕士，不少理解均可能不全面，不正确，你们有不一样意见欢迎讨论。 谢谢！
---------------------------------------------------------------------------------------------------------------------------

卡尔曼滤波算法的应用很普遍，省略不少字。

卡尔曼滤波算法的优势不少，省略不少字。

卡尔曼滤波算法很牛逼，由于有一堆公式，有一堆符号，看起来就很牛逼啊，乍一看不懂的都很牛逼啊！

本文针对卡尔曼滤波算法的核心公式进行推导，不让你们被它华丽的外表吓到。（以后计划写关于针对非线性状况的EKF和UKF，对卡尔曼滤波算法作一个全面一点的应用介绍。感兴趣的能够关注专栏。）

--------------------------------------------------------------------------------------------------------------------------

Okay，进入正题。这篇文章假设读者已经对卡尔曼滤波算法有初步的了解，知道它能作什么，知道它的优势，知道它很牛逼，而且你已经对它产生兴趣，但不知道如何下手。

首先给出一个控制理论中公式，别急着翻控制理论的书，没那么复杂：

两个基本问题：

1.卡尔曼滤波算法要作什么？

对状态进行估计。

2. 卡尔曼滤波算法怎么对状态进行估计？

利用状态过程噪声和测量噪声对状态进行估计。

一个状态在一个时刻点k的状态进入下一个时刻点k+1状态，会有不少外界因素的干扰，咱们把干扰就叫作过程噪声，（这个词一看就是硬翻译过来的，别在乎为何叫噪声）用w表示。任何一个测量仪器，都会有偏差，咱们把这个偏差叫作量测噪声，用v表示。

回到上面那个公式，状态方程表示状态在不断的更新，从一个时刻点进入下一个时刻点，这个很好理解。关键是量测方程，它表示，咱们不断更新的状态有几个能用测量仪器测出来，好比，汽车运动状态参数有不少，好比速度，轮速，滑移率等，可是咱们只能测量出轮速，所以量测方程要作的就是把状态参数中能量测的状态拿出来。

咱们始终要记得咱们要作的事：咱们要获得的是优化的状态量Xk。

理解了上面以后就能够开始推导公式了。

1.首先不考虑过程噪声对状态进行更新，很简单：

举个例子，v(k)=v(k-1)+at，匀加速运动咯。

2.不考虑测量噪声取出能测量的状态，也很简单：

3.用测量仪器测量出来的状态值（你们能够考虑到：测量的值就是被各类噪声干扰后的真实值）减去上面不考虑噪声获得的测量值：

这个值在数学上是一个定义值，叫作新息，有不少有趣的性质，感兴趣的能够本身谷歌。

咱们对步骤暂且停一停。这个叫新息的值有什么用？由上面的过程咱们能够明显看到，它反映了过程噪声和测量噪声综合对测量状态值的影响，也就是它包含了w和v的状况。

回到数学层面，（不要惧怕，很简单的数学应用和思考啦！）一个数值c由两部份内容a和b组成，那么怎样用数学表达式来表达？

通常有两种作法：

I.直接相加：c=a+b;

II. 用比例的方法：a=n*c,b=(1-n)*c

卡尔曼采用了方法II，用比例的方法来作（其实这也是为何叫作滤波的缘由，由于滤波就是给权值之类的操做）。也就是说，过程噪声w=新息*一个比例。这样获得的过程噪声加上原来（第一步）不考虑过程噪声的状态值不就是优化值了吗？ 也就是：

Okay，都写到这里了，有必要作一下前提假设：

a. 什么高斯噪声，均值为零一堆；

b.Ak,Ck,wk的协方差Q,vk的协方差R，系统协方差初始值P0,状态初始值X0，都已知。为何已知，你实际作项目就知道了。不过不懂的能够留言或者私信。

那么到目前为止咱们的思路就是清楚了，找到一个合适的Hk值（卡尔曼增益），那么咱们就能获得状态的最优值。（卡尔曼说的，不是我说的，因此你问为何，你要问他，这么深层次的理论留给博士和学者们去作就好，咱们就现学现用就行，哈哈哈，站在巨人的肩膀！）

问题来了：怎么获得合适的Hk？彷佛不是随便一个参数。

这是偏差协方差矩阵。

思路：使得偏差协方差矩阵Pk最小的Hk。

为何？这里我从感观的角度说明本身的理解，欢迎讨论。

协方差表示什么，协方差表示二者之间的联系或者关系，关系越大，协方差越大。偏差协方差越小说明过程噪声和量测噪声的关系越小。关系越小能作什么，这要回到咱们第3步讨论的咱们用比例的方法分开了w和v。用比例分开，到底多少属于w，多少是v，若是关系越小，分开的越精确，好比一堆白砂糖和盐，若是两种混合的很均匀，咱们说它关系很大，也就越难用比例的方法将其分开。

4.求的偏差协方差矩阵Pk

天然是把里面的Xk先获得，而后公式运算，经过上面的步骤咱们也容易获得：

而后复杂的数学计算，和以前假设的高斯噪声，新息的性质之类（至于过程，我的以为你若是只作应用，不研究算法，就不必深刻去看了），就能获得下面的卡尔曼滤波递推公式：

经过上面的解释，咱们也就不难知道这些公式都在干吗，知道干吗就能够了。在知道A,C,P0,Q,R的状况下，整个公式的运算流程也都很清晰了。

————————————————————————————————————————

欢迎评论！转载请注明出处！谢谢你们！