卡尔曼滤波的一个推导思路

        一个较为简单的的卡尔曼滤波器的公式推导思路。但是公式推导嘛，再简单也还是一堆公式，看完前两部分了解个差不多也就行了。不过认真看，应该还是比较容易的。

一、问题描述

        给定系统状态更新方程（可以认为是根据物理原理建模得到的）以及观测方程（就是有关系统状态的传感器读数），即如下两个公式，并给出对当前时刻系统的观测，要怎么样能够得到一个较好的对系统状态的预测。

        

        上面两个式子中，我们不能确定的变量仅有两个噪声和，但知道它们均值皆为0，协方差分别为和。其它变量都是已知的。

        单独看第一个公式，可以从上一次的系统状态求得，但是由于噪声不知道，我们做不到。

        单独看第二个公式，可以从对当前系统进行观测得到的进行反推得到，但是由于噪声不知道，我们也做不到。

        噪声是无法避免的，是一定存在的。因为噪声的存在，单独靠一个方程无法解决问题，所以我们需要融合两个方程并结合观测值得到一个正确的系统状态估计，这就是卡尔曼滤波要做的事情。

二、问题求解思路

        按理来说，假如知道噪声的具体大小，我们观测到的值就是下面这条式子：

        很可惜，我们不知道噪声的大小，所以我们只能忽略噪声，做一个大致的预测，这里预测的符号咱们先加上一撇，代表不是最优的结果。

        不难看出，这里的融合了最开始提到的两条方程，即状态方程和观测方程。对比和，我们发现观测值相比于预测值，其实就是多了噪声的作用。现在，定义一个变量叫做"新息"，也就是包含了两个噪声信息的变量为：

        新息里面包含了两个噪声的信息，我们不妨假设其中一个噪声占了的倍，也就是说：

        好了，现在定义了一个关于噪声的表达式，这个表达式很牛逼，融合了我们所知道的全部信息，即状态方程、观测方程以及观测值。把噪声带回到最开始那条式子里面，得到一个优化过的系统状态估计，优化后的估计变量就不带一撇了，希望读者能理解我这里的带一撇和不带一撇的含义。

        好了，现在我们得到了一个最优预测的表达式，可以认为就是在原先估计的基础上添加一个修正项。但是式子中的咱们还不知道呢。我们把这个比例因子叫做卡尔曼增益，当求出卡尔曼增益之后，咱们的工作就认为结束了。

三、进入数学推导

        首先咱们要知道什么叫做最优估计，这里咱们采用MMSE（最小均方误差）准则，也就是当预测和实际的差值的平方的均值（均方误差）最小时，我们认为是最优估计。

        然后定义两个预测误差和两个预测误差协方差矩阵，大家注意带一撇和不带一撇的区别。

        设是维的，那么误差也是维的，记作。 那么要最小化的变量，也就是所谓的均方误差可以写成：

 

       可以发现，矢量的均方误差是每个元素的均方误差之和，而这恰巧又是协方差矩阵的对角线元素之和。

        称矩阵对角线元素之和为trace，记协方差矩阵的对角线元素之和为，那么有：

        这就是我们优化的目标的表达式。

        接下来求的表达式，而求的表达式，就要知道的具体的表达式：

        然后求的表达式：

        展开后一共得到四项。对上式求期望就是协方差矩阵。取期望后，中带有负号的两项就消失了，这是因为我们认为误差向量和噪声向量是正交的，所以它们构成的互协方差矩阵为零矩阵。先前已定义了

        所以的表达式如下：

        对求导数：

        当导数为0时均方误差最小。令导数为0，便得到了卡尔曼增益的表达式：

        我们试着将卡尔曼增益带回的表达式，得到的一个比较简单的表达式，它可以通过来计算：

        可是问题又来了，要知道卡尔曼增益和，就得知道，可是现在还不知道呢，那就来求一下咯。和之前同样的顺序，先写出的表达式：

        然后求，最后求期望得到：

        嗯，发现可以由递推得到。

        好了，现在咱们推完全部公式了。

四、总结

        现在将以上的一些结论罗列一下，按两类分，一类是用前一时刻数据推下一时刻数据的，我们叫做预测过程；另一类是用同一时刻的数据优化预测结果的，叫做更新过程。

        预测过程，这里都是带一撇的，表明后面得对它们进行优化更新。这里用前一时刻数据预测下一时刻的系统状态和误差协方差矩阵：

        更新过程，分别是计算卡尔曼增益、利用卡尔曼增益和新息更新对系统状态的估计、然后更新误差协方差矩阵：

     也就是说，可以用预测得到，用来递推得到，知道就可以知道卡尔曼增益，知道之后就可以用更新公式得到一个较好的系统状态估计了，与此同时，我们还要利用来计算一下。

        等下一次数据来了，用预测得到，用来递推得到，然后得到，然后得到，然后得到。就这么循环嘛，直到用完全部观测数据为止。

        需要注意的是，因为预测过程需要上一时刻的数据，所以就得定义两个初始值，也就是-1时刻的和是需要人为定义的，这个要根据先验信息来确定，或者随机初始化应该也可以。

        就这样。

        码公式太累了。