卡尔曼滤波

我是勤劳的搬运工，转自：

1、http://blog.csdn.net/karen99/article/details/7771743

2、http://blog.csdn.net/tudouniurou/article/details/6277512

3、https://blog.csdn.net/xxyy142857/article/details/46972115

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

一、卡尔曼最直白的解释：

        卡尔曼滤波是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。

      卡尔曼滤波建立在线性代数和隐马尔可夫模型（hidden Markov model）上。其基本动态系统可以用一个马尔可夫链表示，该马尔可夫链建立在一个被高斯噪声（即正态分布的噪声）干扰的线性算子上的。系统的状态可以用一个元素为实数的向量表示。随着离散时间的每一个增加，这个线性算子就会作用在当前状态上，产生一个新的状态，并也会带入一些噪声，同时系统的一些已知的控制器的控制信息也会被加入。同时，另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。

 

         用一句最简单的话来说，卡尔曼滤波是来帮助我们做测量的，大家一定不明白测量干嘛搞那么复杂？测量长度拿个尺子比一下，测量温度拿温度表测一下不就完了嘛。的确如此，如果你要测量的东西很容易测准确，没有什么随机干扰，那真的不需要劳驾卡尔曼先生。但在有的时候，我们的测量因为随机干扰，无法准确得到，卡尔曼先生就给我们想了个办法，让我们在干扰为高斯分布的情况下，得到的测量均方误差最小，也就是测量值扰动最小，看起来最平滑。

 

还是举例子最容易明白。我最近养了只小兔子，忍不住拿小兔子做个例子嘻嘻。

         每天给兔子拔草，看她香甜地吃啊吃地，就忍不住关心一下她的体重增长情况。那么我们就以小兔子的体重作为研究对象吧。假定我每周做一次观察，我有两个办法可以知道兔子的体重，一个是拿体重计来称：或许你有办法一下子就称准兔子的体重（兽医通常都有这办法），但现在为了体现卡尔曼先生理论的魅力，我们假定你的称实在很糟糕，误差很大，或者兔子太调皮，不能老实呆着，弹簧秤因为小兔子的晃动会产生很大误差。尽管有误差，那也是一个不可失去的渠道来得到兔子的体重。还有一个途径是根据书本上的资料，和兔子的年龄，我可以估计一下我的小兔子应该会多重，我们把用称称出来的叫观察量，用资料估计出来的叫估计值，无论是观察值还是估计值显然都是有误差的，假定误差是高斯分布。现在问题就来了，按照书本上说我的兔子该3公斤重，称出来却只有2.5公斤，我究竟该信哪个呢？如果称足够准，兔子足够乖，卡尔曼先生就没有用武之地了呵呵，再强调一下是我们的现状是兔兔不够乖，称还很烂呵呵。在这样恶劣的情景下，卡尔曼先生告诉我们一个办法，仍然可以估计出八九不离十的兔兔体重，这个办法其实也很直白，就是加权平均，把称称出来的结果也就是观测值和按照书本经验估算出来的结果也就是估计值分别加一个权值，再做平均。当然这两个权值加起来是等于一的。也就是说如果你有0.7分相信称出来的体重，那么就只有0.3分相信书上的估计。说到这里大家一定更着急了，究竟该有几分相信书上的，有几分相信我自己称的呢？都怪我的称不争气，没法让我百分一百信赖它，还要根据书上的数据来做调整。好在卡尔曼先生也体会到了我们的苦恼，告诉我们一个办法来决定这个权值，这个办法其实也很直白，就是根据以往的表现来做决定，这其实听起来挺公平的，你以前表现好，我就相信你多一点，权值也就给的高一点，以前表现不好，我就相信你少一点，权值自然给的低一点。那么什么是表现好表现不好呢，表现好意思就是测量结果稳定，方差很小，表现不好就是估计值或观测值不稳定，方差很大。想象你用称称你的哦兔子，第一次1公斤第二次10公斤，第三次5公斤，你会相信你的称吗，但是如果第一次3公斤第二次3.2公斤，第三次2.8公斤，自然我就相信它多一点，给它一个大的权值了。

 

        有了这个权值，下面的事情就很好办了。很显然卡尔曼先生是利用多次观察和估计来达到目的的，我们也只能一步一步地调整我们的观察和估计值，来渐渐达到准确的测量，所以整个算法是递归的，需要多次重复调整的。调整的过程也很简单，就是把实测值（称出来的体重）和估计值（书上得来的体重）比较一下，如果估计值比测量值小，那就把估计值加上他们之间的偏差作为新的估计值，当然前面要加个系数，就是我们前面说的加权系数，这个地方我要写个公式，因为很简单就能说明白。

 

         比如我们的观查值是Z，估计值是X， 那么新的估计值就应该是 Xnew  =  X  + K ( Z-X），从这个公式可以看到，如果X估计小了，那么新的估计值会加上一个量K ( Z-X）, 如果估计值大了,大过Z了，那么新的估计值就会减去一个量K ( Z-X），这就保证新的估计值一定比现在的准确，一次一次递归下去就会越来越准却了，当然这里面很有作用的也是这个K，也就是我们前面说的权值，书上都把他叫卡尔曼增益。。。(Xnew  =  X  + K ( Z-X） = X ×（1-K) + KZ ，也就是说估计值X的权值是1-k，而观察值Z的权值是k，究竟k 取多大，全看估计值和观察值以前的表现，也就是他们的方差情况了）

 

发现把一个问题讲明白还真不是件容易的事情，谁听明白了我佩服谁，因为我已经把自己讲糊涂了哈

顺便就把extended kalman filter和particle filter提一下，因为高斯模型有时不适用，于是有了extended kalman filter，而particle filter是用于多个对象的，比如除了兔子我还有只猫，他们的体重有一个联合概率模型，每一个对象就是一个particle。无论是卡尔曼滤波还是particle滤波，都是概率分布传递的过程，卡尔曼传递的是高斯分布，particle filter 传递的是高斯混合分布，每一个峰代表一个动物在我们的例子。

 

------------------------------------------------华丽的分割线------------------------------------------------

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

 

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

 

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

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

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

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

 

下面就是建模：

                                 ​​​​​​                                                                                          (1)

                                                                             (2)

 

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

 

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

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

         所以！以上式子中，固定下来的是H, A, B，这三个矩阵千年不变，万年不变，并且是事先设定好的，全都已知。未知的话....你自己编一个模型吧。 那么w_t，v_t 在这里限定为两个高斯白噪声N(0, Q)和N(0, R)。 哦，对，这里要记得，Q,R都tm是协方差矩阵啊，因为，系统状态和观测值都是向量。把协方差看成两个人的影响度；A,B两个人相互独立就是两个人没啥关系，就是A死了，对B来说没啥影响，B死了对A来说也是没啥影响的。反之如果他们两存在亲情或者仇人关系，A死了可能会使得B难过或者高兴这样的影响，这样他们的协方差就会产生一个值

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

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

提一下，这里" & "表示估计值，" - "表示是用前面式子算出来的估计值，不带" - "表示是最后的估计值。 (3)这个式子是想说，你不是式子估计和观测值都不准么，那么你把他俩加个权，求个和，那就可能更准确了，其中H*x_(t-1) 就是z_t的预测值。所以，最牛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)的结果。

    

          最后小注一下，文章指出，如果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)])，切记切记，这里所说的正态分布，是指已知观测值的情况下，系统状态是一个高斯分布的随机变量。