关于卡尔曼滤波和粒子滤波最直白的解释

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

相信所有学习卡尔曼滤波的同学首先接触的都是状态方程和观测方程，学过控制系统的同学可能不陌生，否则，先被那两个看起来好深奥的公式给吓跑了，关键是还不知道他们究竟是干什么的，什么是状态，什么是观测。。。。。。如果再看到后面的一大串递归推导增益，实在很晕很晕，更糟糕的是还没整明白的时候就已经知道卡尔曼滤波其实已经不够使了，需要extended kalmanfilter 和particle filter了。。。

其实我们完全不用理会这些公式。先来看看究竟卡尔曼滤波是做什么的，理解了卡尔曼滤波，下面的就顺其自然了。

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

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

每天给兔子拔草，看她香甜地吃啊吃地，就忍不住关心一下她的体重增长情况。那么我们就以小兔子的体重作为研究对象吧。假定我每周做一次观察，我有两个办法可以知道兔子的体重，一个是拿体重计来称：或许你有办法一下子就称准兔子的体重（兽医通常都有这办法），但现在为了体现卡尔曼先生理论的魅力，我们假定你的称实在很糟糕，误差很大，或者兔子太调皮，不能老实呆着，弹簧秤因为小兔子的晃动会产生很大误差。尽管有误差，那也是一个不可失去的渠道来得到兔子的体重。还有一个途径是根据书本上的资料，和兔子的年龄，我可以估计一下我的小兔子应该会多重，我们把用称称出来的叫观察量，用资料估计出来的叫估计值，无论是观察值还是估计值显然都是有误差的，假定误差是高斯分布。现在问题就来了，按照书本上说我的兔子该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 传递的是高斯混合分布，每一个峰代表一个动物在我们的例子。

假设你有两个传感器，测的是同一个信号。可是它们每次的读数都不太一样，怎么办？

取平均。

再假设你知道其中贵的那个传感器应该准一些，便宜的那个应该差一些。那有比取平均更好的办法吗？

加权平均。

怎么加权？假设两个传感器的误差都符合正态分布，假设你知道这两个正态分布的方差，用这两个方差值，（此处省略若干数学公式），你可以得到一个“最优”的权重。

接下来，重点来了：假设你只有一个传感器，但是你还有一个数学模型。模型可以帮你算出一个值，但也不是那么准。怎么办？

把模型算出来的值，和传感器测出的值，（就像两个传感器那样），取加权平均。

OK，最后一点说明：你的模型其实只是一个步长的，也就是说，知道x(k)，我可以求x(k+1)。问题是x(k)是多少呢？答案：x(k)就是你上一步卡尔曼滤波得到的、所谓加权平均之后的那个、对x在k时刻的最佳估计值。

于是 迭代也有了。

这就是卡尔曼滤波。

作者：Kent Zeng
链接：https://www.zhihu.com/question/23971601/answer/26254459
来源：知乎
著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。

最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Ｋолмогоров等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。

现设线性时变系统的离散状态防城和观测方程为：

X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1)

Y(k) = H(k)·X(k)+N(k)

其中

X(k)和Y(k)分别是k时刻的状态矢量和观测矢量

F(k,k-1)为状态转移矩阵

U(k)为k时刻动态噪声

T(k,k-1)为系统控制矩阵

H(k)为k时刻观测矩阵

N(k)为k时刻观测噪声

则卡尔曼滤波的算法流程为：

1. 
   
2. 预估计X(k)^= F(k,k-1)·X(k-1) 
   
3. 计算预估计协方差矩阵
   C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)'
   Q(k) = U(k)×U(k)' 
   
4. 计算卡尔曼增益矩阵
   K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1)
   R(k) = N(k)×N(k)' 
   
5. 更新估计
   X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^] 
   
6. 计算更新后估计协防差矩阵
   C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)' 
   
7. X(k+1) = X(k)~
   C(k+1) = C(k)~
   重复以上步骤