卡尔曼滤波算法

二、卡尔曼滤波之数学建模

一直在看，一直不懂。 我这人学数学的毛病，就是需要非常细致的知道每个变量的含义，谁变谁不变必须清清楚楚告诉我，否则我就没有那个直觉。 anyway，从这篇**入手吧：http://www.cs.unc.edu/~welch/kal ... n_intro_chinese.pdf

所谓滤波，实际上是要去掉自己不想要的信号，保留想要的部分。一般来说，是把过程中的噪声去掉。

卡尔曼滤波的默认假定是，世界充满噪声，任何测量结果都有噪声，状态转移过程会有噪声，你想知道系统的真实值么？玩儿蛋去吧。

卡尔曼滤波另一个重要假定模型是这样的，一个系统会处在各种不同的状态，并且会在状态之间转化来转化去。但是呢，倒霉的是我们谁也不知道该系统当前到底是在什么状态；但是呢，幸运的是我们可以通过测量的结果猜测到系统当前在一个什么状态。

那啥叫状态呢？例如系统的速度，加速度，温度，脑残度都算。离散系统的话，我们可以假设一个黑盒，黑盒里有许多颜色的灯（红橙黄绿青蓝紫），同时只能有一个颜色在亮着，ok，哪个灯在亮就是当前状态。

下面就是建模：

z_t = H*x_t + v_t;                                                                                                                                         (1)

                             x_t = A*x_(t-1) + B*u_(t-1) + w_(t-1);                                                                                                                      (2)

        初看起这俩式子来，我也头大，不过稍微分析一下也不太难。x_t是t时刻系统所在状态，z_t是所谓观测值，u_t是系统控制变量（已知，但我做的领域一般用不着），w_t , v_t都是噪声。

         那么式子(1)就是想说，观测值和系统状态的关系： 如果你看到种子发芽了，那么它的状态就是刚出生；如果你看到它开始长叶儿了，那状态就叫生长期；如果丫开花了，就叫啥啥期；如果结果了，就叫成熟期；如果蔫儿了，就叫嗝屁期。

         哦，对了，个人理解一下，以上公式限定为了线性系统，传说中卡尔曼滤波是线性的，来源就在这里了，谁叫你是矩阵乘向量呢，你要是写成f(x_t)，那有可能就是非线性的了。

        那么式子(2)就是说，前一时刻状态和下一时刻状态之间的关系。我在这里卡了好久，总是以为丫是马尔科夫过程，所以就完全无法理解A这个系数是凭啥得来的。其实，就是一个固定的转移方程，该方程完全没有概率问题。

         所以！以上式子中，固定下来的是H, A, B，这三个矩阵千年不变，万年不变，并且是事先设定好的，全都已知。未知的话....你自己编一个模型吧。 那么w_t，v_t 在这里限定为两个高斯白噪声N(0, Q)和N(0, R)。 哦，对，这里要记得，Q,R都tm是协方差矩阵啊，因为，系统状态和观测值都是向量。我对协方差可郁闷可郁闷了。这里提一句，我就完全无法理解协方差想表达什么，为什么俩随机变量独立，协方差一定为0，虽然我也知道怎么推导，但就是不能直观理解之，如果有人知道，还烦请告知。

          那继续扯淡。卡尔曼滤波，本质是想要预测下一步的观测值，或者实时监控一下系统所在状态。但一般在我这个领域，大家还都是在玩儿预测，那咱就从预测角度分析。OK，直觉上，给定上一个位置的状态x_(t-1)，式子(2)足够了。但是，回到开始的默认假设，式子(2)得到的结果x^-_t那是各种不准确啊。不准确怎么办？那就去看观测值呗，于是得到观测值z_t，但是观测值也不准确唉，那怎么办？当当当当，卡尔曼告诉我们一个灰常牛B的事情，一个相对准确的系统值具有如下结构：

                                                                                                                    x&_t = x&-_t + K( z_t - H*x_(t-1) ) ;                                                                                                                       (3)

提一下，这里" & "表示估计值，" - "表示是用前面式子算出来的估计值，不带" - "表示是最后的估计值。 (3)这个式子是想说，你不是式子估计和观测值都不准么，那么你把他俩加个权，求个和，那就可能更准确了。啥？你说这个式子不像加权？你把丫拆了，倒腾倒腾不就像了。 所以，最牛B就牛B在了这个“K”，传说中的卡尔曼增益。 这个K怎么得到的？我也不知道。 **说法是，定义误差 e_t = x_t - x&_t ，P_t为此误差的协方差矩阵，目的是使这个误差协方差矩阵最小化，把(3)代过去，于是折腾来折腾去，再求个导数为0，解得K，这个关键值的算法：

                                                                                                             K = P-_t * H^T * ( H * P-_t * H^T + R) ^(-1);                                                                                                               (4)

哦，对了，另外注意一点，从此以后，我们就都在估计上做**了，你只要记得，咱永远看不到真实值就行了，于是我们的式子里不是带"&"就是带"-"。

那么式子(4)就是在说，你丫这么着就把K求出来了。于是，问题就变成了这个P-_t怎么个求法。

说到这里，传说中的卡尔曼滤波就算讲完了。神马？你说P-_k还没求呢。是啊，卡尔曼滤波一共就俩需要求的玩意儿，还都tm是迭代求解，一个就是这个P-_t，另一个就是状态x-_t。你随便给个初始值，然后就迭着吧。

言归正传，我还得给出迭代的公式：

x-_t = A * x&_(t-1) + B * u_(t-1);                                                                                                                         (5)

P-_t = A * P_(t-1) * A^T + Q;                                                                                                                            (6)

大家一定别搞混Q和R谁是哪个公式冒出来的啊。 另外严重关切一下这里"-","&"以及不加的关系。 注意到啥没有？对了，(6)式中等号右边的P_(t-1)不带任何符号，嘿嘿，那自然是还差一个公式啦：

P_t = (I - K_t * H ) P-_(t-1);                                                                                                                           (7)

大功告成，以上就是传说中的卡尔曼滤波的迭代求解方法，如果你要预测状态呢，就用式子(5)的结果；如果预测观测值呢，就把式子(5)的结果乘个H；如果是监测状态呢，就用式子(3)的结果。

          至于一切式子中的推导过程，还有为神马是这样求出来的，咕~~(╯﹏╰)b，本人一概不知。泪奔告退。

          最后小注一下，**指出，如果Q,R是常数，K会收敛，也即P也会收敛到常量。 另外，大家经常诟病卡尔曼滤波都是假定高斯分布，我勒个去，这里的高斯分布到底说谁呢？噪声项？虽然看上去应该是，但我打赌不是。可是其它又都是定值，唉，头大。我本来就是为了理解这句话才来学习卡尔曼滤波的。 还得慢慢学，继续泪奔

PS：于是，果然，文中提到x_t是一个随机变量，并且在已知z_t的情况下 p(x_t | z_t) 服从N( x&_t, E[(x_t - x&_t)(x_t - x&_t)])，切记切记，这里所说的正态分布，是指已知观测值的情况下，系统状态是一个高斯分布的随机变量。